diff --git a/source/RobotAPI/applications/AronCodeGenerator/cxxopts.hpp b/source/RobotAPI/applications/AronCodeGenerator/cxxopts.hpp
index a11201123e2b6c17939c960c9b7c8239fbdc80dc..11fe04078d717a88bb6808b8c25721c113e54085 100644
--- a/source/RobotAPI/applications/AronCodeGenerator/cxxopts.hpp
+++ b/source/RobotAPI/applications/AronCodeGenerator/cxxopts.hpp
@@ -25,8 +25,8 @@ THE SOFTWARE.
#ifndef CXXOPTS_HPP_INCLUDED
#define CXXOPTS_HPP_INCLUDED
-#include <cstring>
#include <cctype>
+#include <cstring>
#include <exception>
#include <iostream>
#include <map>
@@ -52,13 +52,8 @@ namespace cxxopts
static constexpr struct
{
uint8_t major, minor, patch;
- } version =
- {
- CXXOPTS__VERSION_MAJOR,
- CXXOPTS__VERSION_MINOR,
- CXXOPTS__VERSION_PATCH
- };
-}
+ } version = {CXXOPTS__VERSION_MAJOR, CXXOPTS__VERSION_MINOR, CXXOPTS__VERSION_PATCH};
+} // namespace cxxopts
//when we ask cxxopts to use Unicode, help strings are processed using ICU,
//which results in the correct lengths being computed for strings when they
@@ -73,21 +68,16 @@ namespace cxxopts
{
typedef icu::UnicodeString String;
- inline
- String
+ inline String
toLocalString(std::string s)
{
return icu::UnicodeString::fromUTF8(std::move(s));
}
- class UnicodeStringIterator : public
- std::iterator<std::forward_iterator_tag, int32_t>
+ class UnicodeStringIterator : public std::iterator<std::forward_iterator_tag, int32_t>
{
public:
-
- UnicodeStringIterator(const icu::UnicodeString* string, int32_t pos)
- : s(string)
- , i(pos)
+ UnicodeStringIterator(const icu::UnicodeString* string, int32_t pos) : s(string), i(pos)
{
}
@@ -127,15 +117,13 @@ namespace cxxopts
int32_t i;
};
- inline
- String&
+ inline String&
stringAppend(String& s, String a)
{
return s.append(std::move(a));
}
- inline
- String&
+ inline String&
stringAppend(String& s, int n, UChar32 c)
{
for (int i = 0; i != n; ++i)
@@ -159,15 +147,13 @@ namespace cxxopts
return s;
}
- inline
- size_t
+ inline size_t
stringLength(const String& s)
{
return s.length();
}
- inline
- std::string
+ inline std::string
toUTF8String(const String& s)
{
std::string result;
@@ -176,30 +162,27 @@ namespace cxxopts
return result;
}
- inline
- bool
+ inline bool
empty(const String& s)
{
return s.isEmpty();
}
-}
+} // namespace cxxopts
namespace std
{
- inline
- cxxopts::UnicodeStringIterator
+ inline cxxopts::UnicodeStringIterator
begin(const icu::UnicodeString& s)
{
return cxxopts::UnicodeStringIterator(&s, 0);
}
- inline
- cxxopts::UnicodeStringIterator
+ inline cxxopts::UnicodeStringIterator
end(const icu::UnicodeString& s)
{
return cxxopts::UnicodeStringIterator(&s, s.length());
}
-}
+} // namespace std
//ifdef CXXOPTS_USE_UNICODE
#else
@@ -215,22 +198,19 @@ namespace cxxopts
return std::forward<T>(t);
}
- inline
- size_t
+ inline size_t
stringLength(const String& s)
{
return s.length();
}
- inline
- String&
+ inline String&
stringAppend(String& s, String a)
{
return s.append(std::move(a));
}
- inline
- String&
+ inline String&
stringAppend(String& s, size_t n, char c)
{
return s.append(n, c);
@@ -250,13 +230,12 @@ namespace cxxopts
return std::forward<T>(t);
}
- inline
- bool
+ inline bool
empty(const std::string& s)
{
return s.empty();
}
-}
+} // namespace cxxopts
//ifdef CXXOPTS_USE_UNICODE
#endif
@@ -272,54 +251,40 @@ namespace cxxopts
const std::string LQUOTE("‘");
const std::string RQUOTE("’");
#endif
- }
+ } // namespace
class Value : public std::enable_shared_from_this<Value>
{
public:
-
virtual ~Value() = default;
- virtual
- std::shared_ptr<Value>
- clone() const = 0;
+ virtual std::shared_ptr<Value> clone() const = 0;
- virtual void
- parse(const std::string& text) const = 0;
+ virtual void parse(const std::string& text) const = 0;
- virtual void
- parse() const = 0;
+ virtual void parse() const = 0;
- virtual bool
- has_default() const = 0;
+ virtual bool has_default() const = 0;
- virtual bool
- is_container() const = 0;
+ virtual bool is_container() const = 0;
- virtual bool
- has_implicit() const = 0;
+ virtual bool has_implicit() const = 0;
- virtual std::string
- get_default_value() const = 0;
+ virtual std::string get_default_value() const = 0;
- virtual std::string
- get_implicit_value() const = 0;
+ virtual std::string get_implicit_value() const = 0;
- virtual std::shared_ptr<Value>
- default_value(const std::string& value) = 0;
+ virtual std::shared_ptr<Value> default_value(const std::string& value) = 0;
- virtual std::shared_ptr<Value>
- implicit_value(const std::string& value) = 0;
+ virtual std::shared_ptr<Value> implicit_value(const std::string& value) = 0;
- virtual bool
- is_boolean() const = 0;
+ virtual bool is_boolean() const = 0;
};
class OptionException : public std::exception
{
public:
- OptionException(const std::string& message)
- : m_message(message)
+ OptionException(const std::string& message) : m_message(message)
{
}
@@ -336,9 +301,7 @@ namespace cxxopts
class OptionSpecException : public OptionException
{
public:
-
- OptionSpecException(const std::string& message)
- : OptionException(message)
+ OptionSpecException(const std::string& message) : OptionException(message)
{
}
};
@@ -346,8 +309,7 @@ namespace cxxopts
class OptionParseException : public OptionException
{
public:
- OptionParseException(const std::string& message)
- : OptionException(message)
+ OptionParseException(const std::string& message) : OptionException(message)
{
}
};
@@ -355,8 +317,8 @@ namespace cxxopts
class option_exists_error : public OptionSpecException
{
public:
- option_exists_error(const std::string& option)
- : OptionSpecException("Option " + LQUOTE + option + RQUOTE + " already exists")
+ option_exists_error(const std::string& option) :
+ OptionSpecException("Option " + LQUOTE + option + RQUOTE + " already exists")
{
}
};
@@ -364,8 +326,8 @@ namespace cxxopts
class invalid_option_format_error : public OptionSpecException
{
public:
- invalid_option_format_error(const std::string& format)
- : OptionSpecException("Invalid option format " + LQUOTE + format + RQUOTE)
+ invalid_option_format_error(const std::string& format) :
+ OptionSpecException("Invalid option format " + LQUOTE + format + RQUOTE)
{
}
};
@@ -373,9 +335,9 @@ namespace cxxopts
class option_syntax_exception : public OptionParseException
{
public:
- option_syntax_exception(const std::string& text)
- : OptionParseException("Argument " + LQUOTE + text + RQUOTE +
- " starts with a - but has incorrect syntax")
+ option_syntax_exception(const std::string& text) :
+ OptionParseException("Argument " + LQUOTE + text + RQUOTE +
+ " starts with a - but has incorrect syntax")
{
}
};
@@ -383,8 +345,8 @@ namespace cxxopts
class option_not_exists_exception : public OptionParseException
{
public:
- option_not_exists_exception(const std::string& option)
- : OptionParseException("Option " + LQUOTE + option + RQUOTE + " does not exist")
+ option_not_exists_exception(const std::string& option) :
+ OptionParseException("Option " + LQUOTE + option + RQUOTE + " does not exist")
{
}
};
@@ -392,10 +354,8 @@ namespace cxxopts
class missing_argument_exception : public OptionParseException
{
public:
- missing_argument_exception(const std::string& option)
- : OptionParseException(
- "Option " + LQUOTE + option + RQUOTE + " is missing an argument"
- )
+ missing_argument_exception(const std::string& option) :
+ OptionParseException("Option " + LQUOTE + option + RQUOTE + " is missing an argument")
{
}
};
@@ -403,10 +363,8 @@ namespace cxxopts
class option_requires_argument_exception : public OptionParseException
{
public:
- option_requires_argument_exception(const std::string& option)
- : OptionParseException(
- "Option " + LQUOTE + option + RQUOTE + " requires an argument"
- )
+ option_requires_argument_exception(const std::string& option) :
+ OptionParseException("Option " + LQUOTE + option + RQUOTE + " requires an argument")
{
}
};
@@ -414,16 +372,10 @@ namespace cxxopts
class option_not_has_argument_exception : public OptionParseException
{
public:
- option_not_has_argument_exception
- (
- const std::string& option,
- const std::string& arg
- )
- : OptionParseException(
- "Option " + LQUOTE + option + RQUOTE +
- " does not take an argument, but argument " +
- LQUOTE + arg + RQUOTE + " given"
- )
+ option_not_has_argument_exception(const std::string& option, const std::string& arg) :
+ OptionParseException("Option " + LQUOTE + option + RQUOTE +
+ " does not take an argument, but argument " + LQUOTE + arg +
+ RQUOTE + " given")
{
}
};
@@ -431,8 +383,8 @@ namespace cxxopts
class option_not_present_exception : public OptionParseException
{
public:
- option_not_present_exception(const std::string& option)
- : OptionParseException("Option " + LQUOTE + option + RQUOTE + " not present")
+ option_not_present_exception(const std::string& option) :
+ OptionParseException("Option " + LQUOTE + option + RQUOTE + " not present")
{
}
};
@@ -440,13 +392,8 @@ namespace cxxopts
class argument_incorrect_type : public OptionParseException
{
public:
- argument_incorrect_type
- (
- const std::string& arg
- )
- : OptionParseException(
- "Argument " + LQUOTE + arg + RQUOTE + " failed to parse"
- )
+ argument_incorrect_type(const std::string& arg) :
+ OptionParseException("Argument " + LQUOTE + arg + RQUOTE + " failed to parse")
{
}
};
@@ -454,10 +401,9 @@ namespace cxxopts
class option_required_exception : public OptionParseException
{
public:
- option_required_exception(const std::string& option)
- : OptionParseException(
- "Option " + LQUOTE + option + RQUOTE + " is required but not present"
- )
+ option_required_exception(const std::string& option) :
+ OptionParseException("Option " + LQUOTE + option + RQUOTE +
+ " is required but not present")
{
}
};
@@ -466,13 +412,10 @@ namespace cxxopts
{
namespace
{
- std::basic_regex<char> integer_pattern
- ("(-)?(0x)?([0-9a-zA-Z]+)|((0x)?0)");
- std::basic_regex<char> truthy_pattern
- ("(t|T)(rue)?");
- std::basic_regex<char> falsy_pattern
- ("((f|F)(alse)?)?");
- }
+ std::basic_regex<char> integer_pattern("(-)?(0x)?([0-9a-zA-Z]+)|((0x)?0)");
+ std::basic_regex<char> truthy_pattern("(t|T)(rue)?");
+ std::basic_regex<char> falsy_pattern("((f|F)(alse)?)?");
+ } // namespace
namespace detail
{
@@ -508,7 +451,9 @@ namespace cxxopts
{
template <typename U>
void
- operator()(bool, U, const std::string&) {}
+ operator()(bool, U, const std::string&)
+ {
+ }
};
template <typename T, typename U>
@@ -517,7 +462,7 @@ namespace cxxopts
{
SignedCheck<T, std::numeric_limits<T>::is_signed>()(negative, value, text);
}
- }
+ } // namespace detail
template <typename R, typename T>
R
@@ -598,9 +543,7 @@ namespace cxxopts
if (negative)
{
- value = checked_negate<T>(result,
- text,
- std::integral_constant<bool, is_signed>());
+ value = checked_negate<T>(result, text, std::integral_constant<bool, is_signed>());
}
else
{
@@ -609,7 +552,8 @@ namespace cxxopts
}
template <typename T>
- void stringstream_parser(const std::string& text, T& value)
+ void
+ stringstream_parser(const std::string& text, T& value)
{
std::stringstream in(text);
in >> value;
@@ -619,64 +563,55 @@ namespace cxxopts
}
}
- inline
- void
+ inline void
parse_value(const std::string& text, uint8_t& value)
{
integer_parser(text, value);
}
- inline
- void
+ inline void
parse_value(const std::string& text, int8_t& value)
{
integer_parser(text, value);
}
- inline
- void
+ inline void
parse_value(const std::string& text, uint16_t& value)
{
integer_parser(text, value);
}
- inline
- void
+ inline void
parse_value(const std::string& text, int16_t& value)
{
integer_parser(text, value);
}
- inline
- void
+ inline void
parse_value(const std::string& text, uint32_t& value)
{
integer_parser(text, value);
}
- inline
- void
+ inline void
parse_value(const std::string& text, int32_t& value)
{
integer_parser(text, value);
}
- inline
- void
+ inline void
parse_value(const std::string& text, uint64_t& value)
{
integer_parser(text, value);
}
- inline
- void
+ inline void
parse_value(const std::string& text, int64_t& value)
{
integer_parser(text, value);
}
- inline
- void
+ inline void
parse_value(const std::string& text, bool& value)
{
std::smatch result;
@@ -698,8 +633,7 @@ namespace cxxopts
throw argument_incorrect_type(text);
}
- inline
- void
+ inline void
parse_value(const std::string& text, std::string& value)
{
value = text;
@@ -753,14 +687,11 @@ namespace cxxopts
using Self = abstract_value<T>;
public:
- abstract_value()
- : m_result(std::make_shared<T>())
- , m_store(m_result.get())
+ abstract_value() : m_result(std::make_shared<T>()), m_store(m_result.get())
{
}
- abstract_value(T* t)
- : m_store(t)
+ abstract_value(T* t) : m_store(t)
{
}
@@ -896,8 +827,7 @@ namespace cxxopts
set_default_and_implicit();
}
- standard_value(bool* b)
- : abstract_value(b)
+ standard_value(bool* b) : abstract_value(b)
{
set_default_and_implicit();
}
@@ -909,7 +839,6 @@ namespace cxxopts
}
private:
-
void
set_default_and_implicit()
{
@@ -919,7 +848,7 @@ namespace cxxopts
m_implicit_value = "true";
}
};
- }
+ } // namespace values
template <typename T>
std::shared_ptr<Value>
@@ -940,24 +869,15 @@ namespace cxxopts
class OptionDetails
{
public:
- OptionDetails
- (
- const std::string& short_,
- const std::string& long_,
- const String& desc,
- std::shared_ptr<const Value> val
- )
- : m_short(short_)
- , m_long(long_)
- , m_desc(desc)
- , m_value(val)
- , m_count(0)
+ OptionDetails(const std::string& short_,
+ const std::string& long_,
+ const String& desc,
+ std::shared_ptr<const Value> val) :
+ m_short(short_), m_long(long_), m_desc(desc), m_value(val), m_count(0)
{
}
- OptionDetails(const OptionDetails& rhs)
- : m_desc(rhs.m_desc)
- , m_count(rhs.m_count)
+ OptionDetails(const OptionDetails& rhs) : m_desc(rhs.m_desc), m_count(rhs.m_count)
{
m_value = rhs.m_value->clone();
}
@@ -970,7 +890,8 @@ namespace cxxopts
return m_desc;
}
- const Value& value() const
+ const Value&
+ value() const
{
return *m_value;
}
@@ -1026,11 +947,7 @@ namespace cxxopts
{
public:
void
- parse
- (
- std::shared_ptr<const OptionDetails> details,
- const std::string& text
- )
+ parse(std::shared_ptr<const OptionDetails> details, const std::string& text)
{
ensure_value(details);
++m_count;
@@ -1083,21 +1000,18 @@ namespace cxxopts
class KeyValue
{
public:
- KeyValue(std::string key_, std::string value_)
- : m_key(std::move(key_))
- , m_value(std::move(value_))
+ KeyValue(std::string key_, std::string value_) :
+ m_key(std::move(key_)), m_value(std::move(value_))
{
}
- const
- std::string&
+ const std::string&
key() const
{
return m_key;
}
- const
- std::string&
+ const std::string&
value() const
{
return m_value;
@@ -1120,14 +1034,12 @@ namespace cxxopts
class ParseResult
{
public:
-
ParseResult(
- const std::shared_ptr <
- std::unordered_map<std::string, std::shared_ptr<OptionDetails>>
- >,
+ const std::shared_ptr<std::unordered_map<std::string, std::shared_ptr<OptionDetails>>>,
std::vector<std::string>,
bool allow_unrecognised,
- int&, char**&);
+ int&,
+ char**&);
size_t
count(const std::string& o) const
@@ -1165,40 +1077,26 @@ namespace cxxopts
}
private:
+ void parse(int& argc, char**& argv);
- void
- parse(int& argc, char**& argv);
+ void add_to_option(const std::string& option, const std::string& arg);
- void
- add_to_option(const std::string& option, const std::string& arg);
+ bool consume_positional(std::string a);
- bool
- consume_positional(std::string a);
+ void parse_option(std::shared_ptr<OptionDetails> value,
+ const std::string& name,
+ const std::string& arg = "");
- void
- parse_option
- (
- std::shared_ptr<OptionDetails> value,
- const std::string& name,
- const std::string& arg = ""
- );
+ void parse_default(std::shared_ptr<OptionDetails> details);
- void
- parse_default(std::shared_ptr<OptionDetails> details);
+ void checked_parse_arg(int argc,
+ char* argv[],
+ int& current,
+ std::shared_ptr<OptionDetails> value,
+ const std::string& name);
- void
- checked_parse_arg
- (
- int argc,
- char* argv[],
- int& current,
- std::shared_ptr<OptionDetails> value,
- const std::string& name
- );
-
- const std::shared_ptr <
- std::unordered_map<std::string, std::shared_ptr<OptionDetails>>
- > m_options;
+ const std::shared_ptr<std::unordered_map<std::string, std::shared_ptr<OptionDetails>>>
+ m_options;
std::vector<std::string> m_positional;
std::vector<std::string>::iterator m_next_positional;
std::unordered_set<std::string> m_positional_set;
@@ -1211,19 +1109,18 @@ namespace cxxopts
class Options
{
- typedef std::unordered_map<std::string, std::shared_ptr<OptionDetails>>
- OptionMap;
- public:
+ typedef std::unordered_map<std::string, std::shared_ptr<OptionDetails>> OptionMap;
- Options(std::string program, std::string help_string = "")
- : m_program(std::move(program))
- , m_help_string(toLocalString(std::move(help_string)))
- , m_custom_help("[OPTION...]")
- , m_positional_help("positional parameters")
- , m_show_positional(false)
- , m_allow_unrecognised(false)
- , m_options(std::make_shared<OptionMap>())
- , m_next_positional(m_positional.end())
+ public:
+ Options(std::string program, std::string help_string = "") :
+ m_program(std::move(program)),
+ m_help_string(toLocalString(std::move(help_string))),
+ m_custom_help("[OPTION...]"),
+ m_positional_help("positional parameters"),
+ m_show_positional(false),
+ m_allow_unrecognised(false),
+ m_options(std::make_shared<OptionMap>()),
+ m_next_positional(m_positional.end())
{
}
@@ -1255,70 +1152,45 @@ namespace cxxopts
return *this;
}
- ParseResult
- parse(int& argc, char**& argv);
+ ParseResult parse(int& argc, char**& argv);
- OptionAdder
- add_options(std::string group = "");
+ OptionAdder add_options(std::string group = "");
- void
- add_option
- (
- const std::string& group,
- const std::string& s,
- const std::string& l,
- std::string desc,
- std::shared_ptr<const Value> value,
- std::string arg_help
- );
+ void add_option(const std::string& group,
+ const std::string& s,
+ const std::string& l,
+ std::string desc,
+ std::shared_ptr<const Value> value,
+ std::string arg_help);
//parse positional arguments into the given option
- void
- parse_positional(std::string option);
+ void parse_positional(std::string option);
- void
- parse_positional(std::vector<std::string> options);
+ void parse_positional(std::vector<std::string> options);
- void
- parse_positional(std::initializer_list<std::string> options);
+ void parse_positional(std::initializer_list<std::string> options);
template <typename Iterator>
void
parse_positional(Iterator begin, Iterator end)
{
- parse_positional(std::vector<std::string> {begin, end});
+ parse_positional(std::vector<std::string>{begin, end});
}
- std::string
- help(const std::vector<std::string>& groups = {}) const;
+ std::string help(const std::vector<std::string>& groups = {}) const;
- const std::vector<std::string>
- groups() const;
+ const std::vector<std::string> groups() const;
- const HelpGroupDetails&
- group_help(const std::string& group) const;
+ const HelpGroupDetails& group_help(const std::string& group) const;
private:
+ void add_one_option(const std::string& option, std::shared_ptr<OptionDetails> details);
- void
- add_one_option
- (
- const std::string& option,
- std::shared_ptr<OptionDetails> details
- );
-
- String
- help_one_group(const std::string& group) const;
+ String help_one_group(const std::string& group) const;
- void
- generate_group_help
- (
- String& result,
- const std::vector<std::string>& groups
- ) const;
+ void generate_group_help(String& result, const std::vector<std::string>& groups) const;
- void
- generate_all_groups_help(String& result) const;
+ void generate_all_groups_help(String& result) const;
std::string m_program;
String m_help_string;
@@ -1339,21 +1211,15 @@ namespace cxxopts
class OptionAdder
{
public:
-
- OptionAdder(Options& options, std::string group)
- : m_options(options), m_group(std::move(group))
+ OptionAdder(Options& options, std::string group) :
+ m_options(options), m_group(std::move(group))
{
}
- OptionAdder&
- operator()
- (
- const std::string& opts,
- const std::string& desc,
- std::shared_ptr<const Value> value
- = ::cxxopts::value<bool>(),
- std::string arg_help = ""
- );
+ OptionAdder& operator()(const std::string& opts,
+ const std::string& desc,
+ std::shared_ptr<const Value> value = ::cxxopts::value<bool>(),
+ std::string arg_help = "");
private:
Options& m_options;
@@ -1365,17 +1231,14 @@ namespace cxxopts
constexpr int OPTION_LONGEST = 30;
constexpr int OPTION_DESC_GAP = 2;
- std::basic_regex<char> option_matcher
- ("--([[:alnum:]][-_[:alnum:]]+)(=(.*))?|-([[:alnum:]]+)");
+ std::basic_regex<char>
+ option_matcher("--([[:alnum:]][-_[:alnum:]]+)(=(.*))?|-([[:alnum:]]+)");
- std::basic_regex<char> option_specifier
- ("(([[:alnum:]]),)?[ ]*([[:alnum:]][-_[:alnum:]]*)?");
+ std::basic_regex<char>
+ option_specifier("(([[:alnum:]]),)?[ ]*([[:alnum:]][-_[:alnum:]]*)?");
String
- format_option
- (
- const HelpOptionDetails& o
- )
+ format_option(const HelpOptionDetails& o)
{
auto& s = o.s;
auto& l = o.l;
@@ -1414,12 +1277,7 @@ namespace cxxopts
}
String
- format_description
- (
- const HelpOptionDetails& o,
- size_t start,
- size_t width
- )
+ format_description(const HelpOptionDetails& o, size_t start, size_t width)
{
auto desc = o.desc;
@@ -1480,42 +1338,34 @@ namespace cxxopts
return result;
}
- }
+ } // namespace
- inline
- ParseResult::ParseResult
- (
- const std::shared_ptr <
- std::unordered_map<std::string, std::shared_ptr<OptionDetails>>
- > options,
+ inline ParseResult::ParseResult(
+ const std::shared_ptr<std::unordered_map<std::string, std::shared_ptr<OptionDetails>>>
+ options,
std::vector<std::string> positional,
bool allow_unrecognised,
- int& argc, char**& argv
- )
- : m_options(options)
- , m_positional(std::move(positional))
- , m_next_positional(m_positional.begin())
- , m_allow_unrecognised(allow_unrecognised)
+ int& argc,
+ char**& argv) :
+ m_options(options),
+ m_positional(std::move(positional)),
+ m_next_positional(m_positional.begin()),
+ m_allow_unrecognised(allow_unrecognised)
{
parse(argc, argv);
}
- inline
- OptionAdder
+ inline OptionAdder
Options::add_options(std::string group)
{
return OptionAdder(*this, std::move(group));
}
- inline
- OptionAdder&
- OptionAdder::operator()
- (
- const std::string& opts,
- const std::string& desc,
- std::shared_ptr<const Value> value,
- std::string arg_help
- )
+ inline OptionAdder&
+ OptionAdder::operator()(const std::string& opts,
+ const std::string& desc,
+ std::shared_ptr<const Value> value,
+ std::string arg_help)
{
std::match_results<const char*> result;
std::regex_match(opts.c_str(), result, option_specifier);
@@ -1537,11 +1387,8 @@ namespace cxxopts
throw invalid_option_format_error(opts);
}
- auto option_names = []
- (
- const std::sub_match<const char*>& short_,
- const std::sub_match<const char*>& long_
- )
+ auto option_names =
+ [](const std::sub_match<const char*>& short_, const std::sub_match<const char*>& long_)
{
if (long_.length() == 1)
{
@@ -1551,37 +1398,28 @@ namespace cxxopts
{
return std::make_tuple(short_.str(), long_.str());
}
- }
- (short_match, long_match);
+ }(short_match, long_match);
- m_options.add_option
- (
- m_group,
- std::get<0>(option_names),
- std::get<1>(option_names),
- desc,
- value,
- std::move(arg_help)
- );
+ m_options.add_option(m_group,
+ std::get<0>(option_names),
+ std::get<1>(option_names),
+ desc,
+ value,
+ std::move(arg_help));
return *this;
}
- inline
- void
+ inline void
ParseResult::parse_default(std::shared_ptr<OptionDetails> details)
{
m_results[details].parse_default(details);
}
- inline
- void
- ParseResult::parse_option
- (
- std::shared_ptr<OptionDetails> value,
- const std::string& /*name*/,
- const std::string& arg
- )
+ inline void
+ ParseResult::parse_option(std::shared_ptr<OptionDetails> value,
+ const std::string& /*name*/,
+ const std::string& arg)
{
auto& result = m_results[value];
result.parse(value, arg);
@@ -1589,16 +1427,12 @@ namespace cxxopts
m_sequential.emplace_back(value->long_name(), arg);
}
- inline
- void
- ParseResult::checked_parse_arg
- (
- int argc,
- char* argv[],
- int& current,
- std::shared_ptr<OptionDetails> value,
- const std::string& name
- )
+ inline void
+ ParseResult::checked_parse_arg(int argc,
+ char* argv[],
+ int& current,
+ std::shared_ptr<OptionDetails> value,
+ const std::string& name)
{
if (current + 1 >= argc)
{
@@ -1625,8 +1459,7 @@ namespace cxxopts
}
}
- inline
- void
+ inline void
ParseResult::add_to_option(const std::string& option, const std::string& arg)
{
auto iter = m_options->find(option);
@@ -1639,8 +1472,7 @@ namespace cxxopts
parse_option(iter->second, option, arg);
}
- inline
- bool
+ inline bool
ParseResult::consume_positional(std::string a)
{
while (m_next_positional != m_positional.end())
@@ -1675,15 +1507,13 @@ namespace cxxopts
return false;
}
- inline
- void
+ inline void
Options::parse_positional(std::string option)
{
- parse_positional(std::vector<std::string> {std::move(option)});
+ parse_positional(std::vector<std::string>{std::move(option)});
}
- inline
- void
+ inline void
Options::parse_positional(std::vector<std::string> options)
{
m_positional = std::move(options);
@@ -1692,23 +1522,20 @@ namespace cxxopts
m_positional_set.insert(m_positional.begin(), m_positional.end());
}
- inline
- void
+ inline void
Options::parse_positional(std::initializer_list<std::string> options)
{
parse_positional(std::vector<std::string>(std::move(options)));
}
- inline
- ParseResult
+ inline ParseResult
Options::parse(int& argc, char**& argv)
{
ParseResult result(m_options, m_positional, m_allow_unrecognised, argc, argv);
return result;
}
- inline
- void
+ inline void
ParseResult::parse(int& argc, char**& argv)
{
int current = 1;
@@ -1832,7 +1659,6 @@ namespace cxxopts
checked_parse_arg(argc, argv, current, opt, name);
}
}
-
}
++current;
@@ -1872,20 +1698,15 @@ namespace cxxopts
}
argc = nextKeep;
-
}
- inline
- void
- Options::add_option
- (
- const std::string& group,
- const std::string& s,
- const std::string& l,
- std::string desc,
- std::shared_ptr<const Value> value,
- std::string arg_help
- )
+ inline void
+ Options::add_option(const std::string& group,
+ const std::string& s,
+ const std::string& l,
+ std::string desc,
+ std::shared_ptr<const Value> value,
+ std::string arg_help)
{
auto stringDesc = toLocalString(std::move(desc));
auto option = std::make_shared<OptionDetails>(s, l, stringDesc, value);
@@ -1903,21 +1724,20 @@ namespace cxxopts
//add the help details
auto& options = m_help[group];
- options.options.emplace_back(HelpOptionDetails{s, l, stringDesc,
- value->has_default(), value->get_default_value(),
- value->has_implicit(), value->get_implicit_value(),
- std::move(arg_help),
- value->is_container(),
- value->is_boolean()});
+ options.options.emplace_back(HelpOptionDetails{s,
+ l,
+ stringDesc,
+ value->has_default(),
+ value->get_default_value(),
+ value->has_implicit(),
+ value->get_implicit_value(),
+ std::move(arg_help),
+ value->is_container(),
+ value->is_boolean()});
}
- inline
- void
- Options::add_one_option
- (
- const std::string& option,
- std::shared_ptr<OptionDetails> details
- )
+ inline void
+ Options::add_one_option(const std::string& option, std::shared_ptr<OptionDetails> details)
{
auto in = m_options->emplace(option, details);
@@ -1927,8 +1747,7 @@ namespace cxxopts
}
}
- inline
- String
+ inline String
Options::help_one_group(const std::string& g) const
{
typedef std::vector<std::pair<String, String>> OptionHelp;
@@ -1952,8 +1771,7 @@ namespace cxxopts
for (const auto& o : group->second.options)
{
- if (o.is_container &&
- m_positional_set.find(o.l) != m_positional_set.end() &&
+ if (o.is_container && m_positional_set.find(o.l) != m_positional_set.end() &&
!m_show_positional)
{
continue;
@@ -1972,8 +1790,7 @@ namespace cxxopts
auto fiter = format.begin();
for (const auto& o : group->second.options)
{
- if (o.is_container &&
- m_positional_set.find(o.l) != m_positional_set.end() &&
+ if (o.is_container && m_positional_set.find(o.l) != m_positional_set.end() &&
!m_show_positional)
{
continue;
@@ -1989,9 +1806,8 @@ namespace cxxopts
}
else
{
- result += toLocalString(std::string(longest + OPTION_DESC_GAP -
- stringLength(fiter->first),
- ' '));
+ result += toLocalString(
+ std::string(longest + OPTION_DESC_GAP - stringLength(fiter->first), ' '));
}
result += d;
result += '\n';
@@ -2002,13 +1818,8 @@ namespace cxxopts
return result;
}
- inline
- void
- Options::generate_group_help
- (
- String& result,
- const std::vector<std::string>& print_groups
- ) const
+ inline void
+ Options::generate_group_help(String& result, const std::vector<std::string>& print_groups) const
{
for (size_t i = 0; i != print_groups.size(); ++i)
{
@@ -2025,8 +1836,7 @@ namespace cxxopts
}
}
- inline
- void
+ inline void
Options::generate_all_groups_help(String& result) const
{
std::vector<std::string> all_groups;
@@ -2040,12 +1850,11 @@ namespace cxxopts
generate_group_help(result, all_groups);
}
- inline
- std::string
+ inline std::string
Options::help(const std::vector<std::string>& help_groups) const
{
- String result = m_help_string + "\nUsage:\n " +
- toLocalString(m_program) + " " + toLocalString(m_custom_help);
+ String result = m_help_string + "\nUsage:\n " + toLocalString(m_program) + " " +
+ toLocalString(m_custom_help);
if (m_positional.size() > 0 && m_positional_help.size() > 0)
{
@@ -2066,32 +1875,26 @@ namespace cxxopts
return toUTF8String(result);
}
- inline
- const std::vector<std::string>
+ inline const std::vector<std::string>
Options::groups() const
{
std::vector<std::string> g;
- std::transform(
- m_help.begin(),
- m_help.end(),
- std::back_inserter(g),
- [](const std::map<std::string, HelpGroupDetails>::value_type & pair)
- {
- return pair.first;
- }
- );
+ std::transform(m_help.begin(),
+ m_help.end(),
+ std::back_inserter(g),
+ [](const std::map<std::string, HelpGroupDetails>::value_type& pair)
+ { return pair.first; });
return g;
}
- inline
- const HelpGroupDetails&
+ inline const HelpGroupDetails&
Options::group_help(const std::string& group) const
{
return m_help.at(group);
}
-}
+} // namespace cxxopts
#endif //CXXOPTS_HPP_INCLUDED
diff --git a/source/RobotAPI/applications/AronCodeGenerator/main.cpp b/source/RobotAPI/applications/AronCodeGenerator/main.cpp
index 05185cc8d7c1a244a9797668f91481a8d2357d5e..9d46772cecad9752fa89a61b70b7788b89bb45e5 100644
--- a/source/RobotAPI/applications/AronCodeGenerator/main.cpp
+++ b/source/RobotAPI/applications/AronCodeGenerator/main.cpp
@@ -21,9 +21,9 @@
*/
// STD/STL
-#include <iostream>
-#include <filesystem>
#include <ctime>
+#include <filesystem>
+#include <iostream>
// CXXOPTS
#include "cxxopts.hpp"
@@ -34,40 +34,43 @@
//#include <ArmarXCore/core/logging/Logging.h>
// Aron
-#include <RobotAPI/interface/aron.h>
-
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
-#include <RobotAPI/libraries/aron/codegeneration/typereader/xml/Reader.h>
+#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.h>
+#include <RobotAPI/libraries/aron/codegeneration/typereader/xml/Reader.h>
using namespace armarx;
using namespace aron;
-
/// Aron Code Generator Main Executable.
/// This executable calls generates a aron code file out of an aron xml file.
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
try
{
- cxxopts::Options options("AronArmemCodeGenerator", "An application interface for the aron and armem code generation");
+ cxxopts::Options options("AronArmemCodeGenerator",
+ "An application interface for the aron and armem code generation");
std::string input_default = "/path/to/some/xml/file.xml";
std::string output_default = "/path/to/some/output/folder/";
- options.add_options("General")
- ("v,verbose", "Enable verbose mode", cxxopts::value<bool>()->default_value("false"))
- ("h,help", "Print usage");
+ options.add_options("General")(
+ "v,verbose", "Enable verbose mode", cxxopts::value<bool>()->default_value("false"))(
+ "h,help", "Print usage");
- options.add_options("IO")
- ("i,input", "XML file name", cxxopts::value<std::string>()->default_value(input_default))
- ("o,output", "The output path", cxxopts::value<std::string>()->default_value(output_default))
- ("I,include", "include path", cxxopts::value<std::vector<std::string>>());
+ options.add_options("IO")("i,input",
+ "XML file name",
+ cxxopts::value<std::string>()->default_value(input_default))(
+ "o,output",
+ "The output path",
+ cxxopts::value<std::string>()->default_value(output_default))(
+ "I,include", "include path", cxxopts::value<std::vector<std::string>>());
- options.add_options("Generation")
- ("f,force", "Enforce the generation", cxxopts::value<bool>()->default_value("false"));
+ options.add_options("Generation")(
+ "f,force", "Enforce the generation", cxxopts::value<bool>()->default_value("false"));
auto result = options.parse(argc, argv);
@@ -111,9 +114,9 @@ int main(int argc, char* argv[])
std::vector<std::filesystem::path> includePaths;
- if(result.count("I") > 0)
+ if (result.count("I") > 0)
{
- for(const auto& path: result["I"].as<std::vector<std::string>>())
+ for (const auto& path : result["I"].as<std::vector<std::string>>())
{
includePaths.emplace_back(path);
}
@@ -126,7 +129,8 @@ int main(int argc, char* argv[])
exit(1);
}
- if (!std::filesystem::exists(output_folder) || !std::filesystem::is_directory(output_folder))
+ if (!std::filesystem::exists(output_folder) ||
+ !std::filesystem::is_directory(output_folder))
{
std::cerr << "The output folder does not exist or is not an directory." << std::endl;
exit(1);
@@ -148,7 +152,8 @@ int main(int argc, char* argv[])
if (verbose)
{
std::cout << "Parsing the XML file... done!" << std::endl;
- std::cout << "--> Found " << reader.getGenerateObjects().size() << " types." << std::endl;
+ std::cout << "--> Found " << reader.getGenerateObjects().size() << " types."
+ << std::endl;
std::cout << "--> They are: " << std::endl;
for (const auto& generateType : reader.getGenerateObjects())
{
@@ -169,7 +174,8 @@ int main(int argc, char* argv[])
if (verbose)
{
std::cout << "Running the type class generator... done!" << std::endl;
- std::cout << "--> Found " << writer.getTypeClasses().size() << " type objects." << std::endl;
+ std::cout << "--> Found " << writer.getTypeClasses().size() << " type objects."
+ << std::endl;
std::cout << "--> They are: " << std::endl;
for (const auto& c : writer.getTypeClasses())
{
@@ -181,31 +187,27 @@ int main(int argc, char* argv[])
std::tm* now = std::localtime(¤t_time);
std::string current_year = std::to_string(now->tm_year + 1900);
- std::string fileDoc = std::string("* This file is part of ArmarX. \n") +
-"* \n" +
-"* Copyright (C) 2012-" + current_year + ", High Performance Humanoid Technologies (H2T), \n" +
-"* Karlsruhe Institute of Technology (KIT), all rights reserved. \n" +
-"* \n" +
-"* ArmarX is free software; you can redistribute it and/or modify \n" +
-"* it under the terms of the GNU General Public License version 2 as \n" +
-"* published by the Free Software Foundation. \n" +
-"* \n" +
-"* ArmarX is distributed in the hope that it will be useful, but \n" +
-"* WITHOUT ANY WARRANTY; without even the implied warranty of \n" +
-"* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the \n" +
-"* GNU General Public License for more details. \n" +
-"* \n" +
-"* You should have received a copy of the GNU General Public License \n" +
-"* along with this program. If not, see <http://www.gnu.org/licenses/>. \n" +
-"* \n" +
-"* @copyright http://www.gnu.org/licenses/gpl-2.0.txt \n" +
-"* GNU General Public License \n" +
-"* *********************************************************************** \n" +
-"* WARNING: This file is autogenerated. \n" +
-"* Original file: " + filename + " \n" +
-"* Please do not edit since your changes may be overwritten on the next generation \n" +
-"* If you have any questions please contact: Fabian Peller-Konrad (fabian dot peller-konrad at kit dot edu) \n" +
-"* ***********************************************************************";
+ std::string fileDoc =
+ std::string("* This file is part of ArmarX. \n") + "* \n" + "* Copyright (C) 2012-" +
+ current_year + ", High Performance Humanoid Technologies (H2T), \n" +
+ "* Karlsruhe Institute of Technology (KIT), all rights reserved. \n" + "* \n" +
+ "* ArmarX is free software; you can redistribute it and/or modify \n" +
+ "* it under the terms of the GNU General Public License version 2 as \n" +
+ "* published by the Free Software Foundation. \n" + "* \n" +
+ "* ArmarX is distributed in the hope that it will be useful, but \n" +
+ "* WITHOUT ANY WARRANTY; without even the implied warranty of \n" +
+ "* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the \n" +
+ "* GNU General Public License for more details. \n" + "* \n" +
+ "* You should have received a copy of the GNU General Public License \n" +
+ "* along with this program. If not, see <http://www.gnu.org/licenses/>. \n" + "* \n" +
+ "* @copyright http://www.gnu.org/licenses/gpl-2.0.txt \n" +
+ "* GNU General Public License \n" +
+ "* *********************************************************************** \n" +
+ "* WARNING: This file is autogenerated. \n" + "* Original file: " + filename + " \n" +
+ "* Please do not edit since your changes may be overwritten on the next generation \n" +
+ "* If you have any questions please contact: Fabian Peller-Konrad (fabian dot "
+ "peller-konrad at kit dot edu) \n" +
+ "* ***********************************************************************";
auto w = CppWriterPtr(new CppWriter(true, fileDoc));
w->header.line();
@@ -234,7 +236,9 @@ int main(int argc, char* argv[])
if (enums.size() > 0 && enum_file_generation_content == "")
{
- std::cerr << "\033[31m" << "Error code 11 - Found error in code generation. Aborting!" << "\033[0m" << std::endl;
+ std::cerr << "\033[31m"
+ << "Error code 11 - Found error in code generation. Aborting!"
+ << "\033[0m" << std::endl;
exit(1);
}
@@ -252,7 +256,8 @@ int main(int argc, char* argv[])
w->body.line();
w->body.line();
- std::string class_file_generation_content = simox::alg::remove_prefix(w->getString(), enum_file_generation_content);
+ std::string class_file_generation_content =
+ simox::alg::remove_prefix(w->getString(), enum_file_generation_content);
if (verbose)
{
@@ -261,7 +266,9 @@ int main(int argc, char* argv[])
if (classes.size() > 0 && class_file_generation_content == "")
{
- std::cerr << "\033[31m" << "Error code 21 - Found error in code generation. Aborting!" << "\033[0m" << std::endl;
+ std::cerr << "\033[31m"
+ << "Error code 21 - Found error in code generation. Aborting!"
+ << "\033[0m" << std::endl;
exit(1);
}
@@ -278,13 +285,17 @@ int main(int argc, char* argv[])
if (std::filesystem::exists(output_file))
{
std::ifstream ifs(output_file);
- std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
+ std::string file_content((std::istreambuf_iterator<char>(ifs)),
+ (std::istreambuf_iterator<char>()));
if (file_content == new_file_full_content)
{
if (verbose)
{
- std::cout << "\033[31m" << "Error code 31 - File content not changed for <" + output_file.string() + ">" << "\033[0m" << std::endl;
+ std::cout << "\033[31m"
+ << "Error code 31 - File content not changed for <" +
+ output_file.string() + ">"
+ << "\033[0m" << std::endl;
}
exit(0);
}
@@ -298,12 +309,18 @@ int main(int argc, char* argv[])
if (verbose)
{
- std::cout << "\033[32m" << "Finished generating <" + output_file.string() + ">. The new file ist called <" << output_file.string() << ">" << "\033[0m" << std::endl;
+ std::cout << "\033[32m"
+ << "Finished generating <" + output_file.string() +
+ ">. The new file ist called <"
+ << output_file.string() << ">"
+ << "\033[0m" << std::endl;
}
}
catch (const cxxopts::OptionException& e)
{
- std::cerr << "\033[31m" << "Error code 01 - Error in parsing cxxopts options: " << e.what() << "\033[0m" << std::endl;
+ std::cerr << "\033[31m"
+ << "Error code 01 - Error in parsing cxxopts options: " << e.what() << "\033[0m"
+ << std::endl;
exit(1);
}
exit(0);
diff --git a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
index e67717c8620fdd21bee2b219fcb4404c14afb6d5..3c0de6e24ffbb146896aa0154acd822f6960de8c 100644
--- a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
+++ b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.cpp
@@ -23,35 +23,40 @@
*/
#include "RobotControlUI.h"
-#include <ArmarXCore/core/application/Application.h>
#include <iostream>
+
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
namespace armarx
{
- void RobotControlUI::onInitComponent()
+ void
+ RobotControlUI::onInitComponent()
{
usingProxy("RobotControl");
stateId = -1;
controlTask = new RunningTask<RobotControlUI>(this, &RobotControlUI::run, "RobotControlUI");
}
- void RobotControlUI::onConnectComponent()
+ void
+ RobotControlUI::onConnectComponent()
{
robotProxy = getProxy<RobotControlInterfacePrx>("RobotControl");
controlTask->start();
}
- void RobotControlUI::onExitComponent()
+ void
+ RobotControlUI::onExitComponent()
{
controlTask->stop();
}
- void RobotControlUI::run()
+ void
+ RobotControlUI::run()
{
std::string eventstring;
- std::cout << "Please insert the event string: " << std::flush;
+ std::cout << "Please insert the event string: " << std::flush;
// cin >> eventstring;
eventstring = "EvLoadScenario";
@@ -61,7 +66,8 @@ namespace armarx
}
else
{
- std::cout << "Please insert the state id of the state that should process the event: " << std::flush;
+ std::cout << "Please insert the state id of the state that should process the event: "
+ << std::flush;
int id;
// cin >> id;
id = 11;
@@ -74,4 +80,4 @@ namespace armarx
// cin >> eventstring;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h
index d402525e5c3cac9756c1c4537460637f09576a15..811959bd59b24b1d668310b6d69a1ec096b4d26a 100644
--- a/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h
+++ b/source/RobotAPI/applications/RobotControlUI/RobotControlUI.h
@@ -25,30 +25,34 @@
#pragma once
// ArmarXCore
+#include <string>
+
#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/IceManager.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+
#include <RobotAPI/statecharts/operations/RobotControl.h>
-#include <string>
namespace armarx
{
- class RobotControlUI :
- virtual public Component
+ class RobotControlUI : virtual public Component
{
public:
int stateId;
RobotControlInterfacePrx robotProxy;
- std::string getDefaultName() const override
+
+ std::string
+ getDefaultName() const override
{
return "RobotControlUI";
}
+
void onInitComponent() override;
void onConnectComponent() override;
void onExitComponent() override;
void run();
+
private:
RunningTask<RobotControlUI>::pointer_type controlTask;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/applications/RobotControlUI/main.cpp b/source/RobotAPI/applications/RobotControlUI/main.cpp
index df21ab13fcf25f6e641c44e844ef3552da894b41..7e6f2872d30a738052845ee7d7e6ddbb1a39d6e8 100644
--- a/source/RobotAPI/applications/RobotControlUI/main.cpp
+++ b/source/RobotAPI/applications/RobotControlUI/main.cpp
@@ -22,13 +22,14 @@
* GNU General Public License
*/
-#include "RobotControlUI.h"
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include "RobotControlUI.h"
+
+int
+main(int argc, char* argv[])
{
return armarx::runSimpleComponentApp<armarx::RobotControlUI>(argc, argv, "RobotControlUI");
}
diff --git a/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h b/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
index 20b65851b2c98abb9f61081b9552e3c18194c9e2..7ebc96b023e821df3e7b20e189866d0dfc2776bb 100644
--- a/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
+++ b/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
@@ -21,8 +21,8 @@
*/
-
#include <ArmarXCore/core/application/Application.h>
+
#include <RobotAPI/components/RobotState/RobotStateComponent.h>
namespace armarx
@@ -30,13 +30,14 @@ namespace armarx
/**
* Application for testing armarx::RobotStateComponent
*/
- class RobotStateComponentApp :
- virtual public armarx::Application
+ class RobotStateComponentApp : virtual public armarx::Application
{
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties) override
+ void
+ setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties) override
{
auto state = Component::create<RobotStateComponent>(properties);
auto observer = Component::create<RobotStateObserver>(properties);
@@ -45,4 +46,4 @@ namespace armarx
registry->addObject(observer);
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/applications/RobotStateComponent/main.cpp b/source/RobotAPI/applications/RobotStateComponent/main.cpp
index 9fa623ed0de139c3c4a3f70c3f387a013f6b909a..e5bc173d5f155b58e9efcc8a7974a9b132ae99d4 100644
--- a/source/RobotAPI/applications/RobotStateComponent/main.cpp
+++ b/source/RobotAPI/applications/RobotStateComponent/main.cpp
@@ -22,12 +22,15 @@
* GNU General Public License
*/
-#include "RobotStateComponentApp.h"
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include "RobotStateComponentApp.h"
+
+int
+main(int argc, char* argv[])
{
- armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::RobotStateComponentApp>();
+ armarx::ApplicationPtr app =
+ armarx::Application::createInstance<armarx::RobotStateComponentApp>();
app->setName("RobotStateComponent");
return app->main(argc, argv);
diff --git a/source/RobotAPI/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp b/source/RobotAPI/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp
index 0c70e4777c39461b2ca0336513436c5117d4731b..23464bfedfddc4f72bad369fa2b48695d1f575a6 100644
--- a/source/RobotAPI/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp
+++ b/source/RobotAPI/applications/SimpleEpisodicMemoryKinematicUnitConnector/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotAPI/components/SimpleEpisodicMemoryKinematicUnitConnector/SimpleEpisodicMemoryKinematicUnitConnector.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::SimpleEpisodicMemoryKinematicUnitConnector > (argc, argv, "SimpleEpisodicMemoryKinematicUnitConnector");
+ return armarx::runSimpleComponentApp<armarx::SimpleEpisodicMemoryKinematicUnitConnector>(
+ argc, argv, "SimpleEpisodicMemoryKinematicUnitConnector");
}
diff --git a/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h b/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
index 881b9db33c665f80f64183e75b817de19c8d3cfa..18659a97e4c8a44107ad192098df45cc9df8fd4f 100644
--- a/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
+++ b/source/RobotAPI/applications/TCPControlUnit/TCPControlUnitApp.h
@@ -24,21 +24,20 @@
#include <ArmarXCore/core/application/Application.h>
+
#include <RobotAPI/components/units/TCPControlUnit.h>
#include <RobotAPI/components/units/TCPControlUnitObserver.h>
namespace armarx
{
- class TCPControlUnitApp :
- virtual public armarx::Application
+ class TCPControlUnitApp : virtual public armarx::Application
{
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties) override
+ void
+ setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties) override
{
registry->addObject(Component::create<TCPControlUnit>(properties));
registry->addObject(Component::create<TCPControlUnitObserver>(properties));
}
};
-}
-
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/applications/TCPControlUnit/main.cpp b/source/RobotAPI/applications/TCPControlUnit/main.cpp
index a42ad3ea58def63d73d6bfa66a8b9fd28c8ccb1c..700da13d58da6df298c8bb665b1dd7f1910229f9 100644
--- a/source/RobotAPI/applications/TCPControlUnit/main.cpp
+++ b/source/RobotAPI/applications/TCPControlUnit/main.cpp
@@ -22,10 +22,12 @@
* GNU General Public License
*/
-#include "TCPControlUnitApp.h"
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include "TCPControlUnitApp.h"
+
+int
+main(int argc, char* argv[])
{
armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::TCPControlUnitApp>();
app->setName("TCPControlUnit");
diff --git a/source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h b/source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h
index f9e9ea0a98d92c081469733377931e6bb348c973..6128d92344d2581166ddb0bb39ef6fe92ba1b6eb 100644
--- a/source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h
+++ b/source/RobotAPI/applications/WeissHapticUnit/WeissHapticUnitApp.h
@@ -25,6 +25,7 @@
#pragma once
#include <ArmarXCore/core/application/Application.h>
+
#include <RobotAPI/components/units/HapticObserver.h>
#include <RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.h>
@@ -36,18 +37,17 @@ namespace armarx
*
* This Application runs both the WeissHapticUnit and the HapticObserver in one executable to avoid TCP communication.
*/
- class WeissHapticUnitApp :
- virtual public armarx::Application
+ class WeissHapticUnitApp : virtual public armarx::Application
{
/**
* @see armarx::Application::setup()
*/
- void setup(const ManagedIceObjectRegistryInterfacePtr& registry,
- Ice::PropertiesPtr properties) override
+ void
+ setup(const ManagedIceObjectRegistryInterfacePtr& registry,
+ Ice::PropertiesPtr properties) override
{
registry->addObject(Component::create<WeissHapticUnit>(properties));
registry->addObject(Component::create<HapticObserver>(properties));
}
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/applications/WeissHapticUnit/main.cpp b/source/RobotAPI/applications/WeissHapticUnit/main.cpp
index 0358a13066629555d8239b56e14f4e02bc2fb610..60e16951a2f6c08d38ad4a51a91f3ee59cda2b98 100644
--- a/source/RobotAPI/applications/WeissHapticUnit/main.cpp
+++ b/source/RobotAPI/applications/WeissHapticUnit/main.cpp
@@ -22,12 +22,14 @@
* GNU General Public License
*/
-#include "WeissHapticUnitApp.h"
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include "WeissHapticUnitApp.h"
+
+int
+main(int argc, char* argv[])
{
- armarx::ApplicationPtr app = armarx::Application::createInstance < armarx::WeissHapticUnitApp > ();
+ armarx::ApplicationPtr app = armarx::Application::createInstance<armarx::WeissHapticUnitApp>();
app->setName("WeissHapticUnit");
return app->main(argc, argv);
diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
index aff781b99e790b4aa9dbab8345c78387667a3f2d..c66092ff00bc286fc0c69027859bb47169421921 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
@@ -85,8 +85,10 @@ namespace armarx
"Destination path where the history is serialized to");
- defs->optional(properties_.componentWarnFrequency, "ComponentWarnFrequency",
- "Define a frequency in Hz above which the compnent raises a warning. As you should not send data at a too high rate.");
+ defs->optional(properties_.componentWarnFrequency,
+ "ComponentWarnFrequency",
+ "Define a frequency in Hz above which the compnent raises a warning. As you "
+ "should not send data at a too high rate.");
return defs;
}
@@ -171,7 +173,7 @@ namespace armarx
{
ARMARX_WARNING << deactivateSpam(10) << "Component `" << componentName << "`"
<< "sends data at a too high rate ("
- << history.size() / maxHistoryDur.toSecondsDouble() << ")"
+ << history.size() / maxHistoryDur.toSecondsDouble() << ")"
<< deactivateSpam(10, componentName);
}
}
diff --git a/source/RobotAPI/components/ArViz/ArVizStorageMain.cpp b/source/RobotAPI/components/ArViz/ArVizStorageMain.cpp
index d8e9f97d5bd9be83765f82ffcb9711c8b6e666f7..82bc151db943368cbdaebc0d86bff5ba8f216d4e 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorageMain.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorageMain.cpp
@@ -1,10 +1,11 @@
-#include <RobotAPI/components/ArViz/ArVizStorage.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotAPI/components/ArViz/ArVizStorage.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::ArVizStorage > (argc, argv, "ArVizStorage");
+ return armarx::runSimpleComponentApp<armarx::ArVizStorage>(argc, argv, "ArVizStorage");
}
diff --git a/source/RobotAPI/components/ArViz/Client/Client.cpp b/source/RobotAPI/components/ArViz/Client/Client.cpp
index d6281ea2098784dc7ca6b948eec6c8daf17e86e0..cf99444c9df48b365849b41c65ada2c5ded21dd7 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Client.cpp
@@ -5,110 +5,120 @@
namespace armarx::viz
{
-Client::Client(Component& component, std::string const& topicNameProperty, std::string const& storageNameProperty)
-{
- componentName = component.getName();
- component.getTopicFromProperty(topic, topicNameProperty);
+ Client::Client(Component& component,
+ std::string const& topicNameProperty,
+ std::string const& storageNameProperty)
+ {
+ componentName = component.getName();
+ component.getTopicFromProperty(topic, topicNameProperty);
+
+ // Optional dependency on ArVizStorage
+ std::string storageName;
+ if (component.hasProperty(storageNameProperty))
+ {
+ storageName = component.getProperty<std::string>(storageNameProperty);
+ }
+ else
+ {
+ storageName = "ArVizStorage";
+ }
+ component.getProxy(storage, storageName);
+ }
- // Optional dependency on ArVizStorage
- std::string storageName;
- if (component.hasProperty(storageNameProperty))
+ Client::Client(ManagedIceObject& obj,
+ std::string const& topicName,
+ std::string const& storageName)
{
- storageName = component.getProperty<std::string>(storageNameProperty);
+ componentName = obj.getName();
+ ARMARX_CHECK_NOT_EMPTY(componentName)
+ << "ArViz client must be created with non-empty component name.";
+ obj.getTopic(topic, topicName);
+ obj.getProxy(storage, storageName);
}
- else
+
+ Client
+ Client::createFromTopic(std::string const& componentName, Topic::ProxyType const& topic)
{
- storageName = "ArVizStorage";
+ Client client;
+ ARMARX_CHECK_NOT_EMPTY(componentName);
+ client.componentName = componentName;
+ client.topic = topic;
+ return client;
}
- component.getProxy(storage, storageName);
-}
-Client::Client(ManagedIceObject& obj,
- std::string const& topicName,
- std::string const& storageName)
-{
- componentName = obj.getName();
- ARMARX_CHECK_NOT_EMPTY(componentName) << "ArViz client must be created with non-empty component name.";
- obj.getTopic(topic, topicName);
- obj.getProxy(storage, storageName);
-}
+ Client
+ Client::createFromProxies(std::string const& componentName,
+ Topic::ProxyType const& topic,
+ StorageAndTopicInterfacePrx const& storage)
+ {
+ Client client;
+ client.componentName = componentName;
+ client.topic = topic;
+ client.storage = storage;
+ return client;
+ }
-Client Client::createFromTopic(std::string const& componentName, Topic::ProxyType const& topic)
-{
- Client client;
- ARMARX_CHECK_NOT_EMPTY(componentName);
- client.componentName = componentName;
- client.topic = topic;
- return client;
-}
-
-Client Client::createFromProxies(std::string const& componentName,
- Topic::ProxyType const& topic,
- StorageAndTopicInterfacePrx const& storage)
-{
- Client client;
- client.componentName = componentName;
- client.topic = topic;
- client.storage = storage;
- return client;
-}
-
-Client Client::createForGuiPlugin(Component& component,
- std::string const& topicName,
- std::string const& storageName)
-{
- Client client;
- std::string name = component.getName();
- std::size_t dashPos = name.find('-');
- if (dashPos != std::string::npos)
+ Client
+ Client::createForGuiPlugin(Component& component,
+ std::string const& topicName,
+ std::string const& storageName)
{
- name = name.substr(0, dashPos);
+ Client client;
+ std::string name = component.getName();
+ std::size_t dashPos = name.find('-');
+ if (dashPos != std::string::npos)
+ {
+ name = name.substr(0, dashPos);
+ }
+ client.componentName = name;
+ component.getTopic(client.topic, topicName);
+ component.getProxy(client.storage, storageName);
+ return client;
}
- client.componentName = name;
- component.getTopic(client.topic, topicName);
- component.getProxy(client.storage, storageName);
- return client;
-}
-Layer Client::layer(const std::string &name) const
-{
- ARMARX_CHECK_NOT_EMPTY(componentName) << "Layers must be created with non-empty component name.";
- ARMARX_CHECK_NOT_EMPTY(name) << "Layers must be created with non-empty layer name.";
- return Layer(componentName, name);
-}
+ Layer
+ Client::layer(const std::string& name) const
+ {
+ ARMARX_CHECK_NOT_EMPTY(componentName)
+ << "Layers must be created with non-empty component name.";
+ ARMARX_CHECK_NOT_EMPTY(name) << "Layers must be created with non-empty layer name.";
+ return Layer(componentName, name);
+ }
-CommitResult Client::commit(const StagedCommit& commit)
-{
- CommitResult result;
+ CommitResult
+ Client::commit(const StagedCommit& commit)
+ {
+ CommitResult result;
- ARMARX_CHECK_NOT_NULL(storage);
- result.data_ = storage->commitAndReceiveInteractions(commit.data_);
- return result;
-}
+ ARMARX_CHECK_NOT_NULL(storage);
+ result.data_ = storage->commitAndReceiveInteractions(commit.data_);
+ return result;
+ }
-CommitResultAsync Client::commitAsync(const StagedCommit& commit)
-{
- CommitResultAsync result;
+ CommitResultAsync
+ Client::commitAsync(const StagedCommit& commit)
+ {
+ CommitResultAsync result;
- ARMARX_CHECK_NOT_NULL(storage);
- result.async = storage->begin_commitAndReceiveInteractions(commit.data_);
- result.storage = storage;
- return result;
-}
+ ARMARX_CHECK_NOT_NULL(storage);
+ result.async = storage->begin_commitAndReceiveInteractions(commit.data_);
+ result.storage = storage;
+ return result;
+ }
-void Client::commit(const std::vector<Layer>& layers)
-{
- data::LayerUpdateSeq updates;
- updates.reserve(layers.size());
- for (Layer const& layer : layers)
+ void
+ Client::commit(const std::vector<Layer>& layers)
{
- updates.push_back(layer.data_);
+ data::LayerUpdateSeq updates;
+ updates.reserve(layers.size());
+ for (Layer const& layer : layers)
+ {
+ updates.push_back(layer.data_);
+ }
+ // This commit call still uses the legacy topic API
+ ARMARX_CHECK_NOT_NULL(topic);
+ topic->updateLayers(updates);
}
- // This commit call still uses the legacy topic API
- ARMARX_CHECK_NOT_NULL(topic);
- topic->updateLayers(updates);
-}
-
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index 84dcad9ce36dc40c34553afb085aa3cdd1925460..f3604353d15ef2ebef8f38ed7dbf7a1574896b81 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -3,18 +3,17 @@
#include <functional>
#include <vector>
-#include "Layer.h"
#include "Interaction.h"
-
+#include "Layer.h"
namespace armarx
{
class Component;
class ManagedIceObject;
-namespace viz
-{
- /**
+ namespace viz
+ {
+ /**
* A staged commit prepares multiple layers to be committed.
*
* Add all relevant layer updates via .add(layer).
@@ -28,157 +27,172 @@ namespace viz
* So you can have two threads calling .commit(A) and .commit(B)
* simultaneously without any locking mechanism.
*/
- struct StagedCommit
- {
- /**
+ struct StagedCommit
+ {
+ /**
* @brief Stage a layer to be committed later via client.apply(*this)
* @param layer The layer to be added to this staged commit.
*/
- void add(Layer const& layer)
- {
- data_.updates.push_back(layer.data_);
- }
-
- void add(std::initializer_list<Layer> layers)
- {
- data_.updates.reserve(data_.updates.size() + layers.size());
- for (Layer const& layer : layers)
+ void
+ add(Layer const& layer)
{
data_.updates.push_back(layer.data_);
}
- }
- /**
+ void
+ add(std::initializer_list<Layer> layers)
+ {
+ data_.updates.reserve(data_.updates.size() + layers.size());
+ for (Layer const& layer : layers)
+ {
+ data_.updates.push_back(layer.data_);
+ }
+ }
+
+ /**
* @brief Request interaction feedback for a particular layer.
* @param layer The layer you want to get interaction feedback for.
*/
- void requestInteraction(Layer const& layer)
- {
- data_.interactionComponent = layer.data_.component;
- data_.interactionLayers.push_back(layer.data_.name);
- }
+ void
+ requestInteraction(Layer const& layer)
+ {
+ data_.interactionComponent = layer.data_.component;
+ data_.interactionLayers.push_back(layer.data_.name);
+ }
- /**
+ /**
* @brief Reset all staged layers and interaction requests.
*/
- void reset()
- {
- data_.updates.clear();
- data_.interactionComponent.clear();
- data_.interactionLayers.clear();
- }
+ void
+ reset()
+ {
+ data_.updates.clear();
+ data_.interactionComponent.clear();
+ data_.interactionLayers.clear();
+ }
- viz::data::CommitInput data_;
- };
+ viz::data::CommitInput data_;
+ };
- struct CommitResult
- {
- long revision() const
+ struct CommitResult
{
- return data_.revision;
- }
+ long
+ revision() const
+ {
+ return data_.revision;
+ }
- InteractionFeedbackRange interactions() const
- {
- const InteractionFeedback* begin = reinterpret_cast<const InteractionFeedback*>(data_.interactions.data());
- const InteractionFeedback* end = begin + data_.interactions.size();
- return InteractionFeedbackRange{begin, end};
- }
+ InteractionFeedbackRange
+ interactions() const
+ {
+ const InteractionFeedback* begin =
+ reinterpret_cast<const InteractionFeedback*>(data_.interactions.data());
+ const InteractionFeedback* end = begin + data_.interactions.size();
+ return InteractionFeedbackRange{begin, end};
+ }
- data::CommitResult data_;
- };
+ data::CommitResult data_;
+ };
- struct CommitResultAsync
- {
- bool isAvailable() const
+ struct CommitResultAsync
{
- return async && async->isCompleted();
- }
+ bool
+ isAvailable() const
+ {
+ return async && async->isCompleted();
+ }
- CommitResult waitAndGet() const
+ CommitResult
+ waitAndGet() const
+ {
+ CommitResult result;
+ result.data_ = storage->end_commitAndReceiveInteractions(async);
+ return result;
+ }
+
+ Ice::AsyncResultPtr async;
+ armarx::viz::StorageInterfacePrx storage;
+ };
+
+ struct Client
{
- CommitResult result;
- result.data_ = storage->end_commitAndReceiveInteractions(async);
- return result;
- }
+ Client() = default;
+ Client(const Client&) = default;
- Ice::AsyncResultPtr async;
- armarx::viz::StorageInterfacePrx storage;
- };
+ Client(armarx::Component& component,
+ std::string const& topicNameProperty = "ArVizTopicName",
+ std::string const& storageNameProperty = "ArVizStorageName");
- struct Client
- {
- Client() = default;
- Client(const Client&) = default;
+ Client(ManagedIceObject& obj,
+ std::string const& topicName = "ArVizTopic",
+ std::string const& storageName = "ArVizStorage");
- Client(armarx::Component& component,
- std::string const& topicNameProperty = "ArVizTopicName",
- std::string const& storageNameProperty = "ArVizStorageName");
+ static Client createFromTopic(std::string const& componentName,
+ armarx::viz::Topic::ProxyType const& topic);
- Client(ManagedIceObject& obj,
- std::string const& topicName = "ArVizTopic",
- std::string const& storageName = "ArVizStorage");
+ static Client
+ createFromProxies(std::string const& componentName,
+ armarx::viz::Topic::ProxyType const& topic,
+ armarx::viz::StorageAndTopicInterfacePrx const& storage);
- static Client createFromTopic(std::string const& componentName,
- armarx::viz::Topic::ProxyType const& topic);
+ static Client createForGuiPlugin(armarx::Component& component,
+ std::string const& topicName = "ArVizTopic",
+ std::string const& storageName = "ArVizStorage");
- static Client createFromProxies(std::string const& componentName,
- armarx::viz::Topic::ProxyType const& topic,
- armarx::viz::StorageAndTopicInterfacePrx const& storage);
+ Layer layer(std::string const& name) const;
- static Client createForGuiPlugin(armarx::Component& component,
- std::string const& topicName = "ArVizTopic",
- std::string const& storageName = "ArVizStorage");
+ StagedCommit
+ stage()
+ {
+ return StagedCommit();
+ }
- Layer layer(std::string const& name) const;
+ CommitResult commit(StagedCommit const& commit);
- StagedCommit stage()
- {
- return StagedCommit();
- }
+ CommitResultAsync commitAsync(StagedCommit const& commit);
- CommitResult commit(StagedCommit const& commit);
+ void
+ commit(Layer const& layer)
+ {
+ std::vector<Layer> layers;
+ layers.push_back(layer);
+ commit(layers);
+ }
- CommitResultAsync commitAsync(StagedCommit const& commit);
+ void commit(std::vector<Layer> const& layers);
- void commit(Layer const& layer)
- {
- std::vector<Layer> layers;
- layers.push_back(layer);
- commit(layers);
- }
+ void
+ commitLayerContaining(std::string const& name)
+ {
+ std::vector<viz::Layer> layers;
+ layers.push_back(this->layer(name));
+ commit(layers);
+ }
- void commit(std::vector<Layer> const& layers);
+ template <typename ElementT>
+ void
+ commitLayerContaining(std::string const& name, ElementT const& element)
+ {
+ std::vector<viz::Layer> layers;
+ viz::Layer& newLayer = layers.emplace_back(this->layer(name));
+ newLayer.add(element);
+ commit(layers);
+ }
- void commitLayerContaining(std::string const& name)
- {
- std::vector<viz::Layer> layers;
- layers.push_back(this->layer(name));
- commit(layers);
- }
+ void
+ commitDeleteLayer(std::string const& name)
+ {
+ std::vector<viz::Layer> layers;
+ viz::Layer& layerToDelete = layers.emplace_back(this->layer(name));
+ layerToDelete.markForDeletion();
+ commit(layers);
+ }
- template <typename ElementT>
- void commitLayerContaining(std::string const& name, ElementT const& element)
- {
- std::vector<viz::Layer> layers;
- viz::Layer& newLayer = layers.emplace_back(this->layer(name));
- newLayer.add(element);
- commit(layers);
- }
+ private:
+ std::string componentName;
+ armarx::viz::StorageInterfacePrx storage;
+ armarx::viz::TopicPrx topic;
+ };
- void commitDeleteLayer(std::string const& name)
- {
- std::vector<viz::Layer> layers;
- viz::Layer& layerToDelete = layers.emplace_back(this->layer(name));
- layerToDelete.markForDeletion();
- commit(layers);
- }
-
- private:
- std::string componentName;
- armarx::viz::StorageInterfacePrx storage;
- armarx::viz::TopicPrx topic;
- };
-
-}
-}
+ } // namespace viz
+} // namespace armarx
diff --git a/source/RobotAPI/components/ArViz/Client/ClientCGALExtensions.h b/source/RobotAPI/components/ArViz/Client/ClientCGALExtensions.h
index 4888b73435befbab0e67f400d95c497d7ef19384..aa8acf9dbf6d70c3e3425f4ec55fd6016168f706 100644
--- a/source/RobotAPI/components/ArViz/Client/ClientCGALExtensions.h
+++ b/source/RobotAPI/components/ArViz/Client/ClientCGALExtensions.h
@@ -2,4 +2,3 @@
#include "Client.h"
#include "elements/MeshCGALExtensions.h"
-
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index d97fea3b03d55ddb82a379628197a5f04ffc3d74..24d6d93edaae86385115179f2052fc263ec0c9c3 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -1,20 +1,20 @@
#include "Elements.h"
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-#include <SimoxUtility/math/normal/normal_to_mat4.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include <SimoxUtility/math/normal/normal_to_mat4.h>
#include <SimoxUtility/math/pose/transform.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
namespace armarx::viz
{
struct Convert
{
- static Eigen::Quaternionf directionToQuaternion(Eigen::Vector3f dir)
+ static Eigen::Quaternionf
+ directionToQuaternion(Eigen::Vector3f dir)
{
dir = dir.normalized();
Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
@@ -41,24 +41,32 @@ namespace armarx::viz
}
};
- const std::string Object::DefaultObjectsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
- const std::string Object::DefaultRelativeObjectsDirectory = armarx::ObjectFinder::DefaultObjectsDirectory;
+ const std::string Object::DefaultObjectsPackage =
+ armarx::ObjectFinder::DefaultObjectsPackageName;
+ const std::string Object::DefaultRelativeObjectsDirectory =
+ armarx::ObjectFinder::DefaultObjectsDirectory;
- Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage,
- const std::string& relativeObjectsDirectory)
+ Object&
+ Object::fileByObjectFinder(const std::string& objectID,
+ const std::string& objectsPackage,
+ const std::string& relativeObjectsDirectory)
{
- return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage, relativeObjectsDirectory);
+ return this->fileByObjectFinder(
+ armarx::ObjectID(objectID), objectsPackage, relativeObjectsDirectory);
}
- Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage,
- const std::string& relativeObjectsDirectory)
+ Object&
+ Object::fileByObjectFinder(const armarx::ObjectID& objectID,
+ const std::string& objectsPackage,
+ const std::string& relativeObjectsDirectory)
{
ObjectInfo info(objectsPackage, "", relativeObjectsDirectory, objectID);
armarx::PackageFileLocation file = info.simoxXML();
return this->file(file.package, file.relativePath);
}
- Object& Object::alpha(float alpha)
+ Object&
+ Object::alpha(float alpha)
{
if (alpha < 1)
{
@@ -67,19 +75,22 @@ namespace armarx::viz
return *this;
}
- Box& Box::set(const simox::OrientedBoxBase<float>& b)
+ Box&
+ Box::set(const simox::OrientedBoxBase<float>& b)
{
size(b.dimensions());
return pose(b.transformation_centered());
}
- Box& Box::set(const simox::OrientedBoxBase<double>& b)
+ Box&
+ Box::set(const simox::OrientedBoxBase<double>& b)
{
size(b.dimensions<float>());
return pose(b.transformation_centered<float>());
}
- Cylinder& Cylinder::fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
+ Cylinder&
+ Cylinder::fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
{
position((to + from) / 2);
orientation(Convert::directionToQuaternion((to - from).normalized()));
@@ -88,19 +99,22 @@ namespace armarx::viz
return *this;
}
- Cylinder& Cylinder::direction(Eigen::Vector3f direction)
+ Cylinder&
+ Cylinder::direction(Eigen::Vector3f direction)
{
orientation(Convert::directionToQuaternion(direction));
return *this;
}
- Arrow& Arrow::direction(Eigen::Vector3f dir)
+ Arrow&
+ Arrow::direction(Eigen::Vector3f dir)
{
return orientation(Convert::directionToQuaternion(dir));
}
- ArrowCircle& ArrowCircle::normal(Eigen::Vector3f dir)
+ ArrowCircle&
+ ArrowCircle::normal(Eigen::Vector3f dir)
{
Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitZ();
Eigen::Vector3f cross = naturalDir.cross(dir);
@@ -117,7 +131,8 @@ namespace armarx::viz
return orientation(ori);
}
- Polygon& Polygon::points(const std::vector<Eigen::Vector3f>& ps)
+ Polygon&
+ Polygon::points(const std::vector<Eigen::Vector3f>& ps)
{
auto& points = data_->points;
points.clear();
@@ -130,14 +145,16 @@ namespace armarx::viz
return *this;
}
- Polygon& Polygon::plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size)
+ Polygon&
+ Polygon::plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size)
{
- const Eigen::Quaternionf ori = Eigen::Quaternionf::FromTwoVectors(
- Eigen::Vector3f::UnitZ(), plane.normal());
+ const Eigen::Quaternionf ori =
+ Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), plane.normal());
return this->plane(plane.projection(at), ori, size);
}
- Polygon& Polygon::plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size)
+ Polygon&
+ Polygon::plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size)
{
const Eigen::Vector3f x = 0.5f * size.x() * (orientation * Eigen::Vector3f::UnitX());
const Eigen::Vector3f y = 0.5f * size.y() * (orientation * Eigen::Vector3f::UnitY());
@@ -150,7 +167,11 @@ namespace armarx::viz
return *this;
}
- Polygon& Polygon::circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation)
+ Polygon&
+ Polygon::circle(Eigen::Vector3f center,
+ Eigen::Vector3f normal,
+ float radius,
+ std::size_t tessellation)
{
const Eigen::Matrix4f pose = simox::math::normal_pos_to_mat4(normal, center);
ARMARX_CHECK_GREATER_EQUAL(tessellation, 3);
@@ -170,6 +191,4 @@ namespace armarx::viz
}
-}
-
-
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 11379504f65b8bdbb702227602df5dcd5fdfa533..430c05d16ff6b7f6cf2b47fc0b4e0efb7fc1b1e2 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -1,10 +1,6 @@
#pragma once
-
-
-
-
#include "elements/Color.h"
#include "elements/ElementOps.h"
#include "elements/Line.h"
@@ -14,15 +10,15 @@
#include "elements/Robot.h"
//#include "elements/RobotHand.h" // Not included by default (exposes additional headers).
-#include <RobotAPI/interface/ArViz/Elements.h>
-
-#include <SimoxUtility/shapes/OrientedBoxBase.h>
+#include <ctime>
+#include <string>
-#include <Eigen/Geometry>
#include <Eigen/Core>
+#include <Eigen/Geometry>
-#include <ctime>
-#include <string>
+#include <SimoxUtility/shapes/OrientedBoxBase.h>
+
+#include <RobotAPI/interface/ArViz/Elements.h>
// The has_member macro causes compile errors if *any* other header uses
// the identifier has_member. Boost.Thread does, so this causes compile
@@ -42,7 +38,7 @@ namespace armarx
{
// <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
class ObjectID;
-}
+} // namespace armarx
namespace armarx::viz
{
@@ -52,7 +48,8 @@ namespace armarx::viz
{
using ElementOps::ElementOps;
- Box& size(Eigen::Vector3f const& s)
+ Box&
+ size(Eigen::Vector3f const& s)
{
data_->size.e0 = s.x();
data_->size.e1 = s.y();
@@ -61,7 +58,8 @@ namespace armarx::viz
return *this;
}
- Box& size(float s)
+ Box&
+ size(float s)
{
return size(Eigen::Vector3f(s, s, s));
}
@@ -70,19 +68,20 @@ namespace armarx::viz
Box& set(const simox::OrientedBoxBase<double>& b);
};
-
struct Cylinder : ElementOps<Cylinder, data::ElementCylinder>
{
using ElementOps::ElementOps;
- Cylinder& radius(float r)
+ Cylinder&
+ radius(float r)
{
data_->radius = r;
return *this;
}
- Cylinder& height(float h)
+ Cylinder&
+ height(float h)
{
data_->height = h;
@@ -94,24 +93,24 @@ namespace armarx::viz
Cylinder& direction(Eigen::Vector3f direction);
};
-
struct Cylindroid : ElementOps<Cylindroid, data::ElementCylindroid>
{
- Cylindroid(const std::string& name) :
- ElementOps(name)
+ Cylindroid(const std::string& name) : ElementOps(name)
{
data_->curvature.e0 = 1;
data_->curvature.e1 = 1;
}
- Cylindroid& height(float height)
+ Cylindroid&
+ height(float height)
{
data_->height = height;
return *this;
}
- Cylindroid& axisLengths(const Eigen::Vector2f& axisLengths)
+ Cylindroid&
+ axisLengths(const Eigen::Vector2f& axisLengths)
{
data_->axisLengths.e0 = axisLengths.x();
data_->axisLengths.e1 = axisLengths.y();
@@ -119,7 +118,8 @@ namespace armarx::viz
return *this;
}
- Cylindroid& curvature(const Eigen::Vector2f& curvature)
+ Cylindroid&
+ curvature(const Eigen::Vector2f& curvature)
{
// Warning: Custom curvatures are not yet supported by the visualization backend and
// are thus ignored.
@@ -130,12 +130,12 @@ namespace armarx::viz
}
};
-
struct Sphere : ElementOps<Sphere, data::ElementSphere>
{
using ElementOps::ElementOps;
- Sphere& radius(float r)
+ Sphere&
+ radius(float r)
{
data_->radius = r;
@@ -143,18 +143,17 @@ namespace armarx::viz
}
};
-
struct Ellipsoid : ElementOps<Ellipsoid, data::ElementEllipsoid>
{
- Ellipsoid(const std::string& name) :
- ElementOps(name)
+ Ellipsoid(const std::string& name) : ElementOps(name)
{
data_->curvature.e0 = 1;
data_->curvature.e1 = 1;
data_->curvature.e2 = 1;
}
- Ellipsoid& axisLengths(const Eigen::Vector3f& axisLengths)
+ Ellipsoid&
+ axisLengths(const Eigen::Vector3f& axisLengths)
{
data_->axisLengths.e0 = axisLengths.x();
data_->axisLengths.e1 = axisLengths.y();
@@ -163,7 +162,8 @@ namespace armarx::viz
return *this;
}
- Ellipsoid& curvature(const Eigen::Vector3f& curvature)
+ Ellipsoid&
+ curvature(const Eigen::Vector3f& curvature)
{
// Warning: Custom curvatures are not yet supported by the visualization backend and
// are thus ignored.
@@ -175,18 +175,17 @@ namespace armarx::viz
}
};
-
struct Pose : ElementOps<Pose, data::ElementPose>
{
using ElementOps::ElementOps;
};
-
struct Text : ElementOps<Text, data::ElementText>
{
using ElementOps::ElementOps;
- Text& text(std::string const& t)
+ Text&
+ text(std::string const& t)
{
data_->text = t;
@@ -194,28 +193,30 @@ namespace armarx::viz
}
};
-
struct Arrow : ElementOps<Arrow, data::ElementArrow>
{
using ElementOps::ElementOps;
Arrow& direction(Eigen::Vector3f dir);
- Arrow& length(float l)
+ Arrow&
+ length(float l)
{
data_->length = l;
return *this;
}
- Arrow& width(float w)
+ Arrow&
+ width(float w)
{
data_->width = w;
return *this;
}
- Arrow& fromTo(const Eigen::Vector3f& from, const Eigen::Vector3f& to)
+ Arrow&
+ fromTo(const Eigen::Vector3f& from, const Eigen::Vector3f& to)
{
position(from);
direction((to - from).normalized());
@@ -225,28 +226,30 @@ namespace armarx::viz
}
};
-
struct ArrowCircle : ElementOps<ArrowCircle, data::ElementArrowCircle>
{
using ElementOps::ElementOps;
ArrowCircle& normal(Eigen::Vector3f dir);
- ArrowCircle& radius(float r)
+ ArrowCircle&
+ radius(float r)
{
data_->radius = r;
return *this;
}
- ArrowCircle& width(float w)
+ ArrowCircle&
+ width(float w)
{
data_->width = w;
return *this;
}
- ArrowCircle& completion(float c)
+ ArrowCircle&
+ completion(float c)
{
data_->completion = c;
@@ -254,35 +257,39 @@ namespace armarx::viz
}
};
-
struct Polygon : ElementOps<Polygon, data::ElementPolygon>
{
using ElementOps::ElementOps;
- Polygon& clear()
+ Polygon&
+ clear()
{
data_->points.clear();
return *this;
}
- Polygon& lineColor(Color color)
+ Polygon&
+ lineColor(Color color)
{
data_->lineColor = color;
return *this;
}
- Polygon& lineColor(int r, int g, int b)
+ Polygon&
+ lineColor(int r, int g, int b)
{
return lineColor(viz::Color(r, g, b));
}
- Polygon& lineColorGlasbeyLUT(std::size_t id, int alpha = 255)
+ Polygon&
+ lineColorGlasbeyLUT(std::size_t id, int alpha = 255)
{
return lineColor(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
}
- Polygon& lineWidth(float w)
+ Polygon&
+ lineWidth(float w)
{
data_->lineWidth = w;
@@ -291,7 +298,8 @@ namespace armarx::viz
Polygon& points(std::vector<Eigen::Vector3f> const& ps);
- Polygon& addPoint(Eigen::Vector3f p)
+ Polygon&
+ addPoint(Eigen::Vector3f p)
{
data_->points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
@@ -312,12 +320,15 @@ namespace armarx::viz
* @param orientation The orientation of the coordinate system.
* @param size The XY-size of the rectangle.
*/
- Polygon& plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size);
+ Polygon&
+ plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size);
- Polygon& circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation = 64);
+ Polygon& circle(Eigen::Vector3f center,
+ Eigen::Vector3f normal,
+ float radius,
+ std::size_t tessellation = 64);
};
-
struct Object : ElementOps<Object, data::ElementObject>
{
static const std::string DefaultObjectsPackage;
@@ -325,14 +336,17 @@ namespace armarx::viz
using ElementOps::ElementOps;
- Object& file(std::string const& project, std::string const& filename)
+ Object&
+ file(std::string const& project, std::string const& filename)
{
data_->project = project;
data_->filename = filename;
return *this;
}
- Object& file(std::string const& filename)
+
+ Object&
+ file(std::string const& filename)
{
return file("", filename);
}
@@ -343,43 +357,48 @@ namespace armarx::viz
* @param objectsPackage The objects package ("PriorKnowledgeData" by default)
* @see <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
*/
- Object& fileByObjectFinder(const armarx::ObjectID& objectID,
- const std::string& objectsPackage = DefaultObjectsPackage,
- const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
- Object& fileByObjectFinder(const std::string& objectID,
- const std::string& objectsPackage = DefaultObjectsPackage,
- const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
+ Object& fileByObjectFinder(
+ const armarx::ObjectID& objectID,
+ const std::string& objectsPackage = DefaultObjectsPackage,
+ const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
+ Object& fileByObjectFinder(
+ const std::string& objectID,
+ const std::string& objectsPackage = DefaultObjectsPackage,
+ const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
Object& alpha(float alpha);
- Object& useCollisionModel()
+ Object&
+ useCollisionModel()
{
data_->drawStyle |= data::ModelDrawStyle::COLLISION;
return *this;
}
- Object& useFullModel()
+ Object&
+ useFullModel()
{
data_->drawStyle &= ~data::ModelDrawStyle::COLLISION;
return *this;
}
- Object& overrideColor(Color c)
+ Object&
+ overrideColor(Color c)
{
data_->drawStyle |= data::ModelDrawStyle::OVERRIDE_COLOR;
return color(c);
}
- Object& useOriginalColor()
+ Object&
+ useOriginalColor()
{
data_->drawStyle &= ~data::ModelDrawStyle::OVERRIDE_COLOR;
return *this;
}
-
};
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/Interaction.h b/source/RobotAPI/components/ArViz/Client/Interaction.h
index da979d84fe59c12ac9a08ef8ff00949722b07cd6..b3b90564ed26c04861e755bd6446c282466cb050 100644
--- a/source/RobotAPI/components/ArViz/Client/Interaction.h
+++ b/source/RobotAPI/components/ArViz/Client/Interaction.h
@@ -1,10 +1,10 @@
#pragma once
+#include <Eigen/Core>
+
#include <RobotAPI/components/ArViz/Client/elements/ElementOps.h>
#include <RobotAPI/interface/ArViz/Component.h>
-#include <Eigen/Core>
-
namespace armarx::viz
{
enum class InteractionFeedbackType
@@ -24,26 +24,28 @@ namespace armarx::viz
Transform,
};
- inline const char* toString(InteractionFeedbackType type)
+ inline const char*
+ toString(InteractionFeedbackType type)
{
switch (type)
{
- case InteractionFeedbackType::None:
- return "None";
- case InteractionFeedbackType::Select:
- return "Select";
- case InteractionFeedbackType::Deselect:
- return "Deselect";
- case InteractionFeedbackType::ContextMenuChosen:
- return "ContextMenuChosen";
- case InteractionFeedbackType::Transform:
- return "Transform";
- default:
- return "<Unknown>";
+ case InteractionFeedbackType::None:
+ return "None";
+ case InteractionFeedbackType::Select:
+ return "Select";
+ case InteractionFeedbackType::Deselect:
+ return "Deselect";
+ case InteractionFeedbackType::ContextMenuChosen:
+ return "ContextMenuChosen";
+ case InteractionFeedbackType::Transform:
+ return "Transform";
+ default:
+ return "<Unknown>";
}
}
- inline Eigen::Matrix4f toEigen(data::GlobalPose const& pose)
+ inline Eigen::Matrix4f
+ toEigen(data::GlobalPose const& pose)
{
Eigen::Quaternionf ori(pose.qw, pose.qx, pose.qy, pose.qz);
@@ -56,74 +58,84 @@ namespace armarx::viz
struct InteractionFeedback
{
- InteractionFeedbackType type() const
+ InteractionFeedbackType
+ type() const
{
// Maks out all the flags in the higher bits
int type = data_.type & 0x7;
switch (type)
{
- case data::InteractionFeedbackType::NONE:
- return InteractionFeedbackType::None;
+ case data::InteractionFeedbackType::NONE:
+ return InteractionFeedbackType::None;
- case data::InteractionFeedbackType::SELECT:
- return InteractionFeedbackType::Select;
+ case data::InteractionFeedbackType::SELECT:
+ return InteractionFeedbackType::Select;
- case data::InteractionFeedbackType::DESELECT:
- return InteractionFeedbackType::Deselect;
+ case data::InteractionFeedbackType::DESELECT:
+ return InteractionFeedbackType::Deselect;
- case data::InteractionFeedbackType::CONTEXT_MENU_CHOSEN:
- return InteractionFeedbackType::ContextMenuChosen;
+ case data::InteractionFeedbackType::CONTEXT_MENU_CHOSEN:
+ return InteractionFeedbackType::ContextMenuChosen;
- case data::InteractionFeedbackType::TRANSFORM:
- return InteractionFeedbackType::Transform;
+ case data::InteractionFeedbackType::TRANSFORM:
+ return InteractionFeedbackType::Transform;
- default:
- throw std::runtime_error("Unexpected InteractionFeedbackType");
+ default:
+ throw std::runtime_error("Unexpected InteractionFeedbackType");
}
}
- bool isTransformBegin() const
+ bool
+ isTransformBegin() const
{
return data_.type & data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG;
}
- bool isTransformDuring() const
+ bool
+ isTransformDuring() const
{
return data_.type & data::InteractionFeedbackType::TRANSFORM_DURING_FLAG;
}
- bool isTransformEnd() const
+ bool
+ isTransformEnd() const
{
return data_.type & data::InteractionFeedbackType::TRANSFORM_END_FLAG;
}
- std::string const& layer() const
+ std::string const&
+ layer() const
{
return data_.layer;
}
- std::string const& element() const
+ std::string const&
+ element() const
{
return data_.element;
}
- long revision() const
+ long
+ revision() const
{
return data_.revision;
}
- int chosenContextMenuEntry() const
+ int
+ chosenContextMenuEntry() const
{
return data_.chosenContextMenuEntry;
}
- Eigen::Matrix4f transformation() const
+ Eigen::Matrix4f
+ transformation() const
{
return toEigen(data_.transformation);
}
- Eigen::Vector3f scale() const
+ Eigen::Vector3f
+ scale() const
{
Eigen::Vector3f result(data_.scale.e0, data_.scale.e1, data_.scale.e2);
return result;
@@ -134,22 +146,26 @@ namespace armarx::viz
struct InteractionFeedbackRange
{
- InteractionFeedback const* begin() const
+ InteractionFeedback const*
+ begin() const
{
return begin_;
}
- InteractionFeedback const* end() const
+ InteractionFeedback const*
+ end() const
{
return end_;
}
- std::size_t size() const
+ std::size_t
+ size() const
{
return end_ - begin_;
}
- bool empty() const
+ bool
+ empty() const
{
return begin_ == end_;
}
@@ -171,91 +187,99 @@ namespace armarx::viz
template <typename ElementT>
struct Transformable
{
- Transformable(std::string const& name)
- : element(name)
+ Transformable(std::string const& name) : element(name)
{
}
- Transformable& pose(Eigen::Matrix4f const& pose)
+ Transformable&
+ pose(Eigen::Matrix4f const& pose)
{
element.pose(pose);
initialPose = pose;
return *this;
}
- Transformable& position(Eigen::Vector3f const& position)
+ Transformable&
+ position(Eigen::Vector3f const& position)
{
element.position(position);
initialPose.block<3, 1>(0, 3) = position;
return *this;
}
- Transformable& orientation(Eigen::Matrix3f const& rotationMatrix)
+ Transformable&
+ orientation(Eigen::Matrix3f const& rotationMatrix)
{
element.orientation(rotationMatrix);
initialPose.block<3, 3>(0, 0) = rotationMatrix;
return *this;
}
- Transformable& orientation(Eigen::Quaternionf const& quaternion)
+ Transformable&
+ orientation(Eigen::Quaternionf const& quaternion)
{
element.orientation(quaternion);
initialPose.block<3, 3>(0, 0) = quaternion.toRotationMatrix();
return *this;
}
- Transformable& enable(viz::InteractionDescription const& interaction)
+ Transformable&
+ enable(viz::InteractionDescription const& interaction)
{
element.enable(interaction);
// A movable element is always hidden during the interaction
- element.data_->interaction.enableFlags |= viz::data::InteractionEnableFlags::TRANSFORM_HIDE;
+ element.data_->interaction.enableFlags |=
+ viz::data::InteractionEnableFlags::TRANSFORM_HIDE;
return *this;
}
// The pose after the current transformation has been applied
- Eigen::Matrix4f getCurrentPose() const
+ Eigen::Matrix4f
+ getCurrentPose() const
{
return transformation * initialPose;
}
// Returns true, if the element has been changed and the layer needs to be comitted again
- TransformationResult handle(viz::InteractionFeedback const& interaction)
+ TransformationResult
+ handle(viz::InteractionFeedback const& interaction)
{
TransformationResult result;
if (interaction.element() == element.data_->id)
{
switch (interaction.type())
{
- case viz::InteractionFeedbackType::Transform:
- {
- // Keep track of the transformation
- transformation = interaction.transformation();
- result.wasTransformed = true;
- } break;
- case viz::InteractionFeedbackType::Deselect:
- {
- // If an object is deselected, we apply the transformation
- initialPose = transformation * initialPose;
- transformation = Eigen::Matrix4f::Identity();
- element.pose(initialPose);
-
- // In this case, the user needs to commit the layer
- result.needsLayerUpdate = true;
- } break;
- default:
- {
- // Ignore the other events
- }
+ case viz::InteractionFeedbackType::Transform:
+ {
+ // Keep track of the transformation
+ transformation = interaction.transformation();
+ result.wasTransformed = true;
+ }
+ break;
+ case viz::InteractionFeedbackType::Deselect:
+ {
+ // If an object is deselected, we apply the transformation
+ initialPose = transformation * initialPose;
+ transformation = Eigen::Matrix4f::Identity();
+ element.pose(initialPose);
+
+ // In this case, the user needs to commit the layer
+ result.needsLayerUpdate = true;
+ }
+ break;
+ default:
+ {
+ // Ignore the other events
+ }
}
}
return result;
}
-
Eigen::Matrix4f initialPose = Eigen::Matrix4f::Identity();
Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
ElementT element;
};
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/Layer.h b/source/RobotAPI/components/ArViz/Client/Layer.h
index ef9b0fef855eb89f9151717694a1a2c33517d108..42440bfbdef4e87e8fa0f1e891034649132ea4a7 100644
--- a/source/RobotAPI/components/ArViz/Client/Layer.h
+++ b/source/RobotAPI/components/ArViz/Client/Layer.h
@@ -1,10 +1,10 @@
#pragma once
-#include "Elements.h"
+#include <ArmarXCore/interface/core/BasicVectorTypes.h>
#include <RobotAPI/interface/ArViz/Component.h>
-#include <ArmarXCore/interface/core/BasicVectorTypes.h>
+#include "Elements.h"
namespace armarx::viz
{
@@ -20,32 +20,36 @@ namespace armarx::viz
data_.action = data::Layer_CREATE_OR_UPDATE;
}
- void clear()
+ void
+ clear()
{
data_.elements.clear();
}
template <typename ElementT>
- void add(ElementT const& element)
+ void
+ add(ElementT const& element)
{
data_.elements.push_back(element.data_);
}
-// template <typename ElementT>
-// void add(std::vector<ElementT> const& elements)
-// {
-// for (ElementT const& e : elements)
-// {
-// add(e);
-// }
-// }
-
- void markForDeletion()
+ // template <typename ElementT>
+ // void add(std::vector<ElementT> const& elements)
+ // {
+ // for (ElementT const& e : elements)
+ // {
+ // add(e);
+ // }
+ // }
+
+ void
+ markForDeletion()
{
data_.action = data::LayerAction::Layer_DELETE;
}
- std::size_t size() const noexcept
+ std::size_t
+ size() const noexcept
{
return data_.elements.size();
}
@@ -53,4 +57,4 @@ namespace armarx::viz
data::LayerUpdate data_;
};
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/ScopedClient.h b/source/RobotAPI/components/ArViz/Client/ScopedClient.h
index 4b21418337268222b7c8c95a8c59b72b8cf8ef7a..c80c52cb557661e4234b82baf40fa9c982f2d8a5 100644
--- a/source/RobotAPI/components/ArViz/Client/ScopedClient.h
+++ b/source/RobotAPI/components/ArViz/Client/ScopedClient.h
@@ -39,7 +39,7 @@ namespace armarx::viz
* (as `viz::ScopedClient` will go out of scope as well).
*
*/
- class ScopedClient: virtual public Client
+ class ScopedClient : virtual public Client
{
public:
using Client::Client;
@@ -54,4 +54,4 @@ namespace armarx::viz
private:
mutable std::set<std::string> layers;
};
-} // namespace armarx::viz
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Color.h b/source/RobotAPI/components/ArViz/Client/elements/Color.h
index 272e25b44cfb6a2452d3accef676d970654fe2c0..3e1f8d2e522df468109a009926ca1b0bcbb6d06e 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Color.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Color.h
@@ -1,11 +1,10 @@
#pragma once
-#include <RobotAPI/interface/ArViz/Elements.h>
+#include <Eigen/Core>
#include <SimoxUtility/color/Color.h>
-#include <Eigen/Core>
-
+#include <RobotAPI/interface/ArViz/Elements.h>
namespace armarx::viz
{
@@ -14,7 +13,9 @@ namespace armarx::viz
{
using data::Color::Color;
- Color(const data::Color& c) : data::Color(c) {}
+ Color(const data::Color& c) : data::Color(c)
+ {
+ }
Color(simox::Color c)
{
@@ -23,6 +24,7 @@ namespace armarx::viz
this->b = c.b;
this->a = c.a;
}
+
Color(int r, int g, int b, int a = 255)
{
this->r = simox::color::to_byte(r);
@@ -30,6 +32,7 @@ namespace armarx::viz
this->b = simox::color::to_byte(b);
this->a = simox::color::to_byte(a);
}
+
Color(float r, float g, float b, float a = 1.0)
{
this->r = simox::color::to_byte(r);
@@ -39,133 +42,149 @@ namespace armarx::viz
}
/// Construct a byte color from R, G, B and optional alpha.
- static inline Color fromRGBA(int r, int g, int b, int a = 255)
+ static inline Color
+ fromRGBA(int r, int g, int b, int a = 255)
{
return {r, g, b, a};
}
/// Construct a float color from R, G, B and optional alpha.
- static inline Color fromRGBA(float r, float g, float b, float a = 1.0)
+ static inline Color
+ fromRGBA(float r, float g, float b, float a = 1.0)
{
return {r, g, b, a};
}
/// Construct from a simox Color.
- static inline Color fromRGBA(simox::Color c)
+ static inline Color
+ fromRGBA(simox::Color c)
{
return {c};
}
-
// Colorless
- static inline Color black(int a = 255)
+ static inline Color
+ black(int a = 255)
{
return simox::Color::black(a);
}
- static inline Color white(int a = 255)
+ static inline Color
+ white(int a = 255)
{
return simox::Color::white(a);
}
- static inline Color gray(int g = 128, int a = 255)
+ static inline Color
+ gray(int g = 128, int a = 255)
{
return simox::Color::gray(g, a);
}
// Primary colors
- static inline Color red(int r = 255, int a = 255)
+ static inline Color
+ red(int r = 255, int a = 255)
{
return simox::Color::red(r, a);
}
- static inline Color green(int g = 255, int a = 255)
+ static inline Color
+ green(int g = 255, int a = 255)
{
return simox::Color::green(g, a);
}
- static inline Color blue(int b = 255, int a = 255)
+ static inline Color
+ blue(int b = 255, int a = 255)
{
return simox::Color::blue(b, a);
}
-
// Secondary colors
/// Green + Blue
- static inline Color cyan(int c = 255, int a = 255)
+ static inline Color
+ cyan(int c = 255, int a = 255)
{
return simox::Color::cyan(c, a);
}
/// Red + Green
- static inline Color yellow(int y = 255, int a = 255)
+ static inline Color
+ yellow(int y = 255, int a = 255)
{
return simox::Color::yellow(y, a);
}
/// Red + Blue
- static inline Color magenta(int m = 255, int a = 255)
+ static inline Color
+ magenta(int m = 255, int a = 255)
{
return simox::Color::magenta(m, a);
}
-
// 2:1 Mixed colors
/// 2 Red + 1 Green
- static inline Color orange(int o = 255, int a = 255)
+ static inline Color
+ orange(int o = 255, int a = 255)
{
return simox::Color::orange(o, a);
}
/// 2 Red + 1 Blue
- static inline Color pink(int p = 255, int a = 255)
+ static inline Color
+ pink(int p = 255, int a = 255)
{
return simox::Color::pink(p, a);
}
/// 2 Green + 1 Red
- static inline Color lime(int l = 255, int a = 255)
+ static inline Color
+ lime(int l = 255, int a = 255)
{
return simox::Color::lime(l, a);
}
/// 2 Green + 1 Blue
- static inline Color turquoise(int t = 255, int a = 255)
+ static inline Color
+ turquoise(int t = 255, int a = 255)
{
return simox::Color::turquoise(t, a);
}
/// 2 Blue + 1 Green
- static inline Color azure(int az = 255, int a = 255)
+ static inline Color
+ azure(int az = 255, int a = 255)
{
return simox::Color::azure(az, a);
}
/// 2 Blue + 1 Red
- static inline Color purple(int p = 255, int a = 255)
+ static inline Color
+ purple(int p = 255, int a = 255)
{
return simox::Color::purple(p, a);
}
-
};
-
- inline std::ostream& operator<<(std::ostream& os, const Color& c)
+ inline std::ostream&
+ operator<<(std::ostream& os, const Color& c)
{
- return os << "(" << int(c.r) << " " << int(c.g) << " " << int(c.b)
- << " | " << int(c.a) << ")";
+ return os << "(" << int(c.r) << " " << int(c.g) << " " << int(c.b) << " | " << int(c.a)
+ << ")";
}
- inline bool operator==(const Color& lhs, const Color& rhs)
+ inline bool
+ operator==(const Color& lhs, const Color& rhs)
{
return lhs.r == rhs.r && lhs.g == rhs.g && lhs.b == rhs.b && lhs.a == rhs.a;
}
- inline bool operator!=(const Color& lhs, const Color& rhs)
+ inline bool
+ operator!=(const Color& lhs, const Color& rhs)
{
return !(lhs == rhs);
}
@@ -173,21 +192,24 @@ namespace armarx::viz
namespace data
{
// viz::Color == simox::Color
- inline bool operator==(const viz::data::Color& lhs, const simox::color::Color& rhs)
+ inline bool
+ operator==(const viz::data::Color& lhs, const simox::color::Color& rhs)
{
return lhs.r == rhs.r && lhs.g == rhs.g && lhs.b == rhs.b && lhs.a == rhs.a;
}
// simox::Color == viz::Color
- inline bool operator==(const simox::color::Color& lhs, const armarx::viz::data::Color& rhs)
+ inline bool
+ operator==(const simox::color::Color& lhs, const armarx::viz::data::Color& rhs)
{
return rhs == lhs;
}
- inline std::ostream& operator<<(std::ostream& os, const viz::data::Color& c)
+ inline std::ostream&
+ operator<<(std::ostream& os, const viz::data::Color& c)
{
- return os << "(" << int(c.r) << " " << int(c.g) << " " << int(c.b)
- << " | " << int(c.a) << ")";
+ return os << "(" << int(c.r) << " " << int(c.g) << " " << int(c.b) << " | " << int(c.a)
+ << ")";
}
- }
-}
+ } // namespace data
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
index a7c433454c587ae4a2c2c6fb142fde4ee3d7597b..8f21541b9998d85b3042e8b6f4ff2b2befbfbb44 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/ElementOps.h
@@ -1,17 +1,16 @@
#pragma once
-#include "Color.h"
+#include <string>
-#include <RobotAPI/interface/ArViz/Elements.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
#include <SimoxUtility/color/GlasbeyLUT.h>
#include <SimoxUtility/math/convert/rpy_to_quat.h>
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
-#include <string>
+#include <RobotAPI/interface/ArViz/Elements.h>
+#include "Color.h"
namespace armarx::viz
{
@@ -25,31 +24,34 @@ namespace armarx::viz
bool local = false;
};
- static const AxesFlags AXES_X = {true, false, false, false};
- static const AxesFlags AXES_Y = {false, true, false, false};
- static const AxesFlags AXES_Z = {false, false, true, false};
- static const AxesFlags AXES_XY = {true, true, false, false};
- static const AxesFlags AXES_YZ = {false, true, true, false};
- static const AxesFlags AXES_XZ = {true, false, true, false};
+ static const AxesFlags AXES_X = {true, false, false, false};
+ static const AxesFlags AXES_Y = {false, true, false, false};
+ static const AxesFlags AXES_Z = {false, false, true, false};
+ static const AxesFlags AXES_XY = {true, true, false, false};
+ static const AxesFlags AXES_YZ = {false, true, true, false};
+ static const AxesFlags AXES_XZ = {true, false, true, false};
static const AxesFlags AXES_XYZ = {true, true, true, false};
struct InteractionDescription
{
using Self = InteractionDescription;
- Self& none()
+ Self&
+ none()
{
data_.enableFlags = 0;
return *this;
}
- Self& selection()
+ Self&
+ selection()
{
data_.enableFlags |= data::InteractionEnableFlags::SELECT;
return *this;
}
- Self& contextMenu(std::vector<std::string> const& options)
+ Self&
+ contextMenu(std::vector<std::string> const& options)
{
data_.enableFlags |= data::InteractionEnableFlags::CONTEXT_MENU;
data_.contextMenuOptions = options;
@@ -57,7 +59,8 @@ namespace armarx::viz
return selection();
}
- Self& translation(AxesFlags const& axes = AXES_XYZ)
+ Self&
+ translation(AxesFlags const& axes = AXES_XYZ)
{
data_.enableFlags |= (axes.x ? data::InteractionEnableFlags::TRANSLATION_X : 0);
data_.enableFlags |= (axes.y ? data::InteractionEnableFlags::TRANSLATION_Y : 0);
@@ -66,7 +69,8 @@ namespace armarx::viz
return selection();
}
- Self& rotation(AxesFlags const& axes = AXES_XYZ)
+ Self&
+ rotation(AxesFlags const& axes = AXES_XYZ)
{
data_.enableFlags |= (axes.x ? data::InteractionEnableFlags::ROTATION_X : 0);
data_.enableFlags |= (axes.y ? data::InteractionEnableFlags::ROTATION_Y : 0);
@@ -75,7 +79,8 @@ namespace armarx::viz
return selection();
}
- Self& scaling(AxesFlags const& axes = AXES_XYZ)
+ Self&
+ scaling(AxesFlags const& axes = AXES_XYZ)
{
data_.enableFlags |= (axes.x ? data::InteractionEnableFlags::SCALING_X : 0);
data_.enableFlags |= (axes.y ? data::InteractionEnableFlags::SCALING_Y : 0);
@@ -84,12 +89,14 @@ namespace armarx::viz
return selection();
}
- Self& transform()
+ Self&
+ transform()
{
return translation().rotation();
}
- Self& hideDuringTransform()
+ Self&
+ hideDuringTransform()
{
data_.enableFlags |= data::InteractionEnableFlags::TRANSFORM_HIDE;
return *this;
@@ -98,7 +105,8 @@ namespace armarx::viz
data::InteractionDescription data_;
};
- inline InteractionDescription interaction()
+ inline InteractionDescription
+ interaction()
{
return InteractionDescription();
}
@@ -108,8 +116,7 @@ namespace armarx::viz
class ElementOps
{
public:
- ElementOps(std::string const& id)
- : data_(new ElementT)
+ ElementOps(std::string const& id) : data_(new ElementT)
{
data_->id = id;
data_->scale.e0 = 1.0f;
@@ -117,14 +124,16 @@ namespace armarx::viz
data_->scale.e2 = 1.0f;
}
- DerivedT& id(const std::string& id)
+ DerivedT&
+ id(const std::string& id)
{
data_->id = id;
return *static_cast<DerivedT*>(this);
}
- DerivedT& position(float x, float y, float z)
+ DerivedT&
+ position(float x, float y, float z)
{
auto& pose = data_->pose;
pose.x = x;
@@ -132,12 +141,15 @@ namespace armarx::viz
pose.z = z;
return *static_cast<DerivedT*>(this);
}
- DerivedT& position(Eigen::Vector3f const& pos)
+
+ DerivedT&
+ position(Eigen::Vector3f const& pos)
{
return position(pos.x(), pos.y(), pos.z());
}
- DerivedT& orientation(Eigen::Quaternionf const& ori)
+ DerivedT&
+ orientation(Eigen::Quaternionf const& ori)
{
auto& pose = data_->pose;
pose.qw = ori.w();
@@ -147,36 +159,45 @@ namespace armarx::viz
return *static_cast<DerivedT*>(this);
}
- DerivedT& orientation(Eigen::Matrix3f const& ori)
+
+ DerivedT&
+ orientation(Eigen::Matrix3f const& ori)
{
return orientation(Eigen::Quaternionf(ori));
}
- DerivedT& orientation(float r, float p, float y)
+
+ DerivedT&
+ orientation(float r, float p, float y)
{
return orientation(simox::math::rpy_to_quat(r, p, y));
}
- DerivedT& pose(Eigen::Matrix4f const& pose)
+ DerivedT&
+ pose(Eigen::Matrix4f const& pose)
{
return position(pose.block<3, 1>(0, 3)).orientation(pose.block<3, 3>(0, 0));
}
- DerivedT& pose(Eigen::Vector3f const& position, Eigen::Quaternionf const& orientation)
+ DerivedT&
+ pose(Eigen::Vector3f const& position, Eigen::Quaternionf const& orientation)
{
return this->position(position).orientation(orientation);
}
- DerivedT& pose(Eigen::Vector3f const& position, Eigen::Matrix3f const& orientation)
+ DerivedT&
+ pose(Eigen::Vector3f const& position, Eigen::Matrix3f const& orientation)
{
return this->position(position).orientation(orientation);
}
- DerivedT& pose(const Eigen::Affine3f& pose)
+ DerivedT&
+ pose(const Eigen::Affine3f& pose)
{
return this->position(pose.translation()).orientation(pose.linear());
}
- Eigen::Matrix4f pose() const
+ Eigen::Matrix4f
+ pose() const
{
auto& p = data_->pose;
Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
@@ -187,30 +208,35 @@ namespace armarx::viz
return m;
}
- DerivedT& transformPose(Eigen::Matrix4f const& p)
+ DerivedT&
+ transformPose(Eigen::Matrix4f const& p)
{
return pose(p * pose());
}
- DerivedT& color(Color color)
+ DerivedT&
+ color(Color color)
{
data_->color = color;
return *static_cast<DerivedT*>(this);
}
- template<class...Ts>
- DerivedT& color(Ts&& ...ts)
+ template <class... Ts>
+ DerivedT&
+ color(Ts&&... ts)
{
return color({std::forward<Ts>(ts)...});
}
- DerivedT& colorGlasbeyLUT(std::size_t id, int alpha = 255)
+ DerivedT&
+ colorGlasbeyLUT(std::size_t id, int alpha = 255)
{
return color(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
}
- DerivedT& overrideMaterial(bool value)
+ DerivedT&
+ overrideMaterial(bool value)
{
if (value)
{
@@ -224,7 +250,8 @@ namespace armarx::viz
return *static_cast<DerivedT*>(this);
}
- DerivedT& scale(Eigen::Vector3f scale)
+ DerivedT&
+ scale(Eigen::Vector3f scale)
{
data_->scale.e0 = scale.x();
data_->scale.e1 = scale.y();
@@ -232,7 +259,9 @@ namespace armarx::viz
return *static_cast<DerivedT*>(this);
}
- DerivedT& scale(float x, float y, float z)
+
+ DerivedT&
+ scale(float x, float y, float z)
{
data_->scale.e0 = x;
data_->scale.e1 = y;
@@ -240,26 +269,31 @@ namespace armarx::viz
return *static_cast<DerivedT*>(this);
}
- DerivedT& scale(float s)
+
+ DerivedT&
+ scale(float s)
{
return scale(s, s, s);
}
- DerivedT& hide()
+ DerivedT&
+ hide()
{
data_->flags |= data::ElementFlags::HIDDEN;
return *static_cast<DerivedT*>(this);
}
- DerivedT& show()
+ DerivedT&
+ show()
{
data_->flags &= ~data::ElementFlags::HIDDEN;
return *static_cast<DerivedT*>(this);
}
- DerivedT& visible(bool visible)
+ DerivedT&
+ visible(bool visible)
{
if (visible)
{
@@ -271,14 +305,14 @@ namespace armarx::viz
}
}
- DerivedT& enable(InteractionDescription const& interactionDescription)
+ DerivedT&
+ enable(InteractionDescription const& interactionDescription)
{
data_->interaction = interactionDescription.data_;
return *static_cast<DerivedT*>(this);
}
-
IceInternal::Handle<ElementT> data_;
};
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Line.cpp b/source/RobotAPI/components/ArViz/Client/elements/Line.cpp
index 35287f4d55adc284fab6bf74b02f75ecb5307880..45c45d0f6ed1426ed5fe0a473e2b77ddafd8db97 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Line.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/Line.cpp
@@ -4,16 +4,19 @@
namespace armarx::viz
{
- Line& Line::lineWidth(float w)
+ Line&
+ Line::lineWidth(float w)
{
data_->lineWidth = w;
return *this;
}
- Line& Line::fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
+
+ Line&
+ Line::fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
{
data_->from = ToBasicVectorType(from);
- data_->to = ToBasicVectorType(to);
+ data_->to = ToBasicVectorType(to);
return *this;
}
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp b/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp
index c66cc79b05827c82ad1d4a8f055c52bc446b972e..16e3dc396710c3e35db0161146a43922a0eb4cb1 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/Mesh.cpp
@@ -2,26 +2,30 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::viz
{
- Mesh& Mesh::grid2D(Eigen::Vector2f extents, Eigen::Vector2i numPoints,
- std::function<viz::Color(size_t, size_t, const Eigen::Vector3f&)> colorFunc)
+ Mesh&
+ Mesh::grid2D(Eigen::Vector2f extents,
+ Eigen::Vector2i numPoints,
+ std::function<viz::Color(size_t, size_t, const Eigen::Vector3f&)> colorFunc)
{
// Create vertices.
- std::vector<std::vector<Eigen::Vector3f>> vertices = grid::makeGrid2DVertices(extents, numPoints);
+ std::vector<std::vector<Eigen::Vector3f>> vertices =
+ grid::makeGrid2DVertices(extents, numPoints);
// Create colors.
- std::vector<std::vector<viz::data::Color>> colors = grid::makeGrid2DColors(vertices, colorFunc);
+ std::vector<std::vector<viz::data::Color>> colors =
+ grid::makeGrid2DColors(vertices, colorFunc);
return this->grid2D(vertices, colors);
}
-
- Mesh& Mesh::grid2D(const std::vector<std::vector<Eigen::Vector3f> >& vertices,
- const std::vector<std::vector<viz::data::Color>>& colors)
+ Mesh&
+ Mesh::grid2D(const std::vector<std::vector<Eigen::Vector3f>>& vertices,
+ const std::vector<std::vector<viz::data::Color>>& colors)
{
- ARMARX_CHECK_EQUAL(vertices.size(), colors.size()) << "Numbers of vertices and colors must match.";
+ ARMARX_CHECK_EQUAL(vertices.size(), colors.size())
+ << "Numbers of vertices and colors must match.";
if (vertices.empty())
{
@@ -31,17 +35,19 @@ namespace armarx::viz
const size_t num_x = vertices.size();
const size_t num_y = vertices.front().size();
- bool check = false; // This could unnecessarily slow down building large meshes.
+ bool check = false; // This could unnecessarily slow down building large meshes.
if (check)
{
// Check consistent sizes.
for (const auto& vv : vertices)
{
- ARMARX_CHECK_EQUAL(vv.size(), num_y) << "All nested vectors must have equal length.";
+ ARMARX_CHECK_EQUAL(vv.size(), num_y)
+ << "All nested vectors must have equal length.";
}
for (const auto& cv : colors)
{
- ARMARX_CHECK_EQUAL(cv.size(), num_y) << "All nested vectors must have equal length.";
+ ARMARX_CHECK_EQUAL(cv.size(), num_y)
+ << "All nested vectors must have equal length.";
}
}
@@ -52,22 +58,23 @@ namespace armarx::viz
return this->vertices(grid::flatten(vertices)).colors(grid::flatten(colors)).faces(faces);
}
-
- std::vector<std::vector<Eigen::Vector3f>> grid::makeGrid2DVertices(
- Eigen::Vector2f extents, Eigen::Vector2i numPoints, float height)
+ std::vector<std::vector<Eigen::Vector3f>>
+ grid::makeGrid2DVertices(Eigen::Vector2f extents, Eigen::Vector2i numPoints, float height)
{
- const Eigen::Vector2f minimum = - extents / 2;
+ const Eigen::Vector2f minimum = -extents / 2;
// extents = (num - 1) * step => step = extents / (num - 1)
- const Eigen::Vector2f step = (extents.array() / (numPoints.array() - 1).cast<float>()).matrix();
+ const Eigen::Vector2f step =
+ (extents.array() / (numPoints.array() - 1).cast<float>()).matrix();
- ARMARX_CHECK_POSITIVE(numPoints.minCoeff()) << "Number of points must be positive. " << VAROUT(numPoints);
+ ARMARX_CHECK_POSITIVE(numPoints.minCoeff())
+ << "Number of points must be positive. " << VAROUT(numPoints);
const size_t num_x = size_t(numPoints.x());
const size_t num_y = size_t(numPoints.y());
// Create vertices.
std::vector<std::vector<Eigen::Vector3f>> gridVertices(
- num_x, std::vector<Eigen::Vector3f>(num_y, Eigen::Vector3f::Zero()));
+ num_x, std::vector<Eigen::Vector3f>(num_y, Eigen::Vector3f::Zero()));
for (size_t i = 0; i < num_x; i++)
{
@@ -82,16 +89,16 @@ namespace armarx::viz
return gridVertices;
}
-
- std::vector<std::vector<viz::data::Color> > grid::makeGrid2DColors(
- const std::vector<std::vector<Eigen::Vector3f> >& vertices,
+ std::vector<std::vector<viz::data::Color>>
+ grid::makeGrid2DColors(
+ const std::vector<std::vector<Eigen::Vector3f>>& vertices,
std::function<viz::Color(size_t x, size_t y, const Eigen::Vector3f& p)> colorFunc)
{
size_t num_x = vertices.size();
size_t num_y = vertices.front().size();
std::vector<std::vector<viz::data::Color>> colors(
- num_x, std::vector<viz::data::Color>(num_y, viz::Color::black()));
+ num_x, std::vector<viz::data::Color>(num_y, viz::Color::black()));
for (size_t i = 0; i < num_x; i++)
{
@@ -104,8 +111,8 @@ namespace armarx::viz
return colors;
}
-
- std::vector<viz::data::Face> grid::makeGrid2DFaces(size_t num_x, size_t num_y)
+ std::vector<viz::data::Face>
+ grid::makeGrid2DFaces(size_t num_x, size_t num_y)
{
std::vector<viz::data::Face> faces(2 * (num_x - 1) * (num_y - 1));
@@ -136,6 +143,4 @@ namespace armarx::viz
}
-
-
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
index b2530de115a276e2080ac05ab98c587a3cab4855..52b62997477e81d7fa6ae43cd24cf7282e6a5cd4 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
@@ -1,26 +1,26 @@
#pragma once
-#include "ElementOps.h"
-
-#include <RobotAPI/interface/ArViz/Elements.h>
+#include <functional>
+#include <numeric> // for std::accumulate
+#include <vector>
#include <Eigen/Core>
-#include <functional>
-#include <numeric> // for std::accumulate
-#include <vector>
+#include <RobotAPI/interface/ArViz/Elements.h>
+#include "ElementOps.h"
namespace CGAL
{
- template < class PolyhedronTraits_3,
- class PolyhedronItems_3,
- template < class T, class I, class A>
- class T_HDS,
- class Alloc>
+ template <class PolyhedronTraits_3,
+ class PolyhedronItems_3,
+ template <class T, class I, class A>
+ class T_HDS,
+ class Alloc>
class Polyhedron_3;
- template<class> class Surface_mesh;
-}
+ template <class>
+ class Surface_mesh;
+} // namespace CGAL
namespace armarx::viz
{
@@ -30,7 +30,8 @@ namespace armarx::viz
public:
using ElementOps::ElementOps;
- Mesh& vertices(const Eigen::Vector3f* vs, std::size_t size)
+ Mesh&
+ vertices(const Eigen::Vector3f* vs, std::size_t size)
{
auto& vertices = data_->vertices;
vertices.clear();
@@ -43,66 +44,75 @@ namespace armarx::viz
return *this;
}
- Mesh& vertices(const std::vector<Eigen::Vector3f>& vs)
+
+ Mesh&
+ vertices(const std::vector<Eigen::Vector3f>& vs)
{
return this->vertices(vs.data(), vs.size());
}
- Mesh& vertices(const armarx::Vector3f* vs, std::size_t size)
+ Mesh&
+ vertices(const armarx::Vector3f* vs, std::size_t size)
{
data_->vertices.assign(vs, vs + size);
return *this;
}
- Mesh& vertices(const std::vector<armarx::Vector3f>& vs)
+
+ Mesh&
+ vertices(const std::vector<armarx::Vector3f>& vs)
{
return this->vertices(vs.data(), vs.size());
}
- Mesh& colors(const data::Color* cs, std::size_t size)
+ Mesh&
+ colors(const data::Color* cs, std::size_t size)
{
data_->colors.assign(cs, cs + size);
return *this;
}
- Mesh& colors(const std::vector<data::Color>& cs)
+
+ Mesh&
+ colors(const std::vector<data::Color>& cs)
{
return this->colors(cs.data(), cs.size());
}
- Mesh& faces(const data::Face* fs, std::size_t size)
+ Mesh&
+ faces(const data::Face* fs, std::size_t size)
{
data_->faces.assign(fs, fs + size);
return *this;
}
- Mesh& faces(const std::vector<data::Face>& fs)
+
+ Mesh&
+ faces(const std::vector<data::Face>& fs)
{
return this->faces(fs.data(), fs.size());
}
- template<class T>
+ template <class T>
Mesh& mesh(const CGAL::Surface_mesh<T>& sm);
- template < class PolyhedronTraits_3,
- class PolyhedronItems_3,
- template < class T, class I, class A>
- class T_HDS,
- class Alloc>
- Mesh& mesh(const CGAL::Polyhedron_3 <
- PolyhedronTraits_3,
- PolyhedronItems_3,
- T_HDS,
- Alloc
- > & p3);
+ template <class PolyhedronTraits_3,
+ class PolyhedronItems_3,
+ template <class T, class I, class A>
+ class T_HDS,
+ class Alloc>
+ Mesh&
+ mesh(const CGAL::Polyhedron_3<PolyhedronTraits_3, PolyhedronItems_3, T_HDS, Alloc>& p3);
/**
* @brief Builds a regular 2D grid in the xy-plane.
* @param extents The full extents in x and y direction.
* @param numPoints The number of points in x and y direction.
* @param colorFunc A function determining the color of each vertex.
*/
- Mesh& grid2D(Eigen::Vector2f extents, Eigen::Vector2i numPoints,
- std::function<viz::Color(size_t i, size_t j, const Eigen::Vector3f& p)> colorFunc);
+ Mesh&
+ grid2D(Eigen::Vector2f extents,
+ Eigen::Vector2i numPoints,
+ std::function<viz::Color(size_t i, size_t j, const Eigen::Vector3f& p)> colorFunc);
/**
* @brief Builds a regular 2D grid.
@@ -118,8 +128,6 @@ namespace armarx::viz
const std::vector<std::vector<viz::data::Color>>& colors);
};
-
-
namespace grid
{
/**
@@ -133,8 +141,8 @@ namespace armarx::viz
* @param height The height (z-value).
* @return The vertices.
*/
- std::vector<std::vector<Eigen::Vector3f>> makeGrid2DVertices(
- Eigen::Vector2f extents, Eigen::Vector2i numPoints, float height = 0);
+ std::vector<std::vector<Eigen::Vector3f>>
+ makeGrid2DVertices(Eigen::Vector2f extents, Eigen::Vector2i numPoints, float height = 0);
/**
@@ -146,8 +154,8 @@ namespace armarx::viz
* @see `makeGrid2DVertices()`
*/
std::vector<std::vector<viz::data::Color>> makeGrid2DColors(
- const std::vector<std::vector<Eigen::Vector3f>>& vertices,
- std::function<viz::Color(size_t x, size_t y, const Eigen::Vector3f& p)> colorFunc);
+ const std::vector<std::vector<Eigen::Vector3f>>& vertices,
+ std::function<viz::Color(size_t x, size_t y, const Eigen::Vector3f& p)> colorFunc);
/**
@@ -162,15 +170,15 @@ namespace armarx::viz
*/
std::vector<viz::data::Face> makeGrid2DFaces(size_t num_x, size_t num_y);
-
template <class T>
/// @brief Flattens a 2D vector of nested vectors to a 1D vector.
- std::vector<T> flatten(const std::vector<std::vector<T>>& vector)
+ std::vector<T>
+ flatten(const std::vector<std::vector<T>>& vector)
{
- size_t size = std::accumulate(vector.begin(), vector.end(), size_t(0), [](size_t s, const auto & v)
- {
- return s + v.size();
- });
+ size_t size = std::accumulate(vector.begin(),
+ vector.end(),
+ size_t(0),
+ [](size_t s, const auto& v) { return s + v.size(); });
std::vector<T> flat;
flat.reserve(size);
@@ -183,6 +191,6 @@ namespace armarx::viz
}
return flat;
}
- }
+ } // namespace grid
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h b/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h
index b4a45c3a361cbd0cc9bb6a9889f102828fdfe18a..2541f7b3d5c25e923e840dbc04c1b66077af4032 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h
@@ -11,8 +11,9 @@
namespace armarx::viz
{
- template<class T>
- inline Mesh& Mesh::mesh(const CGAL::Surface_mesh<T>& sm)
+ template <class T>
+ inline Mesh&
+ Mesh::mesh(const CGAL::Surface_mesh<T>& sm)
{
using sm_t = CGAL::Surface_mesh<T>;
using vidx_t = typename sm_t::Vertex_index;
@@ -63,17 +64,13 @@ namespace armarx::viz
return *this;
}
- template < class PolyhedronTraits_3,
- class PolyhedronItems_3,
- template < class T, class I, class A>
- class T_HDS,
- class Alloc>
- inline Mesh& Mesh::mesh(const CGAL::Polyhedron_3 <
- PolyhedronTraits_3,
- PolyhedronItems_3,
- T_HDS,
- Alloc
- > & p3)
+ template <class PolyhedronTraits_3,
+ class PolyhedronItems_3,
+ template <class T, class I, class A>
+ class T_HDS,
+ class Alloc>
+ inline Mesh&
+ Mesh::mesh(const CGAL::Polyhedron_3<PolyhedronTraits_3, PolyhedronItems_3, T_HDS, Alloc>& p3)
{
auto& vertices = data_->vertices;
auto& faces = data_->faces;
@@ -94,8 +91,7 @@ namespace armarx::viz
for (const auto& fidx : CGAL::Iterator_range{p3.facets_begin(), p3.facets_end()})
{
auto circ = fidx.facet_begin();
- ARMARX_CHECK_EQUAL(3, CGAL::circulator_size(circ))
- << "One face is no triangle!";
+ ARMARX_CHECK_EQUAL(3, CGAL::circulator_size(circ)) << "One face is no triangle!";
data::Face f;
f.v0 = std::distance(vbeg, circ->vertex());
++circ;
@@ -104,9 +100,9 @@ namespace armarx::viz
f.v2 = std::distance(vbeg, circ->vertex());
++circ;
ARMARX_CHECK_EXPRESSION(circ == fidx.facet_begin())
- << "Internal error while circulating a facet";
+ << "Internal error while circulating a facet";
faces.emplace_back(f);
}
return *this;
}
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.cpp b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
index cf5e9899e943dd4076c2dd21ca9fe982b3c1a9bc..eb4b6fc0ff4eb64e1ccda3f0c513f9c1a29beeb3 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.cpp
@@ -8,21 +8,24 @@
namespace armarx::viz
{
- Path& Path::clear()
+ Path&
+ Path::clear()
{
data_->points.clear();
return *this;
}
- Path& Path::width(float w)
+ Path&
+ Path::width(float w)
{
data_->lineWidth = w;
return *this;
}
- Path& Path::points(std::vector<Eigen::Vector3f> const& ps)
+ Path&
+ Path::points(std::vector<Eigen::Vector3f> const& ps)
{
auto& points = data_->points;
points.clear();
@@ -31,15 +34,13 @@ namespace armarx::viz
std::transform(ps.begin(),
ps.end(),
std::back_inserter(points),
- [](const auto & e)
- {
- return ToBasicVectorType(e);
- });
+ [](const auto& e) { return ToBasicVectorType(e); });
return *this;
}
- Path& Path::addPoint(Eigen::Vector3f p)
+ Path&
+ Path::addPoint(Eigen::Vector3f p)
{
data_->points.emplace_back(ToBasicVectorType(p));
diff --git a/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h b/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
index 78591128d2daddab633ef3fd04139a4c46ba2340..5447418c87fb7381940593a600db54174cdaff95 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/PointCloud.h
@@ -11,20 +11,16 @@
#include "ElementOps.h"
#include "point_cloud_type_traits.hpp"
-
namespace armarx::viz
{
using data::ColoredPoint;
-
class PointCloud : public ElementOps<PointCloud, data::ElementPointCloud>
{
public:
-
using ElementOps::ElementOps;
-
// Settings
/**
@@ -38,37 +34,39 @@ namespace armarx::viz
*
* @see isfinite()
*/
- PointCloud& checkFinite(bool enabled = true)
+ PointCloud&
+ checkFinite(bool enabled = true)
{
this->_checkFinite = enabled;
return *this;
}
-
- PointCloud& transparency(float t)
+ PointCloud&
+ transparency(float t)
{
data_->transparency = t;
return *this;
}
- PointCloud& pointSizeInPixels(float s)
+ PointCloud&
+ pointSizeInPixels(float s)
{
data_->pointSizeInPixels = s;
return *this;
}
-
// Adding points
- PointCloud& clear()
+ PointCloud&
+ clear()
{
data_->points.clear();
return *this;
}
-
- PointCloud& points(std::vector<ColoredPoint> const& ps)
+ PointCloud&
+ points(std::vector<ColoredPoint> const& ps)
{
std::size_t memorySize = ps.size() * sizeof(ps[0]);
const Ice::Byte* const begin = reinterpret_cast<const Ice::Byte*>(ps.data());
@@ -77,10 +75,10 @@ namespace armarx::viz
return *this;
}
-
// Adding single points
- PointCloud& addPoint(ColoredPoint const& p)
+ PointCloud&
+ addPoint(ColoredPoint const& p)
{
if (isfinite(p))
{
@@ -90,7 +88,8 @@ namespace armarx::viz
return *this;
}
- PointCloud& addPointUnchecked(ColoredPoint const& p)
+ PointCloud&
+ addPointUnchecked(ColoredPoint const& p)
{
const Ice::Byte* const begin = reinterpret_cast<const Ice::Byte*>(&p);
const Ice::Byte* const end = begin + sizeof(p);
@@ -98,7 +97,8 @@ namespace armarx::viz
return *this;
}
- PointCloud& addPoint(float x, float y, float z, const data::Color& color)
+ PointCloud&
+ addPoint(float x, float y, float z, const data::Color& color)
{
ColoredPoint p;
p.x = x;
@@ -108,35 +108,45 @@ namespace armarx::viz
return addPoint(p);
}
- PointCloud& addPoint(float x, float y, float z, const simox::Color& color)
+ PointCloud&
+ addPoint(float x, float y, float z, const simox::Color& color)
{
return addPoint(x, y, z, Color{color});
}
template <typename ColorCoeff = int>
- PointCloud & addPoint(float x, float y, float z, ColorCoeff r, ColorCoeff g, ColorCoeff b, ColorCoeff a = 255)
+ PointCloud&
+ addPoint(float x,
+ float y,
+ float z,
+ ColorCoeff r,
+ ColorCoeff g,
+ ColorCoeff b,
+ ColorCoeff a = 255)
{
return addPoint(x, y, z, simox::Color(r, g, b, a));
}
- PointCloud& addPoint(float x, float y, float z)
+ PointCloud&
+ addPoint(float x, float y, float z)
{
return addPoint(x, y, z, simox::Color::black(255));
}
- PointCloud& addPoint(float x, float y, float z, std::size_t id, int alpha = 255)
+ PointCloud&
+ addPoint(float x, float y, float z, std::size_t id, int alpha = 255)
{
return addPoint(x, y, z, simox::color::GlasbeyLUT::at(id, alpha));
}
-
// Templated setters for usage with PCL point types (`pcl::Point*`).
/**
* @brief Add a point in the given color.
*/
- template < class PointT >
- PointCloud& addPoint(const PointT& p, Color color)
+ template <class PointT>
+ PointCloud&
+ addPoint(const PointT& p, Color color)
{
return addPoint(p.x, p.y, p.z, color);
}
@@ -150,14 +160,15 @@ namespace armarx::viz
*
* @param p Point with members `x, y, z`, optionally `label`, optionally `r, g, b, a`.
*/
- template < class PointT>
- PointCloud& addPoint(const PointT& p)
+ template <class PointT>
+ PointCloud&
+ addPoint(const PointT& p)
{
- if constexpr(detail::has_member_label<PointT>::value)
+ if constexpr (detail::has_member_label<PointT>::value)
{
return addPoint(p.x, p.y, p.z, simox::color::GlasbeyLUT::at(p.label));
}
- else if constexpr(detail::has_members_rgba<PointT>::value)
+ else if constexpr (detail::has_members_rgba<PointT>::value)
{
return addPoint(p.x, p.y, p.z, simox::Color(p.r, p.g, p.b, p.a));
}
@@ -173,8 +184,9 @@ namespace armarx::viz
* @param p Point with members `x, y, z, r, g, b, a, label`.
* @param colorByLabel Whether to color the point by label (true) or by its RGBA (false).
*/
- template < class PointT >
- PointCloud& addPoint(const PointT& p, bool colorByLabel)
+ template <class PointT>
+ PointCloud&
+ addPoint(const PointT& p, bool colorByLabel)
{
if (colorByLabel)
{
@@ -186,74 +198,63 @@ namespace armarx::viz
}
}
-
// Setters for point clouds (for usage with `pcl::Point*`).
/// Draw a point cloud.
template <class PointCloudT>
- PointCloud& pointCloud(const PointCloudT& cloud)
+ PointCloud&
+ pointCloud(const PointCloudT& cloud)
{
- return this->setPointCloud(cloud, [this](const auto & p)
- {
- this->addPoint(p);
- });
+ return this->setPointCloud(cloud, [this](const auto& p) { this->addPoint(p); });
}
/// Draw a point cloud with given indices.
template <class PointCloudT>
- PointCloud& pointCloud(const PointCloudT& cloud, const std::vector<int>& indices)
+ PointCloud&
+ pointCloud(const PointCloudT& cloud, const std::vector<int>& indices)
{
- return this->setPointCloud(cloud, indices, [this](int, const auto & p)
- {
- addPoint(p);
- });
+ return this->setPointCloud(cloud, indices, [this](int, const auto& p) { addPoint(p); });
}
/// Draw a unicolored point cloud with given color.
template <class PointCloudT>
- PointCloud& pointCloud(const PointCloudT& cloud, Color color)
+ PointCloud&
+ pointCloud(const PointCloudT& cloud, Color color)
{
- return this->setPointCloud(cloud, [this, color](const auto & p)
- {
- this->addPoint(p, color);
- });
+ return this->setPointCloud(cloud,
+ [this, color](const auto& p) { this->addPoint(p, color); });
}
-
/// Draw a point cloud.
template <class PointCloudT>
- PointCloud& pointCloud(const PointCloudT& cloud, bool colorByLabel)
+ PointCloud&
+ pointCloud(const PointCloudT& cloud, bool colorByLabel)
{
- return this->setPointCloud(cloud, [this, colorByLabel](const auto & p)
- {
- this->addPoint(p, colorByLabel);
- });
+ return this->setPointCloud(
+ cloud, [this, colorByLabel](const auto& p) { this->addPoint(p, colorByLabel); });
}
/// Draw a point cloud with given indices.
template <class PointCloudT>
- PointCloud& pointCloud(const PointCloudT& cloud, const std::vector<int>& indices, bool colorByLabel)
+ PointCloud&
+ pointCloud(const PointCloudT& cloud, const std::vector<int>& indices, bool colorByLabel)
{
- return this->setPointCloud(cloud, indices, [this, colorByLabel](int, const auto & p)
- {
- addPoint(p, colorByLabel);
- });
+ return this->setPointCloud(cloud,
+ indices,
+ [this, colorByLabel](int, const auto& p)
+ { addPoint(p, colorByLabel); });
}
-
/// Draw a unicolored point cloud with given color and indices.
template <class PointCloudT>
- PointCloud& pointCloud(const PointCloudT& cloud, const std::vector<int>& indices,
- Color color)
+ PointCloud&
+ pointCloud(const PointCloudT& cloud, const std::vector<int>& indices, Color color)
{
- return this->setPointCloud(cloud, indices, [this, color](int, const auto & p)
- {
- addPoint(p, color);
- });
+ return this->setPointCloud(
+ cloud, indices, [this, color](int, const auto& p) { addPoint(p, color); });
return *this;
}
-
/**
* @brief Draw a colored point cloud with custom colors.
*
@@ -262,28 +263,28 @@ namespace armarx::viz
* color as `viz::Color` or `simox::Color`.
*/
template <class PointCloudT, class ColorFuncT>
- PointCloud& pointCloud(const PointCloudT& cloud, const ColorFuncT& colorFunc)
+ PointCloud&
+ pointCloud(const PointCloudT& cloud, const ColorFuncT& colorFunc)
{
- return this->setPointCloud(cloud, [this, &colorFunc](const auto & p)
- {
- addPoint(p, colorFunc(p));
- });
+ return this->setPointCloud(
+ cloud, [this, &colorFunc](const auto& p) { addPoint(p, colorFunc(p)); });
}
/**
* @brief Draw a colored point cloud with custom colors and given indices.
*/
template <class PointCloudT, class ColorFuncT>
- PointCloud& pointCloud(const PointCloudT& cloud, const std::vector<int>& indices,
- const ColorFuncT& colorFunc)
+ PointCloud&
+ pointCloud(const PointCloudT& cloud,
+ const std::vector<int>& indices,
+ const ColorFuncT& colorFunc)
{
- return this->setPointCloud(cloud, indices, [this, &colorFunc](int, const auto & p)
- {
- addPoint(p, colorFunc(p));
- });
+ return this->setPointCloud(cloud,
+ indices,
+ [this, &colorFunc](int, const auto& p)
+ { addPoint(p, colorFunc(p)); });
}
-
/**
* @brief Draw a colored point cloud with using a color map.
*
@@ -292,29 +293,32 @@ namespace armarx::viz
* a scalar value which is passed to `colorMap` to retrieve the point's color.
*/
template <class PointCloudT, class ScalarFuncT>
- PointCloud& pointCloud(const PointCloudT& pointCloud, const simox::ColorMap& colorMap,
- const ScalarFuncT& scalarFunc)
+ PointCloud&
+ pointCloud(const PointCloudT& pointCloud,
+ const simox::ColorMap& colorMap,
+ const ScalarFuncT& scalarFunc)
{
- return this->pointCloud(pointCloud, [&colorMap, scalarFunc](const auto & p)
- {
- return colorMap(scalarFunc(p));
- });
+ return this->pointCloud(pointCloud,
+ [&colorMap, scalarFunc](const auto& p)
+ { return colorMap(scalarFunc(p)); });
}
/**
* @brief Draw a colored point cloud with using a color map and given indices.
*/
template <class PointCloudT, class ScalarFuncT>
- PointCloud& pointCloud(const PointCloudT& pointCloud, const std::vector<int>& indices,
- const simox::ColorMap& colorMap, const ScalarFuncT& scalarFunc)
+ PointCloud&
+ pointCloud(const PointCloudT& pointCloud,
+ const std::vector<int>& indices,
+ const simox::ColorMap& colorMap,
+ const ScalarFuncT& scalarFunc)
{
- return this->pointCloud(pointCloud, indices, [colorMap, scalarFunc](const auto & p)
- {
- return colorMap(scalarFunc(p));
- });
+ return this->pointCloud(pointCloud,
+ indices,
+ [colorMap, scalarFunc](const auto& p)
+ { return colorMap(scalarFunc(p)); });
}
-
// Setters taking processing functions and handling iteration.
/**
@@ -330,8 +334,8 @@ namespace armarx::viz
* @return `*this`
*/
template <class PointCloudT, class PointFunc>
- PointCloud& setPointCloud(const PointCloudT& cloud, const PointFunc& pointFunc,
- bool clear = true)
+ PointCloud&
+ setPointCloud(const PointCloudT& cloud, const PointFunc& pointFunc, bool clear = true)
{
if (clear)
{
@@ -359,8 +363,11 @@ namespace armarx::viz
* @return `*this`
*/
template <class PointCloudT, class PointFunc>
- PointCloud& setPointCloud(const PointCloudT& cloud, const std::vector<int>& indices,
- const PointFunc& pointFunc, bool clear = true)
+ PointCloud&
+ setPointCloud(const PointCloudT& cloud,
+ const std::vector<int>& indices,
+ const PointFunc& pointFunc,
+ bool clear = true)
{
if (clear)
{
@@ -377,17 +384,16 @@ namespace armarx::viz
private:
-
template <class PointT>
- bool isfinite(const PointT& p) const
+ bool
+ isfinite(const PointT& p) const
{
- return !_checkFinite || (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z));
+ return !_checkFinite ||
+ (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z));
}
-
/// Whether to check whether points are finite when adding them.
bool _checkFinite = false;
-
};
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp b/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp
index b77725be1ca0ee3dafdc24a5e1513275aebe796b..89cb5c67fd5e93cac392f05a141d60db9fa4a041 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/Robot.cpp
@@ -1,6 +1,5 @@
#include "Robot.h"
-
namespace armarx::viz
{
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Robot.h b/source/RobotAPI/components/ArViz/Client/elements/Robot.h
index 53c2976cafd2e235290dea6b1ea969408725471d..5f8e0e4a688cb1041ad8ceb572fd81138cff397a 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Robot.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Robot.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/PackagePath.h>
-#include "ElementOps.h"
+#include "ElementOps.h"
namespace armarx::viz
{
@@ -12,7 +12,8 @@ namespace armarx::viz
public:
using ElementOps::ElementOps;
- Robot& file(std::string const& project, std::string const& filename)
+ Robot&
+ file(std::string const& project, std::string const& filename)
{
data_->project = project;
data_->filename = filename;
@@ -20,7 +21,8 @@ namespace armarx::viz
return *this;
}
- Robot& file(const armarx::PackagePath& packagePath)
+ Robot&
+ file(const armarx::PackagePath& packagePath)
{
data_->project = packagePath.serialize().package;
data_->filename = packagePath.serialize().package + "/" + packagePath.serialize().path;
@@ -28,48 +30,53 @@ namespace armarx::viz
return *this;
}
- Robot& useCollisionModel()
+ Robot&
+ useCollisionModel()
{
data_->drawStyle |= data::ModelDrawStyle::COLLISION;
return *this;
}
- Robot& useFullModel()
+ Robot&
+ useFullModel()
{
data_->drawStyle &= ~data::ModelDrawStyle::COLLISION;
return *this;
}
- Robot& overrideColor(Color c)
+ Robot&
+ overrideColor(Color c)
{
data_->drawStyle |= data::ModelDrawStyle::OVERRIDE_COLOR;
return color(c);
}
- Robot& useOriginalColor()
+ Robot&
+ useOriginalColor()
{
data_->drawStyle &= ~data::ModelDrawStyle::OVERRIDE_COLOR;
return *this;
}
- Robot& joint(std::string const& name, float value)
+ Robot&
+ joint(std::string const& name, float value)
{
data_->jointValues[name] = value;
return *this;
}
- Robot& joints(std::map<std::string, float> const& values)
+ Robot&
+ joints(std::map<std::string, float> const& values)
{
data_->jointValues = values;
return *this;
}
-
};
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp
index b4926f0d369e93bd64897a1b8a33313986a1699d..799053efac4b3bf7840dba628b0b2486734dcdec 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.cpp
@@ -2,44 +2,44 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::viz
{
- RobotHand& RobotHand::fileBySide(const std::string& side, RobotInfoNodePtr robotInfo)
+ RobotHand&
+ RobotHand::fileBySide(const std::string& side, RobotInfoNodePtr robotInfo)
{
ARMARX_CHECK_NOT_NULL(robotInfo);
RobotNameHelperPtr nh = RobotNameHelper::Create(robotInfo, nullptr);
return this->fileBySide(side, *nh);
}
- RobotHand& RobotHand::fileBySide(const std::string& side, const RobotNameHelper& nameHelper)
+ RobotHand&
+ RobotHand::fileBySide(const std::string& side, const RobotNameHelper& nameHelper)
{
RobotNameHelper::Arm arm = nameHelper.getArm(side);
return this->fileByArm(arm);
}
- RobotHand& RobotHand::fileByArm(const RobotNameHelper::Arm& arm)
+ RobotHand&
+ RobotHand::fileByArm(const RobotNameHelper::Arm& arm)
{
this->arm = arm;
this->file(arm.getHandModelPackage(), arm.getHandModelPath());
return *this;
}
- RobotHand& RobotHand::fileByArm(const RobotNameHelper::RobotArm& arm)
+ RobotHand&
+ RobotHand::fileByArm(const RobotNameHelper::RobotArm& arm)
{
return this->fileByArm(arm.getArm());
}
- RobotHand& RobotHand::tcpPose(const Eigen::Matrix4f& tcpPose, VirtualRobot::RobotPtr robot)
+ RobotHand&
+ RobotHand::tcpPose(const Eigen::Matrix4f& tcpPose, VirtualRobot::RobotPtr robot)
{
ARMARX_CHECK(arm) << "Set RobotHand::side() before setting the TCP pose.";
RobotNameHelper::RobotArm robotArm = arm->addRobot(robot);
this->pose(tcpPose * robotArm.getTcp2HandRootTransform());
return *this;
}
-}
-
-
-
-
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/RobotHand.h b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.h
index 0a6e7d4a44f5343a4143c338b66131bf7cfe1363..3e9c6a2604036028ac6005b49c284a3261073a21 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/RobotHand.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/RobotHand.h
@@ -9,7 +9,6 @@
#include "Robot.h"
-
namespace armarx::viz
{
@@ -19,7 +18,6 @@ namespace armarx::viz
class RobotHand : public Robot
{
public:
-
using Robot::Robot;
/**
@@ -46,7 +44,6 @@ namespace armarx::viz
/// The arm name helper. Set by `fileBySide()`.
std::optional<RobotNameHelper::Arm> arm;
-
};
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/point_cloud_type_traits.hpp b/source/RobotAPI/components/ArViz/Client/elements/point_cloud_type_traits.hpp
index c3d10f583b6a62c35c4ee66a12205f5f3fd51096..57bdfb84c160b6e5e7196508914d012ef02fe47c 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/point_cloud_type_traits.hpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/point_cloud_type_traits.hpp
@@ -3,7 +3,6 @@
#include <SimoxUtility/meta/has_member_macros/has_member.hpp>
-
namespace armarx::viz::detail
{
@@ -13,17 +12,13 @@ namespace armarx::viz::detail
define_has_member(a);
define_has_member(label);
-
template <typename PointT>
class has_members_rgba
{
public:
- static constexpr bool value =
- has_member_r<PointT>::value && has_member_g<PointT>::value
- && has_member_b<PointT>::value && has_member_a<PointT>::value;
-
+ static constexpr bool value = has_member_r<PointT>::value && has_member_g<PointT>::value &&
+ has_member_b<PointT>::value && has_member_a<PointT>::value;
};
-}
-
+} // namespace armarx::viz::detail
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
index 6208ac28e66c86230f0626970c3a994b7909dee3..c2f372f5a4f355a36f4b0c55b0b8174b08423061 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.cpp
@@ -1,12 +1,12 @@
#include "ElementVisualizer.h"
-#include <RobotAPI/components/ArViz/IceConversions.h>
-
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
-#include <Inventor/nodes/SoUnits.h>
-#include <Inventor/nodes/SoTransform.h>
+#include <RobotAPI/components/ArViz/IceConversions.h>
+
#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoTransform.h>
+#include <Inventor/nodes/SoUnits.h>
namespace armarx::viz::coin
{
@@ -29,7 +29,8 @@ namespace armarx::viz::coin
separator->addChild(switch_);
}
- void ElementVisualization::updateBase(data::Element const& element)
+ void
+ ElementVisualization::updateBase(data::Element const& element)
{
auto& p = element.pose;
transform->translation.setValue(p.x, p.y, p.z);
@@ -48,7 +49,8 @@ namespace armarx::viz::coin
material->setOverride(overrideMaterial);
}
- std::unique_ptr<ElementVisualization> ElementVisualizer::create(const data::Element& element)
+ std::unique_ptr<ElementVisualization>
+ ElementVisualizer::create(const data::Element& element)
{
std::unique_ptr<ElementVisualization> result(createDerived());
update(element, result.get());
@@ -56,10 +58,10 @@ namespace armarx::viz::coin
return result;
}
- bool ElementVisualizer::update(data::Element const& element, ElementVisualization* visu)
+ bool
+ ElementVisualizer::update(data::Element const& element, ElementVisualization* visu)
{
visu->updateBase(element);
return updateDerived(element, visu);
}
-}
-
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
index c29794f2c363a68d39f688bfd7c9aefcba15a319..fd30c79eeabac6192c902f456050857b621d9e38 100644
--- a/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/ElementVisualizer.h
@@ -1,12 +1,12 @@
#pragma once
+#include <memory>
+
#include <RobotAPI/interface/ArViz/Elements.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoSwitch.h>
-#include <memory>
-
namespace armarx::viz::data
{
class Element;
@@ -48,7 +48,6 @@ namespace armarx::viz::coin
virtual bool updateDerived(data::Element const& element, ElementVisualization* data) = 0;
};
-
template <typename NodeT>
struct TypedElementVisualization : ElementVisualization
{
@@ -75,15 +74,16 @@ namespace armarx::viz::coin
using DataType = DataT;
using ElementType = typename DataType::ElementType;
-
- DataType* createDerived() final
+ DataType*
+ createDerived() final
{
DataType* result = new DataType;
result->switch_->addChild(result->node);
return result;
}
- bool updateDerived(data::Element const& element_, ElementVisualization* data_) final
+ bool
+ updateDerived(data::Element const& element_, ElementVisualization* data_) final
{
auto const& element = static_cast<ElementType const&>(element_);
auto* data = dynamic_cast<DataType*>(data_);
@@ -111,4 +111,4 @@ namespace armarx::viz::coin
return false;
}
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/ExportVRML.cpp b/source/RobotAPI/components/ArViz/Coin/ExportVRML.cpp
index 8bdc64d2f1aa2690996fee29d550e458c2a3db73..9f07e51ed4e2b1787b1f069a713c152331e656a2 100644
--- a/source/RobotAPI/components/ArViz/Coin/ExportVRML.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/ExportVRML.cpp
@@ -2,98 +2,100 @@
#include <ArmarXCore/core/logging/Logging.h>
-#include <Inventor/actions/SoWriteAction.h>
+#include <Inventor/VRMLnodes/SoVRMLGroup.h>
#include <Inventor/actions/SoToVRML2Action.h>
+#include <Inventor/actions/SoWriteAction.h>
#include <Inventor/nodes/SoFile.h>
#include <Inventor/nodes/SoSeparator.h>
-#include <Inventor/VRMLnodes/SoVRMLGroup.h>
namespace armarx::viz::coin
{
-static SoGroup* convertSoFileChildren(SoGroup* orig)
-{
- if (!orig)
+ static SoGroup*
+ convertSoFileChildren(SoGroup* orig)
{
- return new SoGroup;
- }
+ if (!orig)
+ {
+ return new SoGroup;
+ }
- SoGroup* storeResult;
+ SoGroup* storeResult;
- if (orig->getTypeId() == SoSeparator::getClassTypeId())
- {
- storeResult = new SoSeparator;
- }
- else
- {
- storeResult = new SoGroup;
- }
+ if (orig->getTypeId() == SoSeparator::getClassTypeId())
+ {
+ storeResult = new SoSeparator;
+ }
+ else
+ {
+ storeResult = new SoGroup;
+ }
- storeResult->ref();
+ storeResult->ref();
- if (orig->getTypeId().isDerivedFrom(SoGroup::getClassTypeId()))
- {
- // process group node
- for (int i = 0; i < orig->getNumChildren(); i++)
+ if (orig->getTypeId().isDerivedFrom(SoGroup::getClassTypeId()))
{
- SoNode* n1 = orig->getChild(i);
-
- if (n1->getTypeId().isDerivedFrom(SoGroup::getClassTypeId()))
- {
- // convert group
- SoGroup* n2 = (SoGroup*)n1;
- SoGroup* gr1 = convertSoFileChildren(n2);
- storeResult->addChild(gr1);
- }
- else if (n1->getTypeId() == SoFile::getClassTypeId())
- {
- // really load file!!
- SoFile* fn = (SoFile*)n1;
- SoGroup* fileChildren;
- fileChildren = fn->copyChildren();
- storeResult->addChild(fileChildren);
- }
- else
+ // process group node
+ for (int i = 0; i < orig->getNumChildren(); i++)
{
- // just copy child node
- storeResult->addChild(n1);
+ SoNode* n1 = orig->getChild(i);
+
+ if (n1->getTypeId().isDerivedFrom(SoGroup::getClassTypeId()))
+ {
+ // convert group
+ SoGroup* n2 = (SoGroup*)n1;
+ SoGroup* gr1 = convertSoFileChildren(n2);
+ storeResult->addChild(gr1);
+ }
+ else if (n1->getTypeId() == SoFile::getClassTypeId())
+ {
+ // really load file!!
+ SoFile* fn = (SoFile*)n1;
+ SoGroup* fileChildren;
+ fileChildren = fn->copyChildren();
+ storeResult->addChild(fileChildren);
+ }
+ else
+ {
+ // just copy child node
+ storeResult->addChild(n1);
+ }
}
}
- }
- storeResult->unrefNoDelete();
- return storeResult;
-}
+ storeResult->unrefNoDelete();
+ return storeResult;
+ }
-void exportToVRML(SoNode* node, std::string const& exportFilePath)
-{
- SoOutput* so = new SoOutput();
- if (!so->openFile(exportFilePath.c_str()))
+ void
+ exportToVRML(SoNode* node, std::string const& exportFilePath)
{
- ARMARX_ERROR << "Could not open file " << exportFilePath << " for writing.";
- return;
- }
+ SoOutput* so = new SoOutput();
+ if (!so->openFile(exportFilePath.c_str()))
+ {
+ ARMARX_ERROR << "Could not open file " << exportFilePath << " for writing.";
+ return;
+ }
- so->setHeaderString("#VRML V2.0 utf8");
+ so->setHeaderString("#VRML V2.0 utf8");
- SoGroup* n = new SoGroup;
- n->ref();
- n->addChild(node);
- SoGroup* newVisu = convertSoFileChildren(n);
- newVisu->ref();
+ SoGroup* n = new SoGroup;
+ n->ref();
+ n->addChild(node);
+ SoGroup* newVisu = convertSoFileChildren(n);
+ newVisu->ref();
- SoToVRML2Action tovrml2;
- tovrml2.apply(newVisu);
- SoVRMLGroup* newroot = tovrml2.getVRML2SceneGraph();
- newroot->ref();
- SoWriteAction wra(so);
- wra.apply(newroot);
- newroot->unref();
+ SoToVRML2Action tovrml2;
+ tovrml2.apply(newVisu);
+ SoVRMLGroup* newroot = tovrml2.getVRML2SceneGraph();
+ newroot->ref();
+ SoWriteAction wra(so);
+ wra.apply(newroot);
+ newroot->unref();
- so->closeFile();
+ so->closeFile();
- newVisu->unref();
- n->unref();
-}
+ newVisu->unref();
+ n->unref();
+ }
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/ExportVRML.h b/source/RobotAPI/components/ArViz/Coin/ExportVRML.h
index 85758493f8008e98dc4a4389a6de0b2c64b456dd..95a01f6135d0c2067c461f770d70dd6435d3748c 100644
--- a/source/RobotAPI/components/ArViz/Coin/ExportVRML.h
+++ b/source/RobotAPI/components/ArViz/Coin/ExportVRML.h
@@ -1,9 +1,9 @@
#pragma once
-#include <Inventor/nodes/SoNode.h>
-
#include <string>
+#include <Inventor/nodes/SoNode.h>
+
namespace armarx::viz::coin
{
diff --git a/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp b/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
index 743f0f5a50ae46d86a13047898dd698cd1b7aa75..916fbd54ee37a15970c38e59277d22f0fe175bf9 100644
--- a/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/RegisterVisualizationTypes.cpp
@@ -1,24 +1,23 @@
-#include "Visualizer.h"
-
+#include "VisualizationArrow.h"
+#include "VisualizationArrowCircle.h"
#include "VisualizationBox.h"
#include "VisualizationCylinder.h"
#include "VisualizationCylindroid.h"
-#include "VisualizationSphere.h"
#include "VisualizationEllipsoid.h"
-#include "VisualizationPose.h"
#include "VisualizationLine.h"
-#include "VisualizationText.h"
-#include "VisualizationArrow.h"
-#include "VisualizationArrowCircle.h"
-#include "VisualizationPointCloud.h"
-#include "VisualizationPolygon.h"
#include "VisualizationMesh.h"
-#include "VisualizationRobot.h"
#include "VisualizationObject.h"
#include "VisualizationPath.h"
+#include "VisualizationPointCloud.h"
+#include "VisualizationPolygon.h"
+#include "VisualizationPose.h"
+#include "VisualizationRobot.h"
+#include "VisualizationSphere.h"
+#include "VisualizationText.h"
+#include "Visualizer.h"
-
-void armarx::viz::CoinVisualizer::registerVisualizationTypes()
+void
+armarx::viz::CoinVisualizer::registerVisualizationTypes()
{
using namespace armarx::viz::coin;
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationArrow.h b/source/RobotAPI/components/ArViz/Coin/VisualizationArrow.h
index 7dd5f5d37b6716717d88ec81b1da2c26a02f89c7..796afca1ce68c24ee4b484528d341229533a8716 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationArrow.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationArrow.h
@@ -1,8 +1,8 @@
#pragma once
-#include "ElementVisualizer.h"
-
#include <RobotAPI/interface/ArViz/Elements.h>
+
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoCone.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoSphere.h>
@@ -27,7 +27,8 @@ namespace armarx::viz::coin
node->addChild(cone);
}
- bool update(ElementType const& element)
+ bool
+ update(ElementType const& element)
{
float coneHeight = element.width * 6.0f;
float coneBottomRadius = element.width * 2.5f;
@@ -52,4 +53,4 @@ namespace armarx::viz::coin
SoTranslation* transl;
SoCone* cone;
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationArrowCircle.h b/source/RobotAPI/components/ArViz/Coin/VisualizationArrowCircle.h
index 179864cc951672758ad36351829e97105d9452e9..17979cf57204d72dfe8f673cf3822b71e55ca7f1 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationArrowCircle.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationArrowCircle.h
@@ -1,18 +1,17 @@
#pragma once
-#include "ElementVisualizer.h"
-
#include <RobotAPI/interface/ArViz/Elements.h>
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoCone.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoIndexedFaceSet.h>
#include <Inventor/nodes/SoShapeHints.h>
#include <Inventor/nodes/SoSphere.h>
+#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoTranslation.h>
-
namespace armarx::viz::coin
{
struct VisualizationArrowCircle : TypedElementVisualization<SoSeparator>
@@ -76,7 +75,8 @@ namespace armarx::viz::coin
matInx.resize(numFaces * 4);
}
- bool update(ElementType const& element)
+ bool
+ update(ElementType const& element)
{
float completion = std::min<float>(1.0f, std::max(-1.0f, element.completion));
int sign = completion >= 0 ? 1 : -1;
@@ -133,7 +133,8 @@ namespace armarx::viz::coin
short rt = (short)((horizontalIt + 1) + verticalIt * (numVerticesPerRow));
short lb = (short)(horizontalIt + (verticalIt + 1) * (numVerticesPerRow));
- short rb = (short)((horizontalIt + 1) + (verticalIt + 1) * (numVerticesPerRow));
+ short rb =
+ (short)((horizontalIt + 1) + (verticalIt + 1) * (numVerticesPerRow));
faces[faceIndex * 4 + 0] = lt;
@@ -189,4 +190,4 @@ namespace armarx::viz::coin
return true;
}
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationBox.h b/source/RobotAPI/components/ArViz/Coin/VisualizationBox.h
index 3c9b05292973e24fb5a53bdb0ba51cf7b15e2a50..fa50baa0aa19fe2c14830d1c9074f652de2fc9f3 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationBox.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationBox.h
@@ -1,8 +1,8 @@
#pragma once
-#include "ElementVisualizer.h"
-
#include <RobotAPI/interface/ArViz/Elements.h>
+
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoCube.h>
namespace armarx::viz::coin
@@ -11,7 +11,8 @@ namespace armarx::viz::coin
{
using ElementType = data::ElementBox;
- bool update(ElementType const& element)
+ bool
+ update(ElementType const& element)
{
node->width = element.size.e0;
node->height = element.size.e1;
@@ -20,4 +21,4 @@ namespace armarx::viz::coin
return true;
}
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationCylinder.h b/source/RobotAPI/components/ArViz/Coin/VisualizationCylinder.h
index bc9246f4590bbd25789271eb92d8ca81128bd79e..119188eec8594ec1158d2ee859a482bba2f0f10f 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationCylinder.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationCylinder.h
@@ -1,8 +1,8 @@
#pragma once
-#include "ElementVisualizer.h"
-
#include <RobotAPI/interface/ArViz/Elements.h>
+
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoCylinder.h>
namespace armarx::viz::coin
@@ -11,7 +11,8 @@ namespace armarx::viz::coin
{
using ElementType = data::ElementCylinder;
- bool update(ElementType const& element)
+ bool
+ update(ElementType const& element)
{
node->radius = element.radius;
node->height = element.height;
@@ -19,4 +20,4 @@ namespace armarx::viz::coin
return true;
}
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.cpp
index 6d2cc827a17e3b5e507ead7888d673cb10d849de..a4149cecff14953dcc5d9253146170c1d8d1fd19 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.cpp
@@ -18,46 +18,46 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
- #include "VisualizationCylindroid.h"
-
+
+#include "VisualizationCylindroid.h"
+
#include <Inventor/nodes/SoMaterial.h>
-
- bool
- armarx::viz::coin::VisualizationCylindroid::update(ElementType const& element)
- {
- auto color = element.color;
- constexpr float conv = 1.0f / 255.0f;
- const float r = color.r * conv;
- const float g = color.g * conv;
- const float b = color.b * conv;
- const float a = color.a * conv;
-
- VirtualRobot::VisualizationNodePtr cylindroid_node;
- {
- // Params.
- SoMaterial* mat = new SoMaterial;
- mat->diffuseColor.setValue(r, g, b);
- mat->ambientColor.setValue(r, g, b);
- mat->transparency.setValue(1. - a);
-
- SoSeparator* res = new SoSeparator();
- res->ref();
- SoUnits* u = new SoUnits();
- u->units = SoUnits::MILLIMETERS;
- res->addChild(u);
- res->addChild(VirtualRobot::CoinVisualizationFactory::CreateCylindroid(
- element.axisLengths.e0, element.axisLengths.e1, element.height, mat));
-
- cylindroid_node.reset(new VirtualRobot::CoinVisualizationNode(res));
- res->unref();
- }
-
- SoNode* cylindroid = dynamic_cast<VirtualRobot::CoinVisualizationNode&>(*cylindroid_node)
- .getCoinVisualization();
-
- node->removeAllChildren();
- node->addChild(cylindroid);
-
- return true;
- }
+
+bool
+armarx::viz::coin::VisualizationCylindroid::update(ElementType const& element)
+{
+ auto color = element.color;
+ constexpr float conv = 1.0f / 255.0f;
+ const float r = color.r * conv;
+ const float g = color.g * conv;
+ const float b = color.b * conv;
+ const float a = color.a * conv;
+
+ VirtualRobot::VisualizationNodePtr cylindroid_node;
+ {
+ // Params.
+ SoMaterial* mat = new SoMaterial;
+ mat->diffuseColor.setValue(r, g, b);
+ mat->ambientColor.setValue(r, g, b);
+ mat->transparency.setValue(1. - a);
+
+ SoSeparator* res = new SoSeparator();
+ res->ref();
+ SoUnits* u = new SoUnits();
+ u->units = SoUnits::MILLIMETERS;
+ res->addChild(u);
+ res->addChild(VirtualRobot::CoinVisualizationFactory::CreateCylindroid(
+ element.axisLengths.e0, element.axisLengths.e1, element.height, mat));
+
+ cylindroid_node.reset(new VirtualRobot::CoinVisualizationNode(res));
+ res->unref();
+ }
+
+ SoNode* cylindroid =
+ dynamic_cast<VirtualRobot::CoinVisualizationNode&>(*cylindroid_node).getCoinVisualization();
+
+ node->removeAllChildren();
+ node->addChild(cylindroid);
+
+ return true;
+}
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h b/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h
index ded11d25008b2d2525294ac9d5bd4aa317bc2c16..2917aadff915ab33802053935db56192b3659f29 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationCylindroid.h
@@ -1,12 +1,11 @@
#pragma once
-#include "ElementVisualizer.h"
-
-#include <RobotAPI/interface/ArViz/Elements.h>
-
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
+#include <RobotAPI/interface/ArViz/Elements.h>
+
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoUnits.h>
namespace armarx::viz::coin
@@ -17,4 +16,4 @@ namespace armarx::viz::coin
bool update(ElementType const& element);
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h b/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
index e38337629eebaac5d26dfd893e43e605e57f574a..9cc91475c75c33a72d61f903750dfd014423e9bd 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationEllipsoid.h
@@ -1,9 +1,9 @@
#pragma once
-#include "ElementVisualizer.h"
-
#include <RobotAPI/interface/ArViz/Elements.h>
+#include "ElementVisualizer.h"
+
class SoComplexity;
class SoScale;
class SoSphere;
@@ -23,4 +23,4 @@ namespace armarx::viz::coin
SoScale* scale = nullptr;
SoSphere* sphere = nullptr;
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationLine.h b/source/RobotAPI/components/ArViz/Coin/VisualizationLine.h
index a25cd1f4b69d54fb87c789e0a416e8809ba3f6d1..791e3889a671e14c0a480f45ef71942dd5f1f09f 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationLine.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationLine.h
@@ -1,12 +1,11 @@
#pragma once
-#include "ElementVisualizer.h"
-
#include <RobotAPI/interface/ArViz/Elements.h>
-#include <Inventor/nodes/SoLineSet.h>
-#include <Inventor/nodes/SoDrawStyle.h>
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoCoordinate3.h>
+#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoLineSet.h>
namespace armarx::viz::coin
{
@@ -27,7 +26,8 @@ namespace armarx::viz::coin
node->addChild(lineSet);
}
- bool update(ElementType const& element)
+ bool
+ update(ElementType const& element)
{
lineStyle->lineWidth.setValue(element.lineWidth);
@@ -42,4 +42,4 @@ namespace armarx::viz::coin
SoDrawStyle* lineStyle;
SoCoordinate3* coordinate3;
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h b/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h
index 391fe0e140a2f5e907be36871a86837535a63321..1b4aec938de210b75c6d85fb699c4104af171280 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h
@@ -1,9 +1,8 @@
#pragma once
-#include "ElementVisualizer.h"
-
#include <RobotAPI/interface/ArViz/Elements.h>
+#include "ElementVisualizer.h"
#include <Inventor/SbColor.h>
class SoCoordinate3;
@@ -29,4 +28,4 @@ namespace armarx::viz::coin
std::vector<int32_t> faces;
std::vector<int32_t> matInx;
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
index f27e344ca8ab1256bd618d1f86e5c2496abcbfd1..780f9a5f77ba5edc46dd6b8e943bce931fd2a6d9 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.cpp
@@ -1,29 +1,29 @@
#include "VisualizationObject.h"
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-#include <Inventor/nodes/SoMaterial.h>
-#include <Inventor/SbColor.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/Import/MeshImport/AssimpReader.h>
+#include <VirtualRobot/ManipulationObject.h>
+#include <VirtualRobot/SceneObject.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/XML/ObjectIO.h>
+#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <VirtualRobot/ManipulationObject.h>
-#include <VirtualRobot/SceneObject.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/XML/ObjectIO.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
-#include <VirtualRobot/Import/MeshImport/AssimpReader.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <Inventor/SbColor.h>
+#include <Inventor/nodes/SoMaterial.h>
namespace armarx::viz::coin
{
namespace
{
- std::string findObjectInArmarXObjects(const std::string& filename)
+ std::string
+ findObjectInArmarXObjects(const std::string& filename)
{
IceUtil::Time start = IceUtil::Time::now();
std::string objectName = std::filesystem::path(filename).filename().stem();
@@ -36,22 +36,24 @@ namespace armarx::viz::coin
if (std::optional<armarx::ObjectInfo> info = objectFinder.findObject("", objectName))
{
fullFilename = info->simoxXML().absolutePath;
- ss << "Found '" << objectName << "' in ArmarXObjects as " << *info << " \nat '" << fullFilename << "'.";
+ ss << "Found '" << objectName << "' in ArmarXObjects as " << *info << " \nat '"
+ << fullFilename << "'.";
}
else
{
ss << "Did not find '" << objectName << "' in ArmarXObjects.";
}
- ss << "\n(Lookup took " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms.)";
+ ss << "\n(Lookup took " << (IceUtil::Time::now() - start).toMilliSecondsDouble()
+ << " ms.)";
ARMARX_INFO << ss.str();
return fullFilename;
}
-
- VirtualRobot::ManipulationObjectPtr loadObject(std::string const& project, std::string const& filename)
+ VirtualRobot::ManipulationObjectPtr
+ loadObject(std::string const& project, std::string const& filename)
{
- VirtualRobot::ManipulationObjectPtr result;
+ VirtualRobot::ManipulationObjectPtr result;
if (filename.empty())
{
@@ -84,8 +86,7 @@ namespace armarx::viz::coin
}
if (fullFilename.empty())
{
- ARMARX_INFO << deactivateSpam()
- << "Unable to find readable file for name "
+ ARMARX_INFO << deactivateSpam() << "Unable to find readable file for name "
<< filename;
return result;
}
@@ -100,19 +101,22 @@ namespace armarx::viz::coin
if (ext == ".wrl" || ext == ".iv")
{
- VirtualRobot::VisualizationFactoryPtr factory = VirtualRobot::VisualizationFactory::fromName("inventor", nullptr);
- VirtualRobot::VisualizationNodePtr vis = factory->getVisualizationFromFile(fullFilename);
- result = VirtualRobot::ManipulationObjectPtr(new VirtualRobot::ManipulationObject(filename, vis));
+ VirtualRobot::VisualizationFactoryPtr factory =
+ VirtualRobot::VisualizationFactory::fromName("inventor", nullptr);
+ VirtualRobot::VisualizationNodePtr vis =
+ factory->getVisualizationFromFile(fullFilename);
+ result = VirtualRobot::ManipulationObjectPtr(
+ new VirtualRobot::ManipulationObject(filename, vis));
}
else if (ext == ".xml" || ext == ".moxml")
{
result = VirtualRobot::ObjectIO::loadManipulationObject(fullFilename);
-
}
else if (VirtualRobot::AssimpReader::can_load(fullFilename))
{
const auto& tri = VirtualRobot::AssimpReader{}.readFileAsTriMesh(fullFilename);
- result = VirtualRobot::ManipulationObjectPtr(new VirtualRobot::ManipulationObject(filename, tri));
+ result = VirtualRobot::ManipulationObjectPtr(
+ new VirtualRobot::ManipulationObject(filename, tri));
}
else
{
@@ -131,7 +135,8 @@ namespace armarx::viz::coin
static std::vector<LoadedObject> objectcache;
- LoadedObject getObjectFromCache(std::string const& project, std::string const& filename)
+ LoadedObject
+ getObjectFromCache(std::string const& project, std::string const& filename)
{
// We can use a global variable, since this code is only executed in the GUI thread
@@ -156,9 +161,10 @@ namespace armarx::viz::coin
return result;
}
- }
+ } // namespace
- bool VisualizationObject::update(ElementType const& element)
+ bool
+ VisualizationObject::update(ElementType const& element)
{
bool fileChanged = loaded.project != element.project || loaded.filename != element.filename;
if (fileChanged)
@@ -170,8 +176,7 @@ namespace armarx::viz::coin
{
ARMARX_WARNING << deactivateSpam(120)
<< "Object will not be visualized since it could not be loaded."
- << "\nID: " << element.id
- << "\nProject: " << element.project
+ << "\nID: " << element.id << "\nProject: " << element.project
<< "\nFilename: " << element.filename;
return true;
}
@@ -211,7 +216,8 @@ namespace armarx::viz::coin
return true;
}
- void VisualizationObject::recreateVisualizationNodes(int drawStyle)
+ void
+ VisualizationObject::recreateVisualizationNodes(int drawStyle)
{
VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Full;
if (drawStyle & data::ModelDrawStyle::COLLISION)
@@ -231,7 +237,8 @@ namespace armarx::viz::coin
nodeMat->setOverride(false);
nodeSep->addChild(nodeMat);
- VirtualRobot::CoinVisualizationPtr nodeVisu = object.getVisualization<VirtualRobot::CoinVisualization>(visuType);
+ VirtualRobot::CoinVisualizationPtr nodeVisu =
+ object.getVisualization<VirtualRobot::CoinVisualization>(visuType);
if (nodeVisu)
{
SoNode* sepRobNode = nodeVisu->getCoinVisualization();
@@ -245,8 +252,9 @@ namespace armarx::viz::coin
node->addChild(nodeSep);
}
- void clearObjectCache()
+ void
+ clearObjectCache()
{
objectcache.clear();
}
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.h b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.h
index 0cde1f700b084c3bc8fbadc7fcf2f5cb36036b17..7f1df15156eb7bc667aa765eb34c19f100bbce09 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationObject.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationObject.h
@@ -1,10 +1,10 @@
#pragma once
-#include "ElementVisualizer.h"
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/interface/ArViz/Elements.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include "ElementVisualizer.h"
namespace armarx::viz::coin
{
@@ -28,4 +28,4 @@ namespace armarx::viz::coin
};
void clearObjectCache();
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
index 84c7b3a30512d5c0a93339d1f602c524fd613a38..272f45179c6d5ff06429aa90b47e251d75e2c6aa 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.cpp
@@ -6,8 +6,8 @@
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoLineSet.h>
-#include <Inventor/nodes/SoPointSet.h>
#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoPointSet.h>
namespace armarx::viz::coin
{
@@ -49,7 +49,8 @@ namespace armarx::viz::coin
node->addChild(new SoPointSet);
}
- bool VisualizationPath::update(ElementType const& element)
+ bool
+ VisualizationPath::update(ElementType const& element)
{
// set position
coordinate3->point.setValuesPointer(element.points.size(),
@@ -60,7 +61,7 @@ namespace armarx::viz::coin
constexpr float toUnit = 1.0F / 255.0F;
- const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
+ const auto color = SbVec3f(lineColor.r, lineColor.g, lineColor.b) * toUnit;
const float transparency = 1.0F - static_cast<float>(lineColor.a) * toUnit;
lineMaterial->diffuseColor.setValue(color);
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
index 487ee9df58baeb6b91e91de6fdb3be873511a0af..591a3e9705421e582158b46decc5c4381caf1750 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPath.h
@@ -21,10 +21,10 @@
#pragma once
-#include "ElementVisualizer.h"
-
#include <RobotAPI/interface/ArViz/Elements.h>
+#include "ElementVisualizer.h"
+
class SoCoordinate3;
class SoDrawStyle;
class SoLineSet;
@@ -50,6 +50,5 @@ namespace armarx::viz::coin
// SoMaterial* pclMat;
SoCoordinate3* pclCoords;
SoDrawStyle* pclStyle;
-
};
-} // namespace armarx::viz::coin
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
index 16df986fe4ef3efa20e84d91392e1475440aed83..90f00aaad65a19de05aaa435423e6e0a5bfd475c 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPointCloud.h
@@ -1,16 +1,15 @@
#pragma once
#include <cfloat>
-#include "ElementVisualizer.h"
#include <RobotAPI/interface/ArViz/Elements.h>
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoPointSet.h>
-
#include <x86intrin.h>
namespace armarx::viz::coin
@@ -36,8 +35,8 @@ namespace armarx::viz::coin
node->addChild(new SoPointSet);
}
- __attribute__((target("default")))
- bool update(ElementType const& element)
+ __attribute__((target("default"))) bool
+ update(ElementType const& element)
{
data::ColoredPoint const* pclData = (data::ColoredPoint const*)element.points.data();
int pclSize = element.points.size() / sizeof(data::ColoredPoint);
@@ -72,8 +71,8 @@ namespace armarx::viz::coin
return true;
}
- __attribute__((target("sse4.1")))
- bool update(ElementType const& element)
+ __attribute__((target("sse4.1"))) bool
+ update(ElementType const& element)
{
float* pclIn = (float*)element.points.data();
int pclSize = element.points.size() / sizeof(data::ColoredPoint);
@@ -209,4 +208,4 @@ namespace armarx::viz::coin
std::vector<float> buffer;
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h
index 5c185f4539f265fd1fba2f8807142df1052ba9c7..e77c48b8cfd915eb9f0faf812edd29fb42bceb50 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPolygon.h
@@ -1,18 +1,16 @@
#pragma once
-#include "ElementVisualizer.h"
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/interface/ArViz/Elements.h>
-#include <ArmarXCore/core/logging/Logging.h>
-
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoFaceSet.h>
#include <Inventor/nodes/SoLineSet.h>
#include <Inventor/nodes/SoMaterial.h>
-
namespace armarx::viz::coin
{
struct VisualizationPolygon : TypedElementVisualization<SoSeparator>
@@ -43,13 +41,13 @@ namespace armarx::viz::coin
node->addChild(lineSep);
}
- bool update(ElementType const& element)
+ bool
+ update(ElementType const& element)
{
if (element.points.size() < 3)
{
// A polygon with < 3 points is invalid.
- ARMARX_WARNING << deactivateSpam(1000)
- << "Ignoring polygon '" << element.id << "' "
+ ARMARX_WARNING << deactivateSpam(1000) << "Ignoring polygon '" << element.id << "' "
<< "with " << element.points.size() << " points "
<< "(a polygon requires >= 3 points).";
return true;
@@ -106,4 +104,4 @@ namespace armarx::viz::coin
SoLineSet* lineSet;
SoMaterial* lineMaterial;
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationPose.h b/source/RobotAPI/components/ArViz/Coin/VisualizationPose.h
index 1e5e9719dbfbc546199417da57823d676fe50f2b..28974d048ae970722e1da23abc85a852f53d9427 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationPose.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationPose.h
@@ -1,9 +1,10 @@
#pragma once
-#include "ElementVisualizer.h"
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <RobotAPI/interface/ArViz/Elements.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoAsciiText.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoMaterial.h>
@@ -65,7 +66,8 @@ namespace armarx::viz::coin
node->addChild(textSep);
}
- bool update(ElementType const& element)
+ bool
+ update(ElementType const& element)
{
const float axisSize = 3.0f;
const float axisLength = 100.0f;
@@ -154,4 +156,4 @@ namespace armarx::viz::coin
std::array<SoCube*, 3> c2_;
std::array<SoTransform*, 3> t2_;
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 68cfb856e64ef8c225436dec38e9a5a894c493dc..59f0f8173828c2ea4c2d1e44f8dc15b307b57a7c 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -4,11 +4,12 @@
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/time/Duration.h>
-#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/ScopedStopWatch.h>
+
#include <Inventor/SbColor.h>
#include <Inventor/nodes/SoMaterial.h>
@@ -116,7 +117,7 @@ namespace armarx::viz::coin
{
// 2) We do not have unused instances in the pool ==> Clone one
ARMARX_INFO << "Cloning robot from cache " << VAROUT(project) << ", "
- << VAROUT(filename);
+ << VAROUT(filename);
if (!instancePool.robots.empty())
{
@@ -126,11 +127,13 @@ namespace armarx::viz::coin
bool preventCloningMeshesIfScalingIs1 = true;
{
- armarx::core::time::ScopedStopWatch sw([](const armarx::Duration& duration){
- ARMARX_DEBUG << "Cloning took " << duration.toSecondsDouble() << " seconds.";
- });
+ armarx::core::time::ScopedStopWatch sw(
+ [](const armarx::Duration& duration) {
+ ARMARX_DEBUG << "Cloning took "
+ << duration.toSecondsDouble() << " seconds.";
+ });
result.robot = robotToClone->clone(
- nullptr, scaling, preventCloningMeshesIfScalingIs1);
+ nullptr, scaling, preventCloningMeshesIfScalingIs1);
}
// Insert the cloned robot into the instance pool
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h
index 8bb77b7051bbb574eb4fc6f07d068eeebd33029c..0ec333b51ada9f7164fe6918c980289eed2547bf 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h
@@ -1,10 +1,10 @@
#pragma once
-#include "ElementVisualizer.h"
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/interface/ArViz/Elements.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include "ElementVisualizer.h"
namespace armarx::viz::coin
{
@@ -31,4 +31,4 @@ namespace armarx::viz::coin
};
void clearRobotCache();
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationSphere.h b/source/RobotAPI/components/ArViz/Coin/VisualizationSphere.h
index e4b707e6f6dd38061cbbddad9450ba69c7b7623b..9af11b24c805ad6a8e16dc17d59cdb0777e7e37c 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationSphere.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationSphere.h
@@ -1,8 +1,8 @@
#pragma once
-#include "ElementVisualizer.h"
-
#include <RobotAPI/interface/ArViz/Elements.h>
+
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoSphere.h>
namespace armarx::viz::coin
@@ -11,11 +11,12 @@ namespace armarx::viz::coin
{
using ElementType = data::ElementSphere;
- bool update(ElementType const& element)
+ bool
+ update(ElementType const& element)
{
node->radius = element.radius;
return true;
}
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationText.h b/source/RobotAPI/components/ArViz/Coin/VisualizationText.h
index d91f5af7109e5647b61c479e5d75d13f88510c9e..4bace0c10cb61c637c25fd527909b89e1860d1b5 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationText.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationText.h
@@ -1,9 +1,8 @@
#pragma once
-#include "ElementVisualizer.h"
-
#include <RobotAPI/interface/ArViz/Elements.h>
+#include "ElementVisualizer.h"
#include <Inventor/nodes/SoAsciiText.h>
namespace armarx::viz::coin
@@ -12,11 +11,12 @@ namespace armarx::viz::coin
{
using ElementType = data::ElementText;
- bool update(ElementType const& element)
+ bool
+ update(ElementType const& element)
{
node->string = element.text.c_str();
return true;
}
};
-}
+} // namespace armarx::viz::coin
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 04776c9f5941c60530506ee23f5a9234820df4eb..620c711d55be0ff16a91fed7121b97a8e138a78b 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -1,78 +1,83 @@
#include "Visualizer.h"
-#include "ExportVRML.h"
+#include <Eigen/Core>
+#include <Eigen/Geometry>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/GetTypeString.h>
+#include "ExportVRML.h"
#include <Inventor/SoPath.h>
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
namespace armarx::viz
{
-namespace coin
-{
- void clearRobotCache();
- void clearObjectCache();
-}
- static const int ANY_TRANSFORM = data::InteractionEnableFlags::TRANSLATION_X |
- data::InteractionEnableFlags::TRANSLATION_Y |
- data::InteractionEnableFlags::TRANSLATION_Z |
- data::InteractionEnableFlags::ROTATION_X |
- data::InteractionEnableFlags::ROTATION_Y |
- data::InteractionEnableFlags::ROTATION_Z |
- data::InteractionEnableFlags::SCALING_X |
- data::InteractionEnableFlags::SCALING_Y |
- data::InteractionEnableFlags::SCALING_Z;
+ namespace coin
+ {
+ void clearRobotCache();
+ void clearObjectCache();
+ } // namespace coin
+
+ static const int ANY_TRANSFORM =
+ data::InteractionEnableFlags::TRANSLATION_X | data::InteractionEnableFlags::TRANSLATION_Y |
+ data::InteractionEnableFlags::TRANSLATION_Z | data::InteractionEnableFlags::ROTATION_X |
+ data::InteractionEnableFlags::ROTATION_Y | data::InteractionEnableFlags::ROTATION_Z |
+ data::InteractionEnableFlags::SCALING_X | data::InteractionEnableFlags::SCALING_Y |
+ data::InteractionEnableFlags::SCALING_Z;
struct CoinVisualizerWrapper : IceUtil::Shared
{
class CoinVisualizer* this_;
- void onUpdateSuccess(data::LayerUpdates const& updates)
+ void
+ onUpdateSuccess(data::LayerUpdates const& updates)
{
this_->onUpdateSuccess(updates);
}
- void onUpdateFailure(Ice::Exception const& ex)
+ void
+ onUpdateFailure(Ice::Exception const& ex)
{
this_->onUpdateFailure(ex);
}
};
- static void selectionCallback(void* data, SoPath* path)
+ static void
+ selectionCallback(void* data, SoPath* path)
{
CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
this_->onSelectEvent(path, data::InteractionFeedbackType::SELECT);
}
- static void deselectionCallback(void* data, SoPath* path)
+ static void
+ deselectionCallback(void* data, SoPath* path)
{
CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
this_->onSelectEvent(path, data::InteractionFeedbackType::DESELECT);
}
- static void startManipulationCallback(void* data, SoDragger* dragger)
+ static void
+ startManipulationCallback(void* data, SoDragger* dragger)
{
CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_BEGIN_FLAG);
}
- static void duringManipulationCallback(void* data, SoDragger* dragger)
+ static void
+ duringManipulationCallback(void* data, SoDragger* dragger)
{
CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_DURING_FLAG);
}
- static void finishManipulationCallback(void* data, SoDragger* dragger)
+ static void
+ finishManipulationCallback(void* data, SoDragger* dragger)
{
CoinVisualizer* this_ = static_cast<CoinVisualizer*>(data);
this_->onManipulation(dragger, data::InteractionFeedbackType::TRANSFORM_END_FLAG);
}
- static const char* toString(CoinVisualizerState state)
+ static const char*
+ toString(CoinVisualizerState state)
{
switch (state)
{
@@ -90,9 +95,7 @@ namespace coin
struct TimedBlock
{
- TimedBlock(const char* function)
- : start(IceUtil::Time::now())
- , function(function)
+ TimedBlock(const char* function) : start(IceUtil::Time::now()), function(function)
{
}
@@ -100,8 +103,7 @@ namespace coin
{
IceUtil::Time diff = IceUtil::Time::now() - start;
// Horrible: We need to plot this actually
- ARMARX_INFO << "Time '" << function << "': "
- << diff.toMilliSecondsDouble() << " ms";
+ ARMARX_INFO << "Time '" << function << "': " << diff.toMilliSecondsDouble() << " ms";
}
private:
@@ -115,9 +117,10 @@ namespace coin
callbackData = new CoinVisualizerWrapper;
callbackData->this_ = this;
- callback = newCallback_StorageInterface_pullUpdatesSinceAndSendInteractions(callbackData,
- &CoinVisualizerWrapper::onUpdateSuccess,
- &CoinVisualizerWrapper::onUpdateFailure);
+ callback = newCallback_StorageInterface_pullUpdatesSinceAndSendInteractions(
+ callbackData,
+ &CoinVisualizerWrapper::onUpdateSuccess,
+ &CoinVisualizerWrapper::onUpdateFailure);
manipulatorGroup = new SoSeparator;
@@ -139,14 +142,15 @@ namespace coin
{
}
- void CoinVisualizer::clearCache()
+ void
+ CoinVisualizer::clearCache()
{
coin::clearRobotCache();
coin::clearObjectCache();
}
-
- void CoinVisualizer::startAsync(StorageInterfacePrx const& storage)
+ void
+ CoinVisualizer::startAsync(StorageInterfacePrx const& storage)
{
std::unique_lock<std::mutex> lock(stateMutex);
if (state != CoinVisualizerState::STOPPED)
@@ -160,7 +164,8 @@ namespace coin
stateStorage = storage;
}
- void CoinVisualizer::stop()
+ void
+ CoinVisualizer::stop()
{
if (state == CoinVisualizerState::STOPPED)
{
@@ -170,7 +175,8 @@ namespace coin
state = CoinVisualizerState::STOPPED;
}
- CoinVisualizer_ApplyTiming CoinVisualizer::apply(data::LayerUpdate const& update)
+ CoinVisualizer_ApplyTiming
+ CoinVisualizer::apply(data::LayerUpdate const& update)
{
IceUtil::Time time_start = IceUtil::Time::now();
@@ -201,7 +207,8 @@ namespace coin
return timing;
}
- CoinLayer& CoinVisualizer::findOrAddLayer(CoinLayerID const& layerID)
+ CoinLayer&
+ CoinVisualizer::findOrAddLayer(CoinLayerID const& layerID)
{
auto layerIt = layers.lowerBound(layerID);
@@ -219,7 +226,8 @@ namespace coin
return *layerIt;
}
- void CoinVisualizer::addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update)
+ void
+ CoinVisualizer::addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update)
{
layer->elements.reserve(update.elements.size());
for (viz::data::ElementPtr const& updatedElementPtr : update.elements)
@@ -244,8 +252,7 @@ namespace coin
}
if (visuIndex >= visuSize)
{
- ARMARX_WARNING << deactivateSpam(1)
- << "No visualizer for element type found: "
+ ARMARX_WARNING << deactivateSpam(1) << "No visualizer for element type found: "
<< armarx::GetTypeString(elementType);
continue;
}
@@ -253,7 +260,8 @@ namespace coin
auto oldElementIter = layer->lowerBound(updatedElement.id);
CoinLayerElement* oldElement = nullptr;
- if (oldElementIter != layer->elements.end() && oldElementIter->data->id == updatedElement.id)
+ if (oldElementIter != layer->elements.end() &&
+ oldElementIter->data->id == updatedElement.id)
{
// Element already exists
CoinLayerElement* oldElement = &*oldElementIter;
@@ -266,12 +274,15 @@ namespace coin
if (updated)
{
// Has an interaction been added?
- viz::data::InteractionDescription& oldInteraction = oldElement->data->interaction;
- viz::data::InteractionDescription& newInteraction = updatedElementPtr->interaction;
- if (newInteraction.enableFlags != oldInteraction.enableFlags
- || oldInteraction.contextMenuOptions != newInteraction.contextMenuOptions)
+ viz::data::InteractionDescription& oldInteraction =
+ oldElement->data->interaction;
+ viz::data::InteractionDescription& newInteraction =
+ updatedElementPtr->interaction;
+ if (newInteraction.enableFlags != oldInteraction.enableFlags ||
+ oldInteraction.contextMenuOptions != newInteraction.contextMenuOptions)
{
- addOrUpdateInteraction(layer->id, updatedElement.id, newInteraction, oldElement->visu.get());
+ addOrUpdateInteraction(
+ layer->id, updatedElement.id, newInteraction, oldElement->visu.get());
}
oldElement->data = updatedElementPtr;
@@ -291,7 +302,8 @@ namespace coin
viz::data::InteractionDescription& newInteraction = updatedElementPtr->interaction;
if (newInteraction.enableFlags != 0)
{
- addOrUpdateInteraction(layer->id, updatedElement.id, newInteraction, elementVisu.get());
+ addOrUpdateInteraction(
+ layer->id, updatedElement.id, newInteraction, elementVisu.get());
}
layer->node->addChild(elementVisu->separator);
@@ -303,29 +315,34 @@ namespace coin
else
{
// Need to add a new element
- layer->elements.insert(oldElementIter, CoinLayerElement{updatedElementPtr, std::move(elementVisu)});
+ layer->elements.insert(
+ oldElementIter,
+ CoinLayerElement{updatedElementPtr, std::move(elementVisu)});
}
}
else
{
std::string typeName = armarx::GetTypeString(elementType);
ARMARX_WARNING << deactivateSpam(typeName, 1)
- << "CoinElementVisualizer returned null for type: " << typeName << "\n"
- << "You need to register a visualizer for each type in ArViz/Coin/RegisterVisualizationTypes.cpp";
+ << "CoinElementVisualizer returned null for type: " << typeName
+ << "\n"
+ << "You need to register a visualizer for each type in "
+ "ArViz/Coin/RegisterVisualizationTypes.cpp";
}
}
}
- void CoinVisualizer::addOrUpdateInteraction(CoinLayerID const& layerID, std::string const& elementID,
- data::InteractionDescription const& interactionDesc,
- coin::ElementVisualization* visu)
+ void
+ CoinVisualizer::addOrUpdateInteraction(CoinLayerID const& layerID,
+ std::string const& elementID,
+ data::InteractionDescription const& interactionDesc,
+ coin::ElementVisualization* visu)
{
// Lookup the interaction entry
ElementInteractionData* foundInteraction = nullptr;
for (auto& interaction : elementInteractions)
{
- if (interaction->layer == layerID
- && interaction->element == elementID)
+ if (interaction->layer == layerID && interaction->element == elementID)
{
foundInteraction = interaction.get();
}
@@ -333,8 +350,7 @@ namespace coin
if (foundInteraction == nullptr)
{
// Need to add a new entry
- foundInteraction = elementInteractions.emplace_back(
- new ElementInteractionData).get();
+ foundInteraction = elementInteractions.emplace_back(new ElementInteractionData).get();
}
foundInteraction->layer = layerID;
foundInteraction->element = elementID;
@@ -347,7 +363,8 @@ namespace coin
visu->separator->setName("InteractiveNode");
}
- void CoinVisualizer::removeElementsIfNotUpdated(CoinLayer* layer)
+ void
+ CoinVisualizer::removeElementsIfNotUpdated(CoinLayer* layer)
{
for (auto iter = layer->elements.begin(); iter != layer->elements.end();)
{
@@ -363,11 +380,11 @@ namespace coin
if (userData)
{
// Remove interaction entry if element has been removed
- auto removedInteractionIter = std::find_if(elementInteractions.begin(), elementInteractions.end(),
- [userData](std::unique_ptr<ElementInteractionData> const& entry)
- {
- return entry.get() == userData;
- });
+ auto removedInteractionIter = std::find_if(
+ elementInteractions.begin(),
+ elementInteractions.end(),
+ [userData](std::unique_ptr<ElementInteractionData> const& entry)
+ { return entry.get() == userData; });
ElementInteractionData* removedInteraction = removedInteractionIter->get();
if (removedInteraction == selectedElement)
{
@@ -386,7 +403,8 @@ namespace coin
}
}
- void CoinVisualizer::update()
+ void
+ CoinVisualizer::update()
{
{
std::lock_guard lock(timingMutex);
@@ -438,7 +456,7 @@ namespace coin
updateResult = CoinVisualizerUpdateResult::WAITING;
timing.waitStart = time_start;
storage->begin_pullUpdatesSinceAndSendInteractions(
- currentUpdates.revision, interactionFeedbackBuffer, callback);
+ currentUpdates.revision, interactionFeedbackBuffer, callback);
// Clear interaction feedback buffer after it has been sent
interactionFeedbackBuffer.clear();
@@ -495,23 +513,24 @@ namespace coin
}
break;
}
-
}
- void CoinVisualizer::onUpdateSuccess(const data::LayerUpdates& updates)
+ void
+ CoinVisualizer::onUpdateSuccess(const data::LayerUpdates& updates)
{
pulledUpdates = updates;
updateResult = CoinVisualizerUpdateResult::SUCCESS;
}
- void CoinVisualizer::onUpdateFailure(const Ice::Exception& ex)
+ void
+ CoinVisualizer::onUpdateFailure(const Ice::Exception& ex)
{
- ARMARX_WARNING << "Lost connection to ArVizStorage\n"
- << ex.what();
+ ARMARX_WARNING << "Lost connection to ArVizStorage\n" << ex.what();
updateResult = CoinVisualizerUpdateResult::FAILURE;
}
- void CoinVisualizer::showLayer(CoinLayerID const& id, bool visible)
+ void
+ CoinVisualizer::showLayer(CoinLayerID const& id, bool visible)
{
CoinLayer* layer = layers.findLayer(id);
if (layer == nullptr)
@@ -544,13 +563,15 @@ namespace coin
}
}
- CoinVisualizer_UpdateTiming CoinVisualizer::getTiming()
+ CoinVisualizer_UpdateTiming
+ CoinVisualizer::getTiming()
{
std::lock_guard lock(timingMutex);
return lastTiming;
}
- std::vector<CoinLayerID> CoinVisualizer::getLayerIDs()
+ std::vector<CoinLayerID>
+ CoinVisualizer::getLayerIDs()
{
std::vector<CoinLayerID> result;
result.reserve(layers.data.size());
@@ -561,7 +582,8 @@ namespace coin
return result;
}
- void CoinVisualizer::emitLayersChanged(std::vector<CoinLayerID> const& layerIDs)
+ void
+ CoinVisualizer::emitLayersChanged(std::vector<CoinLayerID> const& layerIDs)
{
if (layersChangedCallback)
{
@@ -569,7 +591,8 @@ namespace coin
}
}
- void CoinVisualizer::emitLayerUpdated(const CoinLayerID& layerID, const CoinLayer& layer)
+ void
+ CoinVisualizer::emitLayerUpdated(const CoinLayerID& layerID, const CoinLayer& layer)
{
for (auto& callback : layerUpdatedCallbacks)
{
@@ -580,7 +603,8 @@ namespace coin
}
}
- void CoinVisualizer::selectElement(int index)
+ void
+ CoinVisualizer::selectElement(int index)
{
if (index >= (int)elementInteractions.size())
{
@@ -593,7 +617,8 @@ namespace coin
selection->select(id->visu->separator);
}
- static ElementInteractionData* findInteractionDataOnPath(SoPath* path)
+ static ElementInteractionData*
+ findInteractionDataOnPath(SoPath* path)
{
// Search for user data in the path
// We stored ElementInteractionData into the user data of the parent SoSeparator
@@ -616,7 +641,8 @@ namespace coin
return static_cast<ElementInteractionData*>(userData);
}
- void CoinVisualizer::onSelectEvent(SoPath* path, int eventType)
+ void
+ CoinVisualizer::onSelectEvent(SoPath* path, int eventType)
{
if (state != CoinVisualizerState::RUNNING)
{
@@ -679,7 +705,6 @@ namespace coin
{
id->visu->switch_->whichChild = SO_SWITCH_NONE;
}
-
}
else
{
@@ -693,13 +718,16 @@ namespace coin
// Now, we should apply the transformation to the original object (at least temporary)
// This avoids flickering, after the object is deselected
SbMatrix manipMatrix;
- manipMatrix.setTransform(1000.0f * manipulator->translation.getValue(), manipulator->rotation.getValue(),
- manipulator->scaleFactor.getValue(), manipulator->scaleOrientation.getValue(),
+ manipMatrix.setTransform(1000.0f * manipulator->translation.getValue(),
+ manipulator->rotation.getValue(),
+ manipulator->scaleFactor.getValue(),
+ manipulator->scaleOrientation.getValue(),
manipulator->center.getValue());
id->visu->transform->multLeft(manipMatrix);
SbVec3f translation = id->visu->transform->translation.getValue();
- ARMARX_IMPORTANT << "Visu translation: " << translation[0] << ", " << translation[1] << ", " << translation[2];
+ ARMARX_IMPORTANT << "Visu translation: " << translation[0] << ", " << translation[1]
+ << ", " << translation[2];
id->visu->switch_->whichChild = SO_SWITCH_ALL;
}
@@ -716,7 +744,8 @@ namespace coin
feedback.revision = pulledUpdates.revision;
}
- SbVec3f translationDueToScaling(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
+ SbVec3f
+ translationDueToScaling(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
{
SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
SbVec3f t_added_rotation_and_scale;
@@ -730,7 +759,8 @@ namespace coin
return t_added_scale;
}
- SbVec3f translationDueToRotation(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
+ SbVec3f
+ translationDueToRotation(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
{
SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
SbVec3f t_added_rotation_and_scale;
@@ -738,14 +768,15 @@ namespace coin
t_added_rotation_and_scale -= t_o;
// Do we need to exclude
-// SbVec3f t_added_rotation;
-// r_m.multVec(t_o, t_added_rotation);
-// t_added_rotation -= t_o;
-// SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
+ // SbVec3f t_added_rotation;
+ // r_m.multVec(t_o, t_added_rotation);
+ // t_added_rotation -= t_o;
+ // SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
return t_added_rotation_and_scale;
}
- Eigen::Matrix4f toEigen(SbMat const& mat)
+ Eigen::Matrix4f
+ toEigen(SbMat const& mat)
{
Eigen::Matrix4f result;
for (int y = 0; y < 4; ++y)
@@ -760,7 +791,8 @@ namespace coin
}
// This should constrain the rotation according to the enabled axes
- static SbRotation constrainRotation(SbRotation input, int enableFlags)
+ static SbRotation
+ constrainRotation(SbRotation input, int enableFlags)
{
SbMatrix mat;
mat.setRotate(input);
@@ -782,16 +814,17 @@ namespace coin
}
// ARMARX_INFO << "rpy after: " << rpy.transpose();
- mat_rot = Eigen::AngleAxisf(rpy(0), Eigen::Vector3f::UnitX())
- * Eigen::AngleAxisf(rpy(1), Eigen::Vector3f::UnitY())
- * Eigen::AngleAxisf(rpy(2), Eigen::Vector3f::UnitZ());
+ mat_rot = Eigen::AngleAxisf(rpy(0), Eigen::Vector3f::UnitX()) *
+ Eigen::AngleAxisf(rpy(1), Eigen::Vector3f::UnitY()) *
+ Eigen::AngleAxisf(rpy(2), Eigen::Vector3f::UnitZ());
Eigen::Quaternionf q(mat_rot);
SbRotation result(q.x(), q.y(), q.z(), q.w());
return result;
}
- static SbVec3f constrainScaling(SbVec3f input, int enableFlags)
+ static SbVec3f
+ constrainScaling(SbVec3f input, int enableFlags)
{
SbVec3f result = input;
if ((enableFlags & data::InteractionEnableFlags::SCALING_X) == 0)
@@ -809,7 +842,8 @@ namespace coin
return result;
}
- void CoinVisualizer::onManipulation(SoDragger* dragger, int eventType)
+ void
+ CoinVisualizer::onManipulation(SoDragger* dragger, int eventType)
{
if (state != CoinVisualizerState::RUNNING)
{
@@ -826,10 +860,10 @@ namespace coin
viz::data::InteractionFeedback* newFeedback = nullptr;
for (viz::data::InteractionFeedback& feedback : interactionFeedbackBuffer)
{
- if ((feedback.type & 0x7) == data::InteractionFeedbackType::TRANSFORM
- && feedback.component == selectedElement->layer.first
- && feedback.layer == selectedElement->layer.second
- && feedback.element == selectedElement->element)
+ if ((feedback.type & 0x7) == data::InteractionFeedbackType::TRANSFORM &&
+ feedback.component == selectedElement->layer.first &&
+ feedback.layer == selectedElement->layer.second &&
+ feedback.element == selectedElement->element)
{
// This is a transform interaction concerning the same element
newFeedback = &feedback;
@@ -939,8 +973,9 @@ namespace coin
scale.e2 = s_m[2];
}
- void CoinVisualizer::exportToVRML(const std::string& exportFilePath)
+ void
+ CoinVisualizer::exportToVRML(const std::string& exportFilePath)
{
coin::exportToVRML(selection, exportFilePath);
}
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index 573e86708b32818db41f866f5e99b739bf44c271..554c2df623d1bb6c747fc0071d0fb1d164a05b3d 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -1,45 +1,41 @@
#pragma once
-#include "ElementVisualizer.h"
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <typeindex>
+
+#include <IceUtil/Shared.h>
#include <RobotAPI/interface/ArViz/Component.h>
+#include "ElementVisualizer.h"
#include <Inventor/manips/SoTransformerManip.h>
#include <Inventor/nodes/SoSelection.h>
#include <Inventor/nodes/SoSeparator.h>
-#include <IceUtil/Shared.h>
-
-#include <functional>
-#include <typeindex>
-#include <memory>
-#include <string>
-#include <mutex>
-
namespace armarx::viz
{
using CoinLayerID = std::pair<std::string, std::string>;
-
struct CoinLayerElement
{
data::ElementPtr data;
std::unique_ptr<coin::ElementVisualization> visu;
};
- inline bool isElementIdLess(CoinLayerElement const& left, CoinLayerElement const& right)
+ inline bool
+ isElementIdLess(CoinLayerElement const& left, CoinLayerElement const& right)
{
return left.data->id < right.data->id;
}
-
struct CoinLayer
{
CoinLayer() = default;
- CoinLayer(CoinLayerID const& id, SoSeparator* node)
- : id(id)
- , node(node)
+ CoinLayer(CoinLayerID const& id, SoSeparator* node) : id(id), node(node)
{
// Increase ref-count, so we can add/remove this node without it being deleted
// This is needed for showing/hiding layers
@@ -47,11 +43,11 @@ namespace armarx::viz
pivot.data = new data::Element;
}
- CoinLayer(CoinLayer&& other)
- : id(std::move(other.id))
- , node(other.node)
- , elements(std::move(other.elements))
- , pivot(std::move(other.pivot))
+ CoinLayer(CoinLayer&& other) :
+ id(std::move(other.id)),
+ node(other.node),
+ elements(std::move(other.elements)),
+ pivot(std::move(other.pivot))
{
other.node = nullptr;
}
@@ -65,9 +61,10 @@ namespace armarx::viz
}
CoinLayer(CoinLayer const&) = delete;
- void operator= (CoinLayer const&) = delete;
+ void operator=(CoinLayer const&) = delete;
- void operator= (CoinLayer&& other)
+ void
+ operator=(CoinLayer&& other)
{
id = std::move(other.id);
node = other.node;
@@ -75,13 +72,15 @@ namespace armarx::viz
elements = std::move(other.elements);
}
- auto lowerBound(std::string const& id)
+ auto
+ lowerBound(std::string const& id)
{
pivot.data->id = id;
return std::lower_bound(elements.begin(), elements.end(), pivot, &isElementIdLess);
}
- CoinLayerElement* findElement(std::string const& id)
+ CoinLayerElement*
+ findElement(std::string const& id)
{
auto iter = lowerBound(id);
if (iter != elements.end() && iter->data->id == id)
@@ -91,7 +90,8 @@ namespace armarx::viz
return nullptr;
}
- CoinLayerElement const* findElement(std::string const& id) const
+ CoinLayerElement const*
+ findElement(std::string const& id) const
{
return const_cast<CoinLayer*>(this)->findElement(id);
}
@@ -102,20 +102,23 @@ namespace armarx::viz
CoinLayerElement pivot;
};
- inline bool isCoinLayerIdLess(CoinLayer const& left, CoinLayer const& right)
+ inline bool
+ isCoinLayerIdLess(CoinLayer const& left, CoinLayer const& right)
{
return left.id < right.id;
}
struct CoinLayerMap
{
- auto lowerBound(CoinLayerID const& id)
+ auto
+ lowerBound(CoinLayerID const& id)
{
pivot.id = id;
return std::lower_bound(data.begin(), data.end(), pivot, &isCoinLayerIdLess);
}
- CoinLayer* findLayer(CoinLayerID const& id)
+ CoinLayer*
+ findLayer(CoinLayerID const& id)
{
auto iter = lowerBound(id);
if (iter != data.end() && iter->id == id)
@@ -125,7 +128,8 @@ namespace armarx::viz
return nullptr;
}
- CoinLayer const* findLayer(CoinLayerID const& id) const
+ CoinLayer const*
+ findLayer(CoinLayerID const& id) const
{
return const_cast<CoinLayerMap*>(this)->findLayer(id);
}
@@ -158,7 +162,8 @@ namespace armarx::viz
IceUtil::Time removeElements = IceUtil::Time::seconds(0);
IceUtil::Time total = IceUtil::Time::seconds(0);
- void add(CoinVisualizer_ApplyTiming const& other)
+ void
+ add(CoinVisualizer_ApplyTiming const& other)
{
addLayer += other.addLayer;
updateElements += other.updateElements;
@@ -217,7 +222,8 @@ namespace armarx::viz
void exportToVRML(std::string const& exportFilePath);
template <typename ElementVisuT>
- void registerVisualizerFor()
+ void
+ registerVisualizerFor()
{
using ElementT = typename ElementVisuT::ElementType;
using VisualizerT = coin::TypedElementVisualizer<ElementVisuT>;
@@ -232,7 +238,8 @@ namespace armarx::viz
CoinLayer& findOrAddLayer(CoinLayerID const& layerID);
void addOrUpdateElements(CoinLayer* layer, data::LayerUpdate const& update);
- void addOrUpdateInteraction(CoinLayerID const& layerID, std::string const& elementID,
+ void addOrUpdateInteraction(CoinLayerID const& layerID,
+ std::string const& elementID,
data::InteractionDescription const& interactionDesc,
coin::ElementVisualization* visu);
void removeElementsIfNotUpdated(CoinLayer* layer);
@@ -280,9 +287,10 @@ namespace armarx::viz
std::function<void(std::vector<CoinLayerID> const&)> layersChangedCallback;
/// A layer's data has changed.
- std::vector<std::function<void(CoinLayerID const& layerID, CoinLayer const& layer)>> layerUpdatedCallbacks;
+ std::vector<std::function<void(CoinLayerID const& layerID, CoinLayer const& layer)>>
+ layerUpdatedCallbacks;
std::mutex timingMutex;
CoinVisualizer_UpdateTiming lastTiming;
};
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
index e52609fa085feac2dc4ef8fc93f68127aef8b040..7b5db2789d7d73f1b17d4d67fa40357724126025 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.cpp
@@ -32,35 +32,39 @@
#include <RobotAPI/components/ArViz/Client/Client.h>
-
namespace armarx
{
- armarx::PropertyDefinitionsPtr ArVizExample::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ ArVizExample::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
+ armarx::PropertyDefinitionsPtr defs(
+ new ComponentPropertyDefinitions(getConfigIdentifier()));
// In this example, this is automatically done by deriving the component
// from armarx::ArVizComponentPluginUser.
if (false)
{
- defs->defineOptionalProperty<std::string>("ArVizTopicName", "ArVizTopic", "Layer updates are sent over this topic.");
+ defs->defineOptionalProperty<std::string>(
+ "ArVizTopicName", "ArVizTopic", "Layer updates are sent over this topic.");
}
- defs->optional(properties.manyLayers, "layers.ManyElements",
- "Show a layer with a lot of elements (used for testing, may impact performance).");
+ defs->optional(
+ properties.manyLayers,
+ "layers.ManyElements",
+ "Show a layer with a lot of elements (used for testing, may impact performance).");
return defs;
}
-
- std::string ArVizExample::getDefaultName() const
+ std::string
+ ArVizExample::getDefaultName() const
{
return "ArVizExample";
}
-
- void ArVizExample::onInitComponent()
+ void
+ ArVizExample::onInitComponent()
{
// In this example, this is automatically done by deriving the component
// from armarx::ArVizComponentPluginUser.
@@ -70,37 +74,37 @@ namespace armarx
}
}
-
- void ArVizExample::onConnectComponent()
+ void
+ ArVizExample::onConnectComponent()
{
task = new RunningTask<ArVizExample>(this, &ArVizExample::run);
task->start();
}
-
- void ArVizExample::onDisconnectComponent()
+ void
+ ArVizExample::onDisconnectComponent()
{
const bool join = true;
task->stop(join);
task = nullptr;
}
-
- void ArVizExample::onExitComponent()
+ void
+ ArVizExample::onExitComponent()
{
}
-
- void fillTestLayer(viz::Layer& layer, double timeInSeconds)
+ void
+ fillTestLayer(viz::Layer& layer, double timeInSeconds)
{
{
Eigen::Vector3f pos = Eigen::Vector3f::Zero();
pos.x() = 100.0f * std::sin(timeInSeconds);
viz::Box box = viz::Box("box")
- .position(pos)
- .color(viz::Color::red())
- .size(Eigen::Vector3f(100.0f, 100.0f, 100.0f));
+ .position(pos)
+ .color(viz::Color::red())
+ .size(Eigen::Vector3f(100.0f, 100.0f, 100.0f));
bool toggleVisibility = (static_cast<int>(timeInSeconds) % 2 == 0);
box.visible(toggleVisibility);
@@ -110,9 +114,9 @@ namespace armarx
{
const float delta = 20. * std::sin(timeInSeconds);
layer.add(viz::Ellipsoid{"ellipsoid"}
- .position(Eigen::Vector3f{0, 100, 150})
- .color(viz::Color::blue())
- .axisLengths(Eigen::Vector3f{70.f + delta, 70.f - delta, 30.f}));
+ .position(Eigen::Vector3f{0, 100, 150})
+ .color(viz::Color::blue())
+ .axisLengths(Eigen::Vector3f{70.f + delta, 70.f - delta, 30.f}));
}
{
Eigen::Vector3f pos = Eigen::Vector3f::Zero();
@@ -120,10 +124,10 @@ namespace armarx
pos.x() = 150.0f;
viz::Cylinder cyl = viz::Cylinder("cylinder")
- .position(pos)
- .color(viz::Color::green())
- .radius(50.0f)
- .height(100.0f);
+ .position(pos)
+ .color(viz::Color::green())
+ .radius(50.0f)
+ .height(100.0f);
layer.add(cyl);
}
@@ -133,9 +137,7 @@ namespace armarx
pos.z() = 100.0f * std::sin(timeInSeconds);
pos.x() = -150.0f;
- viz::Pose pose = viz::Pose("pose")
- .position(pos)
- .scale(1.0f);
+ viz::Pose pose = viz::Pose("pose").position(pos).scale(1.0f);
layer.add(pose);
}
@@ -144,10 +146,10 @@ namespace armarx
pos.z() = +300.0f;
viz::Text text = viz::Text("text")
- .text("Test Text")
- .scale(4.0f)
- .position(pos)
- .color(viz::Color::black());
+ .text("Test Text")
+ .scale(4.0f)
+ .position(pos)
+ .color(viz::Color::black());
layer.add(text);
}
@@ -171,24 +173,24 @@ namespace armarx
}
}
-
- void fillPathsAndLinesLayer(viz::Layer& layer)
+ void
+ fillPathsAndLinesLayer(viz::Layer& layer)
{
Eigen::Vector3f offset = {-800, 0, 500};
- std::vector<Eigen::Vector3f> pathPoints {
- { 0, 0, 0 },
- { -200, 0, 0 },
- { -200, 200, 0 },
+ std::vector<Eigen::Vector3f> pathPoints{
+ {0, 0, 0},
+ {-200, 0, 0},
+ {-200, 200, 0},
};
Eigen::Vector3f additionalPoint = {-200, 200, 200};
// Path: Connected sequence of lines.
{
viz::Path path = viz::Path("path")
- .position(offset)
- .points(pathPoints)
- .color(simox::Color::magenta()).width(10)
- ;
+ .position(offset)
+ .points(pathPoints)
+ .color(simox::Color::magenta())
+ .width(10);
path.addPoint(additionalPoint);
@@ -203,10 +205,11 @@ namespace armarx
for (size_t i = 0; i < pathPoints.size() - 1; i += 2)
{
- viz::Line line = viz::Line("line " + std::to_string(i) + " -> " + std::to_string(i+1))
- .fromTo(pathPoints.at(i) + offset, pathPoints.at(i+1) + offset)
- .color(simox::Color::lime()).lineWidth(10)
- ;
+ viz::Line line =
+ viz::Line("line " + std::to_string(i) + " -> " + std::to_string(i + 1))
+ .fromTo(pathPoints.at(i) + offset, pathPoints.at(i + 1) + offset)
+ .color(simox::Color::lime())
+ .lineWidth(10);
layer.add(line);
}
@@ -218,10 +221,11 @@ namespace armarx
for (size_t i = 0; i < pathPoints.size() - 1; ++i)
{
- layer.add(viz::Cylinder("path cylinder " + std::to_string(i) + " -> " + std::to_string(i+1))
- .fromTo(pathPoints.at(i) + offset, pathPoints.at(i+1) + offset)
- .color(simox::Color::cyan()).radius(10)
- );
+ layer.add(viz::Cylinder("path cylinder " + std::to_string(i) + " -> " +
+ std::to_string(i + 1))
+ .fromTo(pathPoints.at(i) + offset, pathPoints.at(i + 1) + offset)
+ .color(simox::Color::cyan())
+ .radius(10));
}
}
@@ -236,22 +240,23 @@ namespace armarx
if (i < pathPoints.size() - 1)
{
- layer.add(viz::Cylinder("path cylinder with spheres " + std::to_string(i) + " -> " + std::to_string(i+1))
- .fromTo(pathPoints.at(i) + offset, pathPoints.at(i+1) + offset)
- .color(color).radius(radius)
- );
+ layer.add(viz::Cylinder("path cylinder with spheres " + std::to_string(i) +
+ " -> " + std::to_string(i + 1))
+ .fromTo(pathPoints.at(i) + offset, pathPoints.at(i + 1) + offset)
+ .color(color)
+ .radius(radius));
}
layer.add(viz::Sphere("path sphere " + std::to_string(i))
- .position(pathPoints.at(i) + offset)
- .color(color).radius(radius)
- );
+ .position(pathPoints.at(i) + offset)
+ .color(color)
+ .radius(radius));
}
}
}
-
- void fillRobotHandsLayer(viz::Layer& layer)
+ void
+ fillRobotHandsLayer(viz::Layer& layer)
{
Eigen::Vector3f pos = Eigen::Vector3f::Zero();
@@ -263,32 +268,33 @@ namespace armarx
pos.x() = 1500.0f;
pos.y() = i * 200.0f;
- viz::Robot robot = viz::Robot(name)
- .position(pos)
- .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml")
- .overrideColor(simox::Color::green(64 + i * 8));
+ viz::Robot robot =
+ viz::Robot(name)
+ .position(pos)
+ .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-RightHand-v3.xml")
+ .overrideColor(simox::Color::green(64 + i * 8));
layer.add(robot);
}
}
-
- void fillExampleLayer(viz::Layer& layer, double timeInSeconds)
+ void
+ fillExampleLayer(viz::Layer& layer, double timeInSeconds)
{
for (int i = 0; i < 6; ++i)
{
Eigen::Vector3f pos = Eigen::Vector3f(-200.0, 200.0, 300);
- pos.x() += -300 * i;
+ pos.x() += -300 * i;
Eigen::Vector3f normal = Eigen::Vector3f::Zero();
normal(i / 2) = (i % 2 == 0 ? 1.0 : -1.0);
viz::ArrowCircle circle = viz::ArrowCircle("circle " + std::to_string(i))
- .position(pos)
- .radius(100.0f)
- .normal(normal)
- .width(10.0f)
- .color(viz::Color::fromRGBA(255, 0, 255));
+ .position(pos)
+ .radius(100.0f)
+ .normal(normal)
+ .width(10.0f)
+ .color(viz::Color::fromRGBA(255, 0, 255));
float modTime = std::fmod(timeInSeconds, 2.0 * M_PI);
circle.completion(std::sin(modTime));
@@ -300,10 +306,10 @@ namespace armarx
pos.z() = +1000.0f;
viz::Polygon poly = viz::Polygon("poly")
- .position(pos)
- .color(viz::Color::fromRGBA(0, 128, 255, 128))
- .lineColor(viz::Color::fromRGBA(0, 0, 255))
- .lineWidth(1.0f);
+ .position(pos)
+ .color(viz::Color::fromRGBA(0, 128, 255, 128))
+ .lineColor(viz::Color::fromRGBA(0, 0, 255))
+ .lineWidth(1.0f);
float t = 1.0f + std::sin(timeInSeconds);
float offset = 50.0f * t;
@@ -319,9 +325,9 @@ namespace armarx
pos.z() = +1500.0f;
viz::Polygon poly = viz::Polygon("poly2")
- .position(pos)
- .color(viz::Color::fromRGBA(255, 128, 0, 128))
- .lineWidth(0.0f);
+ .position(pos)
+ .color(viz::Color::fromRGBA(255, 128, 0, 128))
+ .lineWidth(0.0f);
float t = 1.0f + std::sin(timeInSeconds);
float offset = 20.0f * t;
@@ -333,8 +339,7 @@ namespace armarx
layer.add(poly);
}
{
- armarx::Vector3f vertices[] =
- {
+ armarx::Vector3f vertices[] = {
{-100.0f, -100.0f, 0.0f},
{-100.0f, +100.0f, 0.0f},
{+100.0f, +100.0f, 0.0f},
@@ -342,23 +347,29 @@ namespace armarx
};
std::size_t verticesSize = sizeof(vertices) / sizeof(vertices[0]);
- armarx::viz::data::Color colors[] =
- {
+ armarx::viz::data::Color colors[] = {
{255, 255, 0, 0},
{255, 0, 255, 0},
{255, 0, 0, 255},
};
std::size_t colorsSize = sizeof(colors) / sizeof(colors[0]);
- viz::data::Face faces[] =
- {
+ viz::data::Face faces[] = {
{
- 0, 1, 2,
- 0, 1, 2,
+ 0,
+ 1,
+ 2,
+ 0,
+ 1,
+ 2,
},
{
- 1, 2, 3,
- 0, 1, 2,
+ 1,
+ 2,
+ 3,
+ 0,
+ 1,
+ 2,
},
};
std::size_t facesSize = sizeof(faces) / sizeof(faces[0]);
@@ -368,10 +379,10 @@ namespace armarx
pos.x() = -500.0f;
viz::Mesh mesh = viz::Mesh("mesh")
- .position(pos)
- .vertices(vertices, verticesSize)
- .colors(colors, colorsSize)
- .faces(faces, facesSize);
+ .position(pos)
+ .vertices(vertices, verticesSize)
+ .colors(colors, colorsSize)
+ .faces(faces, facesSize);
layer.add(mesh);
}
@@ -379,9 +390,8 @@ namespace armarx
Eigen::Vector3f pos = Eigen::Vector3f::Zero();
pos.x() = 500.0f;
- viz::Robot robot = viz::Robot("robot")
- .position(pos)
- .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
+ viz::Robot robot = viz::Robot("robot").position(pos).file(
+ "armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
// Full model
if (true)
@@ -390,8 +400,7 @@ namespace armarx
}
else
{
- robot.useCollisionModel()
- .overrideColor(viz::Color::fromRGBA(0, 255, 128, 128));
+ robot.useCollisionModel().overrideColor(viz::Color::fromRGBA(0, 255, 128, 128));
}
float value = 0.5f * (1.0f + std::sin(timeInSeconds));
@@ -402,23 +411,23 @@ namespace armarx
}
}
-
- void fillPermanentLayer(viz::Layer& layer)
+ void
+ fillPermanentLayer(viz::Layer& layer)
{
viz::Box box = viz::Box("permBox")
- .position(Eigen::Vector3f(2000.0f, 0.0f, 0.0f))
- .size(Eigen::Vector3f(200.0f, 200.0f, 200.0f))
- .color(viz::Color::fromRGBA(255, 165, 0));
+ .position(Eigen::Vector3f(2000.0f, 0.0f, 0.0f))
+ .size(Eigen::Vector3f(200.0f, 200.0f, 200.0f))
+ .color(viz::Color::fromRGBA(255, 165, 0));
layer.add(box);
}
-
- void fillPointsLayer(viz::Layer& layer, double timeInSeconds)
+ void
+ fillPointsLayer(viz::Layer& layer, double timeInSeconds)
{
viz::PointCloud pc = viz::PointCloud("points")
- .position(Eigen::Vector3f(2000.0f, 0.0f, 400.0f))
- .transparency(0.0f);
+ .position(Eigen::Vector3f(2000.0f, 0.0f, 400.0f))
+ .transparency(0.0f);
pc.enable(viz::interaction().selection());
viz::ColoredPoint p;
@@ -442,19 +451,19 @@ namespace armarx
layer.add(pc);
}
-
- void fillObjectsLayer(viz::Layer& layer, double timeInSeconds)
+ void
+ fillObjectsLayer(viz::Layer& layer, double timeInSeconds)
{
{
Eigen::Vector3f pos = Eigen::Vector3f::Zero();
pos.x() = 100.0f * std::sin(timeInSeconds);
pos.y() = 1000.0f;
- viz::Object sponge =
- viz::Object("screwbox")
- .position(pos)
- .file("PriorKnowledgeData",
- "PriorKnowledgeData/objects/Maintenance/bauhaus-screwbox-large/bauhaus-screwbox-large.xml");
+ viz::Object sponge = viz::Object("screwbox")
+ .position(pos)
+ .file("PriorKnowledgeData",
+ "PriorKnowledgeData/objects/Maintenance/"
+ "bauhaus-screwbox-large/bauhaus-screwbox-large.xml");
layer.add(sponge);
layer.add(viz::Pose("screwbox pose").position(pos));
@@ -467,7 +476,7 @@ namespace armarx
Eigen::AngleAxisf orientation(M_PI_2, Eigen::Vector3f::UnitX());
viz::Object spraybottle =
- viz::Object("spraybottle")
+ viz::Object("spraybottle")
.position(pos)
.orientation(Eigen::Quaternionf(orientation))
.file("PriorKnowledgeData",
@@ -478,46 +487,50 @@ namespace armarx
}
}
-
- void fillDisAppearingLayer(viz::Layer& layer, double timeInSeconds)
+ void
+ fillDisAppearingLayer(viz::Layer& layer, double timeInSeconds)
{
int time = int(timeInSeconds);
const Eigen::Vector3f at = {-400, 0, 100};
const Eigen::Vector3f dir = {-150, 0, 0};
- layer.add(viz::Box("box_always")
- .position(at).size(100)
- .color(simox::Color::azure())
- );
+ layer.add(viz::Box("box_always").position(at).size(100).color(simox::Color::azure()));
layer.add(viz::Text("text_seconds")
- .position(at + Eigen::Vector3f(0, 0, 100))
- .orientation(Eigen::AngleAxisf(float(M_PI), Eigen::Vector3f::UnitZ())
- * Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(), - Eigen::Vector3f::UnitY()))
- .text(std::to_string(time % 3))
- .scale(5)
- .color(simox::Color::black())
- );
+ .position(at + Eigen::Vector3f(0, 0, 100))
+ .orientation(Eigen::AngleAxisf(float(M_PI), Eigen::Vector3f::UnitZ()) *
+ Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitZ(),
+ -Eigen::Vector3f::UnitY()))
+ .text(std::to_string(time % 3))
+ .scale(5)
+ .color(simox::Color::black()));
switch (time % 3)
{
case 0:
- layer.add(viz::Sphere("sphere_0_1").position(at + 1.0 * dir).radius(50)
- .color(simox::Color::purple()));
+ layer.add(viz::Sphere("sphere_0_1")
+ .position(at + 1.0 * dir)
+ .radius(50)
+ .color(simox::Color::purple()));
// fallthrough
case 1:
- layer.add(viz::Sphere("sphere_1").position(at + 2.0 * dir).radius(50)
- .color(simox::Color::pink()));
+ layer.add(viz::Sphere("sphere_1")
+ .position(at + 2.0 * dir)
+ .radius(50)
+ .color(simox::Color::pink()));
break;
case 2:
- layer.add(viz::Cylinder("cylinder_2").position(at + 3.0 * dir).radius(50).height(100)
- .color(simox::Color::turquoise()));
+ layer.add(viz::Cylinder("cylinder_2")
+ .position(at + 3.0 * dir)
+ .radius(50)
+ .height(100)
+ .color(simox::Color::turquoise()));
break;
}
}
-
- void fillManyElementsLayer(viz::Layer& layer, double timeInSeconds)
+ void
+ fillManyElementsLayer(viz::Layer& layer, double timeInSeconds)
{
const Eigen::Vector3f at = {-800, 0, 500};
const float size = 5;
@@ -527,7 +540,7 @@ namespace armarx
const float angle = float(2 * M_PI * std::fmod(timeInSeconds, period) / period);
const int num = 10;
- float cf = 1.f / num; // Color factor
+ float cf = 1.f / num; // Color factor
for (int x = 0; x < num; ++x)
{
for (int y = 0; y < num; ++y)
@@ -537,23 +550,25 @@ namespace armarx
std::stringstream ss;
ss << "box_" << x << "_" << y << "_" << z;
layer.add(viz::Box(ss.str())
- .position(at + dist * Eigen::Vector3f(x, y, z))
- .orientation(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()).toRotationMatrix())
- .size(size)
- .color(simox::Color(cf * x, cf * y, cf * z)));
+ .position(at + dist * Eigen::Vector3f(x, y, z))
+ .orientation(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ())
+ .toRotationMatrix())
+ .size(size)
+ .color(simox::Color(cf * x, cf * y, cf * z)));
}
}
}
}
-
- void fillColorMapsLayer(viz::Layer& layer, double timeInSeconds)
+ void
+ fillColorMapsLayer(viz::Layer& layer, double timeInSeconds)
{
- (void) timeInSeconds;
+ (void)timeInSeconds;
namespace E = Eigen;
const E::Vector3f at(-500, -500, 1500);
- const E::Quaternionf ori = E::Quaternionf::FromTwoVectors(E::Vector3f::UnitZ(), - E::Vector3f::UnitY());
+ const E::Quaternionf ori =
+ E::Quaternionf::FromTwoVectors(E::Vector3f::UnitZ(), -E::Vector3f::UnitY());
const E::Vector2f size(200, 20);
const E::Vector2i num(64, 2);
@@ -563,63 +578,67 @@ namespace armarx
std::string const& name = pair.first;
const simox::color::ColorMap& cmap = pair.second;
- Eigen::Vector3f shift(0, 0, - 1.5f * index * size.y());
+ Eigen::Vector3f shift(0, 0, -1.5f * index * size.y());
viz::Mesh mesh = viz::Mesh(name + "_mesh")
- .position(at + shift)
- .orientation(ori)
- .grid2D(size, num, [&cmap](size_t, size_t, const E::Vector3f & p)
- {
- return viz::Color(cmap((p.x() + 100.f) / 200.f));
- });
+ .position(at + shift)
+ .orientation(ori)
+ .grid2D(size,
+ num,
+ [&cmap](size_t, size_t, const E::Vector3f& p)
+ { return viz::Color(cmap((p.x() + 100.f) / 200.f)); });
layer.add(mesh);
- layer.add(viz::Text(name + "_text").text(name)
- .position(at + shift + E::Vector3f(2 + size.x() / 2, -2, 2 - size.y() / 2))
- .orientation(ori).scale(1.5).color(simox::Color::black()));
+ layer.add(
+ viz::Text(name + "_text")
+ .text(name)
+ .position(at + shift + E::Vector3f(2 + size.x() / 2, -2, 2 - size.y() / 2))
+ .orientation(ori)
+ .scale(1.5)
+ .color(simox::Color::black()));
index++;
}
-
}
- void fillInteractionLayer(viz::Layer& layer)
+ void
+ fillInteractionLayer(viz::Layer& layer)
{
// Make box selectable
viz::Box box = viz::Box("box")
- .position(Eigen::Vector3f(2000.0f, 0.0f, 2000.0f))
- .size(Eigen::Vector3f(200.0f, 200.0f, 200.0f))
- .color(viz::Color::fromRGBA(0, 165, 255))
- .scale(2.0f);
+ .position(Eigen::Vector3f(2000.0f, 0.0f, 2000.0f))
+ .size(Eigen::Vector3f(200.0f, 200.0f, 200.0f))
+ .color(viz::Color::fromRGBA(0, 165, 255))
+ .scale(2.0f);
// Enable some interaction possibilities
box.enable(viz::interaction()
- .selection()
- .contextMenu({"First Option", "Second Option", "Third Option"})
- .rotation()
- .translation(viz::AXES_XY)
- .scaling(viz::AXES_XYZ));
+ .selection()
+ .contextMenu({"First Option", "Second Option", "Third Option"})
+ .rotation()
+ .translation(viz::AXES_XY)
+ .scaling(viz::AXES_XYZ));
layer.add(box);
viz::Cylinder cyl = viz::Cylinder("cylinder")
- .position(Eigen::Vector3f(1000.0f, 0.0f, 2000.0f))
- .direction(Eigen::Vector3f::UnitZ())
- .height(200.0f)
- .radius(50.0f)
- .color(viz::Color::fromRGBA(255, 165, 0));
+ .position(Eigen::Vector3f(1000.0f, 0.0f, 2000.0f))
+ .direction(Eigen::Vector3f::UnitZ())
+ .height(200.0f)
+ .radius(50.0f)
+ .color(viz::Color::fromRGBA(255, 165, 0));
// Enable some interaction possibilities
cyl.enable(viz::interaction()
- .selection()
- .contextMenu({"Cyl Option 1", "Cyl Option 2"})
- .rotation()
- .translation(viz::AXES_YZ)
- .scaling());
+ .selection()
+ .contextMenu({"Cyl Option 1", "Cyl Option 2"})
+ .rotation()
+ .translation(viz::AXES_YZ)
+ .scaling());
layer.add(cyl);
}
-
- void ArVizExample::run()
+ void
+ ArVizExample::run()
{
// This example uses the member `arviz` provided by armarx::ArVizComponentPluginUser.
{
@@ -671,8 +690,7 @@ namespace armarx
// Apply the staged commits in a single network call
viz::CommitResult result = arviz.commit(stage);
- ARMARX_INFO << "Permanent layers committed in revision: "
- << result.revision();
+ ARMARX_INFO << "Permanent layers committed in revision: " << result.revision();
CycleUtil c(25);
@@ -709,8 +727,7 @@ namespace armarx
pointsLayer,
objectsLayer,
disAppearingLayer,
- robotHandsLayer
- });
+ robotHandsLayer});
// We can request interaction feedback for specific layers
stage.requestInteraction(interactionLayer);
@@ -722,14 +739,13 @@ namespace armarx
if (interactions.size() > 0)
{
ARMARX_INFO << "We got some interactions: " << interactions.size();
- for (viz::InteractionFeedback const& interaction: interactions)
+ for (viz::InteractionFeedback const& interaction : interactions)
{
if (interaction.type() == viz::InteractionFeedbackType::ContextMenuChosen)
{
- ARMARX_INFO << "[" << interaction.layer()
- << "/" << interaction.element()
- << "] Chosen context menu: "
- << interaction.chosenContextMenuEntry();
+ ARMARX_INFO
+ << "[" << interaction.layer() << "/" << interaction.element()
+ << "] Chosen context menu: " << interaction.chosenContextMenuEntry();
}
else if (interaction.type() == viz::InteractionFeedbackType::Transform)
{
@@ -750,16 +766,14 @@ namespace armarx
{
transformState = "<Unknwon>";
}
- ARMARX_INFO << "[" << interaction.layer()
- << "/" << interaction.element()
- << "] Transformation " << transformState
- << ": \n" << interaction.transformation()
+ ARMARX_INFO << "[" << interaction.layer() << "/" << interaction.element()
+ << "] Transformation " << transformState << ": \n"
+ << interaction.transformation()
<< "\n scale: " << interaction.scale().transpose();
}
else
{
- ARMARX_INFO << "[" << interaction.layer()
- << "/" << interaction.element()
+ ARMARX_INFO << "[" << interaction.layer() << "/" << interaction.element()
<< "] " << toString(interaction.type());
}
}
@@ -769,8 +783,6 @@ namespace armarx
}
}
-
ARMARX_DECOUPLED_REGISTER_COMPONENT(ArVizExample);
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizExample.h b/source/RobotAPI/components/ArViz/Example/ArVizExample.h
index 252a0c6f2221b95872db648f05dcdbdf7d487c5f..9c6e6bdaef82975b28a9d5fb23be474d70b7f17d 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizExample.h
+++ b/source/RobotAPI/components/ArViz/Example/ArVizExample.h
@@ -23,12 +23,10 @@
#pragma once
#include <ArmarXCore/core/Component.h>
-
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-
namespace armarx
{
@@ -77,7 +75,6 @@ namespace armarx
virtual public armarx::ArVizComponentPluginUser
{
public:
-
std::string getDefaultName() const override;
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -89,19 +86,17 @@ namespace armarx
private:
-
void run();
private:
-
RunningTask<ArVizExample>::pointer_type task;
struct Properties
{
bool manyLayers = false;
};
- Properties properties;
+ Properties properties;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp
index aa8282a8e2aa6ef63c62c54dfbaf7d42f2df2dd5..c929a6ec1d19ef9c98dbdc80c88273e4f9a2e798 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp
@@ -20,25 +20,21 @@
* GNU General Public License
*/
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-
-
-#include <ArmarXCore/libraries/DecoupledSingleComponent/DecoupledMain.h>
-#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/DecoupledMain.h>
+
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
namespace armarx
{
struct SingleSlider
{
- SingleSlider(std::string const& name, viz::Color color)
- : box(name)
- , color(color)
+ SingleSlider(std::string const& name, viz::Color color) : box(name), color(color)
{
-
}
viz::Box box;
@@ -46,73 +42,64 @@ namespace armarx
Eigen::Vector3f initial = Eigen::Vector3f::Zero();
Eigen::Vector3f translation = Eigen::Vector3f::Zero();
-
};
struct SlidersState
{
- SlidersState(Eigen::Vector3f origin)
- : origin(origin)
- , x("BoxX", viz::Color::red())
- , y("BoxY", viz::Color::green())
- , z("BoxZ", viz::Color::blue())
- , sphere("Sphere")
+ SlidersState(Eigen::Vector3f origin) :
+ origin(origin),
+ x("BoxX", viz::Color::red()),
+ y("BoxY", viz::Color::green()),
+ z("BoxZ", viz::Color::blue()),
+ sphere("Sphere")
{
float boxSize = 50.0f;
x.initial = origin + Eigen::Vector3f(0.5f * ARROW_LENGTH, 0.0f, 0.0f);
- x.box.position(x.initial)
- .color(x.color)
- .size(boxSize)
- .enable(viz::interaction()
- .translation(viz::AXES_X)
- .hideDuringTransform());
+ x.box.position(x.initial).color(x.color).size(boxSize).enable(
+ viz::interaction().translation(viz::AXES_X).hideDuringTransform());
y.initial = origin + Eigen::Vector3f(0.0f, 0.5f * ARROW_LENGTH, 0.0f);
- y.box.position(y.initial)
- .color(y.color)
- .size(boxSize)
- .enable(viz::interaction()
- .translation(viz::AXES_Y)
- .hideDuringTransform());
+ y.box.position(y.initial).color(y.color).size(boxSize).enable(
+ viz::interaction().translation(viz::AXES_Y).hideDuringTransform());
z.initial = origin + Eigen::Vector3f(0.0f, 0.0f, 0.5f * ARROW_LENGTH);
- z.box.position(z.initial)
- .color(z.color)
- .size(boxSize)
- .enable(viz::interaction()
- .translation(viz::AXES_Z)
- .hideDuringTransform());
+ z.box.position(z.initial).color(z.color).size(boxSize).enable(
+ viz::interaction().translation(viz::AXES_Z).hideDuringTransform());
sphere.position(origin + 0.5f * ARROW_LENGTH * Eigen::Vector3f(1.0f, 1.0f, 1.0f))
- .color(viz::Color::orange())
- .radius(30.0f);
+ .color(viz::Color::orange())
+ .radius(30.0f);
}
static constexpr const float ARROW_LENGTH = 1000.0f;
- void visualize(viz::Client& arviz)
+ void
+ visualize(viz::Client& arviz)
{
layerInteract = arviz.layer("Sliders");
float arrowWidth = 10.0f;
- viz::Arrow arrowX = viz::Arrow("ArrowX")
- .color(viz::Color::red())
- .fromTo(origin, origin + Eigen::Vector3f(ARROW_LENGTH, 0.0f, 0.0f))
- .width(arrowWidth);
+ viz::Arrow arrowX =
+ viz::Arrow("ArrowX")
+ .color(viz::Color::red())
+ .fromTo(origin, origin + Eigen::Vector3f(ARROW_LENGTH, 0.0f, 0.0f))
+ .width(arrowWidth);
layerInteract.add(arrowX);
- viz::Arrow arrowY = viz::Arrow("ArrowY")
- .color(viz::Color::green())
- .fromTo(origin, origin + Eigen::Vector3f(0.0f, ARROW_LENGTH, 0.0f))
- .width(arrowWidth);
+ viz::Arrow arrowY =
+ viz::Arrow("ArrowY")
+ .color(viz::Color::green())
+ .fromTo(origin, origin + Eigen::Vector3f(0.0f, ARROW_LENGTH, 0.0f))
+ .width(arrowWidth);
layerInteract.add(arrowY);
- viz::Arrow arrowZ = viz::Arrow("ArrowZ")
- .color(viz::Color::blue())
- .fromTo(origin, origin + Eigen::Vector3f(0.0f, 0.0f, ARROW_LENGTH))
- .width(arrowWidth);
+ viz::Arrow arrowZ =
+ viz::Arrow("ArrowZ")
+ .color(viz::Color::blue())
+ .fromTo(origin, origin + Eigen::Vector3f(0.0f, 0.0f, ARROW_LENGTH))
+ .width(arrowWidth);
layerInteract.add(arrowZ);
@@ -124,8 +111,8 @@ namespace armarx
layerResult.add(sphere);
}
- void handle(viz::InteractionFeedback const& interaction,
- viz::StagedCommit* stage)
+ void
+ handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage)
{
std::string const& element = interaction.element();
Eigen::Matrix4f transform = interaction.transformation();
@@ -152,43 +139,43 @@ namespace armarx
switch (interaction.type())
{
- case viz::InteractionFeedbackType::Transform:
- {
- slider->translation = translation;
+ case viz::InteractionFeedbackType::Transform:
+ {
+ slider->translation = translation;
- Eigen::Vector3f spherePosition(
- x.initial.x() + x.translation.x(),
- y.initial.y() + y.translation.y(),
- z.initial.z() + z.translation.z());
- sphere.position(spherePosition);
+ Eigen::Vector3f spherePosition(x.initial.x() + x.translation.x(),
+ y.initial.y() + y.translation.y(),
+ z.initial.z() + z.translation.z());
+ sphere.position(spherePosition);
- stage->add(layerResult);
- } break;
+ stage->add(layerResult);
+ }
+ break;
- case viz::InteractionFeedbackType::Select:
- {
- // Do nothing
- } break;
+ case viz::InteractionFeedbackType::Select:
+ {
+ // Do nothing
+ }
+ break;
- case viz::InteractionFeedbackType::Deselect:
- {
- // If an object is deselected, we apply the transformation
- slider->initial = slider->initial + slider->translation;
- slider->translation = Eigen::Vector3f::Zero();
- ARMARX_IMPORTANT << "Setting position to "
- << slider->initial.transpose();
- slider->box.position(slider->initial);
+ case viz::InteractionFeedbackType::Deselect:
+ {
+ // If an object is deselected, we apply the transformation
+ slider->initial = slider->initial + slider->translation;
+ slider->translation = Eigen::Vector3f::Zero();
+ ARMARX_IMPORTANT << "Setting position to " << slider->initial.transpose();
+ slider->box.position(slider->initial);
- stage->add(layerInteract);
- } break;
+ stage->add(layerInteract);
+ }
+ break;
- default:
- {
- // Do nothing for the other interaction types
- } break;
+ default:
+ {
+ // Do nothing for the other interaction types
+ }
+ break;
}
-
-
}
Eigen::Vector3f origin;
@@ -202,7 +189,6 @@ namespace armarx
viz::Layer layerResult;
};
-
// ---------------
// What abstractions are needed?
@@ -210,16 +196,10 @@ namespace armarx
// SpawnerElement
-
-
struct SlidersState2
{
- SlidersState2(Eigen::Vector3f origin)
- : origin(origin)
- , x("BoxX")
- , y("BoxY")
- , z("BoxZ")
- , sphere("Sphere")
+ SlidersState2(Eigen::Vector3f origin) :
+ origin(origin), x("BoxX"), y("BoxY"), z("BoxZ"), sphere("Sphere")
{
float boxSize = 50.0f;
@@ -231,49 +211,50 @@ namespace armarx
x.enable(viz::interaction().translation(viz::AXES_X));
// Other attributes of the element can be set directly on the member
- x.element.color(viz::Color::red())
- .size(boxSize);
+ x.element.color(viz::Color::red()).size(boxSize);
y.position(origin + Eigen::Vector3f(0.0f, 0.5f * ARROW_LENGTH, 0.0f));
y.enable(viz::interaction().translation(viz::AXES_Y));
- y.element.color(viz::Color::green())
- .size(boxSize);
+ y.element.color(viz::Color::green()).size(boxSize);
z.position(origin + Eigen::Vector3f(0.0f, 0.0f, 0.5f * ARROW_LENGTH));
z.enable(viz::interaction().translation(viz::AXES_Z));
- z.element.color(viz::Color::blue())
- .size(boxSize);
+ z.element.color(viz::Color::blue()).size(boxSize);
sphere.position(origin + 0.5f * ARROW_LENGTH * Eigen::Vector3f(1.0f, 1.0f, 1.0f))
- .color(viz::Color::orange())
- .radius(30.0f);
+ .color(viz::Color::orange())
+ .radius(30.0f);
}
static constexpr const float ARROW_LENGTH = 1000.0f;
- void visualize(viz::Client& arviz)
+ void
+ visualize(viz::Client& arviz)
{
layerInteract = arviz.layer("Sliders2");
float arrowWidth = 10.0f;
- viz::Arrow arrowX = viz::Arrow("ArrowX")
- .color(viz::Color::red())
- .fromTo(origin, origin + Eigen::Vector3f(ARROW_LENGTH, 0.0f, 0.0f))
- .width(arrowWidth);
+ viz::Arrow arrowX =
+ viz::Arrow("ArrowX")
+ .color(viz::Color::red())
+ .fromTo(origin, origin + Eigen::Vector3f(ARROW_LENGTH, 0.0f, 0.0f))
+ .width(arrowWidth);
layerInteract.add(arrowX);
- viz::Arrow arrowY = viz::Arrow("ArrowY")
- .color(viz::Color::green())
- .fromTo(origin, origin + Eigen::Vector3f(0.0f, ARROW_LENGTH, 0.0f))
- .width(arrowWidth);
+ viz::Arrow arrowY =
+ viz::Arrow("ArrowY")
+ .color(viz::Color::green())
+ .fromTo(origin, origin + Eigen::Vector3f(0.0f, ARROW_LENGTH, 0.0f))
+ .width(arrowWidth);
layerInteract.add(arrowY);
- viz::Arrow arrowZ = viz::Arrow("ArrowZ")
- .color(viz::Color::blue())
- .fromTo(origin, origin + Eigen::Vector3f(0.0f, 0.0f, ARROW_LENGTH))
- .width(arrowWidth);
+ viz::Arrow arrowZ =
+ viz::Arrow("ArrowZ")
+ .color(viz::Color::blue())
+ .fromTo(origin, origin + Eigen::Vector3f(0.0f, 0.0f, ARROW_LENGTH))
+ .width(arrowWidth);
layerInteract.add(arrowZ);
@@ -285,8 +266,8 @@ namespace armarx
layerResult.add(sphere);
}
- void handle(viz::InteractionFeedback const& interaction,
- viz::StagedCommit* stage)
+ void
+ handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage)
{
// Let the Transformable<T> handle all events internally
bool needsLayerUpdate = false;
@@ -312,10 +293,9 @@ namespace armarx
{
// We can query getCurrentPose() to determine the poses of the sliders
// with the transformation applied to them
- Eigen::Vector3f spherePosition(
- x.getCurrentPose().col(3).x(),
- y.getCurrentPose().col(3).y(),
- z.getCurrentPose().col(3).z());
+ Eigen::Vector3f spherePosition(x.getCurrentPose().col(3).x(),
+ y.getCurrentPose().col(3).y(),
+ z.getCurrentPose().col(3).z());
sphere.position(spherePosition);
stage->add(layerResult);
@@ -356,42 +336,43 @@ namespace armarx
float size = 100.0f;
viz::Color color = viz::Color::black();
- void visualize(int i, viz::Layer& layer)
+ void
+ visualize(int i, viz::Layer& layer)
{
- viz::InteractionDescription interaction = viz::interaction()
- .selection().transform().scaling()
- .contextMenu({"Delete All", "Delete All of Type"});
+ viz::InteractionDescription interaction =
+ viz::interaction().selection().transform().scaling().contextMenu(
+ {"Delete All", "Delete All of Type"});
std::string name = "Spawner_" + std::to_string(i);
switch (type)
{
- case SpawnerType::Box:
- {
- viz::Box box = viz::Box(name)
- .position(position)
- .size(size)
- .color(color)
- .enable(interaction);
- layer.add(box);
- } break;
- case SpawnerType::Cylinder:
- {
- viz::Cylinder cylinder = viz::Cylinder(name)
- .position(position)
- .radius(size*0.5f)
- .height(size)
- .color(color)
- .enable(interaction);
- layer.add(cylinder);
- } break;
- case SpawnerType::Sphere:
- {
- viz::Sphere sphere = viz::Sphere(name)
- .position(position)
- .radius(size*0.5f)
- .color(color)
- .enable(interaction);
- layer.add(sphere);
- } break;
+ case SpawnerType::Box:
+ {
+ viz::Box box = viz::Box(name).position(position).size(size).color(color).enable(
+ interaction);
+ layer.add(box);
+ }
+ break;
+ case SpawnerType::Cylinder:
+ {
+ viz::Cylinder cylinder = viz::Cylinder(name)
+ .position(position)
+ .radius(size * 0.5f)
+ .height(size)
+ .color(color)
+ .enable(interaction);
+ layer.add(cylinder);
+ }
+ break;
+ case SpawnerType::Sphere:
+ {
+ viz::Sphere sphere = viz::Sphere(name)
+ .position(position)
+ .radius(size * 0.5f)
+ .color(color)
+ .enable(interaction);
+ layer.add(sphere);
+ }
+ break;
}
}
};
@@ -403,7 +384,8 @@ namespace armarx
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
Eigen::Vector3f scale = Eigen::Vector3f::Ones();
- void visualize(viz::Layer& layer)
+ void
+ visualize(viz::Layer& layer)
{
if (source == nullptr)
{
@@ -411,9 +393,8 @@ namespace armarx
return;
}
- viz::InteractionDescription interaction = viz::interaction()
- .selection()
- .contextMenu({"Delete"});
+ viz::InteractionDescription interaction =
+ viz::interaction().selection().contextMenu({"Delete"});
std::string name = "Object_" + std::to_string(index);
Eigen::Matrix4f initial = Eigen::Matrix4f::Identity();
@@ -422,45 +403,47 @@ namespace armarx
switch (source->type)
{
- case SpawnerType::Box:
- {
- viz::Box box = viz::Box(name)
- .pose(pose)
- .scale(scale)
- .size(source->size)
- .color(source->color)
- .enable(interaction);
- layer.add(box);
- } break;
- case SpawnerType::Cylinder:
- {
- viz::Cylinder cylinder = viz::Cylinder(name)
- .pose(pose)
- .scale(scale)
- .radius(source->size * 0.5f)
- .height(source->size)
- .color(source->color)
- .enable(interaction);
- layer.add(cylinder);
- } break;
- case SpawnerType::Sphere:
- {
- viz::Sphere sphere = viz::Sphere(name)
- .pose(pose)
- .scale(scale)
- .radius(source->size * 0.5f)
- .color(source->color)
- .enable(interaction);
- layer.add(sphere);
- } break;
+ case SpawnerType::Box:
+ {
+ viz::Box box = viz::Box(name)
+ .pose(pose)
+ .scale(scale)
+ .size(source->size)
+ .color(source->color)
+ .enable(interaction);
+ layer.add(box);
+ }
+ break;
+ case SpawnerType::Cylinder:
+ {
+ viz::Cylinder cylinder = viz::Cylinder(name)
+ .pose(pose)
+ .scale(scale)
+ .radius(source->size * 0.5f)
+ .height(source->size)
+ .color(source->color)
+ .enable(interaction);
+ layer.add(cylinder);
+ }
+ break;
+ case SpawnerType::Sphere:
+ {
+ viz::Sphere sphere = viz::Sphere(name)
+ .pose(pose)
+ .scale(scale)
+ .radius(source->size * 0.5f)
+ .color(source->color)
+ .enable(interaction);
+ layer.add(sphere);
+ }
+ break;
}
}
};
struct SpawnersState
{
- SpawnersState(Eigen::Vector3f origin)
- : origin(origin)
+ SpawnersState(Eigen::Vector3f origin) : origin(origin)
{
float size = 100.0f;
{
@@ -483,12 +466,13 @@ namespace armarx
}
}
- void visualize(viz::Client& arviz)
+ void
+ visualize(viz::Client& arviz)
{
layerSpawners = arviz.layer("Spawners");
int index = 0;
- for (Spawner& spawner: spawners)
+ for (Spawner& spawner : spawners)
{
spawner.visualize(index, layerSpawners);
index += 1;
@@ -497,7 +481,8 @@ namespace armarx
layerObjects = arviz.layer("SpawnedObjects");
}
- void visualizeSpawnedObjects(viz::StagedCommit* stage)
+ void
+ visualizeSpawnedObjects(viz::StagedCommit* stage)
{
layerObjects.clear();
for (auto& object : objects)
@@ -507,8 +492,8 @@ namespace armarx
stage->add(layerObjects);
}
- void handle(viz::InteractionFeedback const& interaction,
- viz::StagedCommit* stage)
+ void
+ handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage)
{
Spawner* spawner = nullptr;
for (int i = 0; i < (int)spawners.size(); ++i)
@@ -524,17 +509,19 @@ namespace armarx
{
ARMARX_INFO << "No spawner" << interaction.element();
// A spawned object was selected instead of a spawner
- if (interaction.type() == viz::InteractionFeedbackType::ContextMenuChosen
- && interaction.chosenContextMenuEntry() == 0)
+ if (interaction.type() == viz::InteractionFeedbackType::ContextMenuChosen &&
+ interaction.chosenContextMenuEntry() == 0)
{
// The delete context menu option was chosen
// So we remove the object from the internal list and redraw
- auto newEnd = std::remove_if(objects.begin(), objects.end(),
- [&interaction](SpawnedObject const& object)
- {
- std::string name = "Object_" + std::to_string(object.index);
- return interaction.element() == name;
- });
+ auto newEnd = std::remove_if(objects.begin(),
+ objects.end(),
+ [&interaction](SpawnedObject const& object)
+ {
+ std::string name =
+ "Object_" + std::to_string(object.index);
+ return interaction.element() == name;
+ });
objects.erase(newEnd, objects.end());
visualizeSpawnedObjects(stage);
@@ -544,68 +531,73 @@ namespace armarx
switch (interaction.type())
{
- case viz::InteractionFeedbackType::Select:
- {
- // Create a spawned object
- spawnedObject.index = spawnedObjectCounter++;
- spawnedObject.source = spawner;
- spawnedObject.transform = Eigen::Matrix4f::Identity();
- spawnedObject.scale.setOnes();
- } break;
-
- case viz::InteractionFeedbackType::Transform:
- {
- // Update state of spawned object
- spawnedObject.transform = interaction.transformation();
- spawnedObject.scale = interaction.scale();
- if (interaction.isTransformBegin())
+ case viz::InteractionFeedbackType::Select:
{
- // Visualize all other objects except the currently spawned one
- visualizeSpawnedObjects(stage);
+ // Create a spawned object
+ spawnedObject.index = spawnedObjectCounter++;
+ spawnedObject.source = spawner;
+ spawnedObject.transform = Eigen::Matrix4f::Identity();
+ spawnedObject.scale.setOnes();
}
- if (interaction.isTransformEnd())
+ break;
+
+ case viz::InteractionFeedbackType::Transform:
{
- spawnedObject.visualize(layerObjects);
- stage->add(layerObjects);
+ // Update state of spawned object
+ spawnedObject.transform = interaction.transformation();
+ spawnedObject.scale = interaction.scale();
+ if (interaction.isTransformBegin())
+ {
+ // Visualize all other objects except the currently spawned one
+ visualizeSpawnedObjects(stage);
+ }
+ if (interaction.isTransformEnd())
+ {
+ spawnedObject.visualize(layerObjects);
+ stage->add(layerObjects);
+ }
}
- } break;
-
- case viz::InteractionFeedbackType::Deselect:
- {
- // Save state of spawned object
- objects.push_back(spawnedObject);
- } break;
+ break;
- case viz::InteractionFeedbackType::ContextMenuChosen:
- {
- SpawnerOption option = (SpawnerOption)(interaction.chosenContextMenuEntry());
- switch (option)
- {
- case SpawnerOption::DeleteAll:
+ case viz::InteractionFeedbackType::Deselect:
{
- objects.clear();
- layerObjects.clear();
+ // Save state of spawned object
+ objects.push_back(spawnedObject);
+ }
+ break;
- stage->add(layerObjects);
- } break;
- case SpawnerOption::DeleteType:
+ case viz::InteractionFeedbackType::ContextMenuChosen:
{
- auto newEnd = std::remove_if(objects.begin(), objects.end(),
- [spawner](SpawnedObject const& obj)
+ SpawnerOption option = (SpawnerOption)(interaction.chosenContextMenuEntry());
+ switch (option)
{
- return obj.source == spawner;
- });
- objects.erase(newEnd, objects.end());
-
- visualizeSpawnedObjects(stage);
- } break;
+ case SpawnerOption::DeleteAll:
+ {
+ objects.clear();
+ layerObjects.clear();
+
+ stage->add(layerObjects);
+ }
+ break;
+ case SpawnerOption::DeleteType:
+ {
+ auto newEnd = std::remove_if(objects.begin(),
+ objects.end(),
+ [spawner](SpawnedObject const& obj)
+ { return obj.source == spawner; });
+ objects.erase(newEnd, objects.end());
+
+ visualizeSpawnedObjects(stage);
+ }
+ break;
+ }
}
- }
- default:
- {
- // Ignore other interaction types
- } break;
+ default:
+ {
+ // Ignore other interaction types
+ }
+ break;
}
}
@@ -664,38 +656,47 @@ namespace armarx
// and provides a ready-to-use ArViz client called `arviz`.
virtual armarx::ArVizComponentPluginUser
{
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "ArVizInteractExample";
}
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
+ armarx::PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
+ armarx::PropertyDefinitionsPtr defs(
+ new ComponentPropertyDefinitions(getConfigIdentifier()));
return defs;
}
- void onInitComponent() override
- { }
+ void
+ onInitComponent() override
+ {
+ }
- void onConnectComponent() override
+ void
+ onConnectComponent() override
{
task = new RunningTask<ArVizInteractExample>(this, &ArVizInteractExample::run);
task->start();
}
- void onDisconnectComponent() override
+ void
+ onDisconnectComponent() override
{
const bool join = true;
task->stop(join);
task = nullptr;
}
- void onExitComponent() override
- { }
-
+ void
+ onExitComponent() override
+ {
+ }
- void run()
+ void
+ run()
{
viz::StagedCommit stage;
@@ -705,14 +706,16 @@ namespace armarx
Eigen::Vector3f origin3(-2000.0f, -2000.0f, 0.0f);
Eigen::Vector3f origin4(0.0f, -2000.0f, 0.0f);
{
- viz::Cylinder separatorX = viz::Cylinder("SeparatorX")
- .fromTo(origin1, origin1 + 4000.0f * Eigen::Vector3f::UnitX())
- .radius(5.0f);
+ viz::Cylinder separatorX =
+ viz::Cylinder("SeparatorX")
+ .fromTo(origin1, origin1 + 4000.0f * Eigen::Vector3f::UnitX())
+ .radius(5.0f);
regions.add(separatorX);
- viz::Cylinder separatorY = viz::Cylinder("SeparatorY")
- .fromTo(origin4, origin4 + 4000.0f * Eigen::Vector3f::UnitY())
- .radius(5.0f);
+ viz::Cylinder separatorY =
+ viz::Cylinder("SeparatorY")
+ .fromTo(origin4, origin4 + 4000.0f * Eigen::Vector3f::UnitY())
+ .radius(5.0f);
regions.add(separatorY);
stage.add(regions);
@@ -762,7 +765,8 @@ namespace armarx
{
sliders2.handle(interaction, &stage);
}
- if (interaction.layer() == "Spawners" || interaction.layer() == "SpawnedObjects")
+ if (interaction.layer() == "Spawners" ||
+ interaction.layer() == "SpawnedObjects")
{
spawners.handle(interaction, &stage);
}
@@ -773,14 +777,13 @@ namespace armarx
}
RunningTask<ArVizInteractExample>::pointer_type task;
-
};
ARMARX_DECOUPLED_REGISTER_COMPONENT(ArVizInteractExample);
-}
+} // namespace armarx
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
return armarx::DecoupledMain(argc, argv);
}
-
diff --git a/source/RobotAPI/components/ArViz/Example/main.cpp b/source/RobotAPI/components/ArViz/Example/main.cpp
index 39a144f6892a546c18168eea8d5878d0c9a521c6..d9ae379edc3523de41f436843653073214734541 100644
--- a/source/RobotAPI/components/ArViz/Example/main.cpp
+++ b/source/RobotAPI/components/ArViz/Example/main.cpp
@@ -1,6 +1,7 @@
#include <ArmarXCore/libraries/DecoupledSingleComponent/DecoupledMain.h>
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
return armarx::DecoupledMain(argc, argv);
}
diff --git a/source/RobotAPI/components/ArViz/IceConversions.h b/source/RobotAPI/components/ArViz/IceConversions.h
index 4c1dc54f449c9a6f6d455a9c6d1ade1627e08a34..d73d435dedc728fcbe0de383a6ae267bb262e439 100644
--- a/source/RobotAPI/components/ArViz/IceConversions.h
+++ b/source/RobotAPI/components/ArViz/IceConversions.h
@@ -1,23 +1,26 @@
#pragma once
-#include <RobotAPI/interface/ArViz/Elements.h>
-
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <RobotAPI/interface/ArViz/Elements.h>
+
namespace armarx::viz
{
- inline Eigen::Matrix4f toEigen(data::GlobalPose const& pose)
+ inline Eigen::Matrix4f
+ toEigen(data::GlobalPose const& pose)
{
Eigen::Matrix4f result;
- result.block<3, 3>(0, 0) = Eigen::Quaternionf(pose.qw, pose.qx, pose.qy, pose.qz).toRotationMatrix();
+ result.block<3, 3>(0, 0) =
+ Eigen::Quaternionf(pose.qw, pose.qx, pose.qy, pose.qz).toRotationMatrix();
result.block<3, 1>(0, 3) = Eigen::Vector3f(pose.x, pose.y, pose.z);
result.block<1, 4>(3, 0) = Eigen::Vector4f(0.0f, 0.0f, 0.0f, 1.0f);
return result;
}
- inline data::GlobalPose poseToIce(Eigen::Vector3f const& pos, Eigen::Quaternionf const& ori)
+ inline data::GlobalPose
+ poseToIce(Eigen::Vector3f const& pos, Eigen::Quaternionf const& ori)
{
data::GlobalPose result;
result.x = pos.x();
@@ -30,4 +33,4 @@ namespace armarx::viz
return result;
}
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.cpp b/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.cpp
index 1a5e2ef4bbfc3af359fe47797abd6e588cf6011b..bb9f25c74532cf2e3f0b7700886c2d7a81a47194 100644
--- a/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.cpp
@@ -1,9 +1,9 @@
#include "ElementJsonSerializers.h"
-
namespace armarx::viz
{
- std::string json::getTypeName(const nlohmann::json& j, const std::string& key)
+ std::string
+ json::getTypeName(const nlohmann::json& j, const std::string& key)
{
try
{
@@ -19,7 +19,8 @@ namespace armarx::viz
}
}
- void json::setTypeName(nlohmann::json& j, const std::string& key, const std::string& typeName)
+ void
+ json::setTypeName(nlohmann::json& j, const std::string& key, const std::string& typeName)
{
if (j.count(key) > 0)
{
@@ -28,35 +29,36 @@ namespace armarx::viz
j[key] = typeName;
}
- void data::to_json(nlohmann::json& j, const Element& element)
+ void
+ data::to_json(nlohmann::json& j, const Element& element)
{
json::ElementJsonSerializers::to_json(j, element);
}
- void data::from_json(const nlohmann::json& j, Element& element)
+ void
+ data::from_json(const nlohmann::json& j, Element& element)
{
json::ElementJsonSerializers::from_json(j, element);
}
- void data::to_json(nlohmann::json& j, const ElementPtr& shapePtr)
+ void
+ data::to_json(nlohmann::json& j, const ElementPtr& shapePtr)
{
json::ElementJsonSerializers::to_json(j, shapePtr.get());
}
- void data::from_json(const nlohmann::json& j, ElementPtr& shapePtr)
+ void
+ data::from_json(const nlohmann::json& j, ElementPtr& shapePtr)
{
json::ElementJsonSerializers::from_json(j, shapePtr);
}
- void data::to_json(nlohmann::json& j, const Element* shapePtr)
+ void
+ data::to_json(nlohmann::json& j, const Element* shapePtr)
{
json::ElementJsonSerializers::to_json(j, shapePtr);
}
-}
-
-
-
-
+} // namespace armarx::viz
namespace armarx::viz::json
{
@@ -65,53 +67,66 @@ namespace armarx::viz::json
ElementJsonSerializers ElementJsonSerializers::_instance = {};
-
- void ElementJsonSerializers::ElementJsonSerializer::to_json(nlohmann::json& j, const data::Element& element)
+ void
+ ElementJsonSerializers::ElementJsonSerializer::to_json(nlohmann::json& j,
+ const data::Element& element)
{
_to_json(j, element);
}
- void ElementJsonSerializers::ElementJsonSerializer::from_json(const nlohmann::json& j, data::Element& element)
+ void
+ ElementJsonSerializers::ElementJsonSerializer::from_json(const nlohmann::json& j,
+ data::Element& element)
{
_from_json(j, element);
}
- void ElementJsonSerializers::ElementJsonSerializer::to_json(nlohmann::json& j, const data::ElementPtr& shapePtr)
+ void
+ ElementJsonSerializers::ElementJsonSerializer::to_json(nlohmann::json& j,
+ const data::ElementPtr& shapePtr)
{
_to_json(j, *shapePtr);
}
- void ElementJsonSerializers::ElementJsonSerializer::from_json(const nlohmann::json& j, data::ElementPtr& shapePtr)
+ void
+ ElementJsonSerializers::ElementJsonSerializer::from_json(const nlohmann::json& j,
+ data::ElementPtr& shapePtr)
{
_from_json_ptr(j, shapePtr);
}
- ElementJsonSerializers::ElementJsonSerializer& ElementJsonSerializers::getSerializer(const nlohmann::json& j)
+ ElementJsonSerializers::ElementJsonSerializer&
+ ElementJsonSerializers::getSerializer(const nlohmann::json& j)
{
return _instance._getSerializer(getTypeName(j));
}
- ElementJsonSerializers::ElementJsonSerializer& ElementJsonSerializers::getSerializer(const std::string& demangledTypeName)
+ ElementJsonSerializers::ElementJsonSerializer&
+ ElementJsonSerializers::getSerializer(const std::string& demangledTypeName)
{
return _instance._getSerializer(demangledTypeName);
}
- std::vector<std::string> ElementJsonSerializers::getRegisteredTypes()
+ std::vector<std::string>
+ ElementJsonSerializers::getRegisteredTypes()
{
return _instance._getRegisteredTypes();
}
- bool ElementJsonSerializers::isTypeRegistered(const std::string& typeName)
+ bool
+ ElementJsonSerializers::isTypeRegistered(const std::string& typeName)
{
return _instance._isTypeRegistered(typeName);
}
- void ElementJsonSerializers::to_json(nlohmann::json& j, const data::Element& element)
+ void
+ ElementJsonSerializers::to_json(nlohmann::json& j, const data::Element& element)
{
return getSerializer(simox::meta::get_type_name(element)).to_json(j, element);
}
- void ElementJsonSerializers::from_json(const nlohmann::json& j, data::Element& element)
+ void
+ ElementJsonSerializers::from_json(const nlohmann::json& j, data::Element& element)
{
const std::string typeName = getTypeName(j);
if (typeName != simox::meta::get_type_name(element))
@@ -121,21 +136,25 @@ namespace armarx::viz::json
getSerializer(typeName).from_json(j, element);
}
- void ElementJsonSerializers::to_json(nlohmann::json& j, const data::Element* elementPtr)
+ void
+ ElementJsonSerializers::to_json(nlohmann::json& j, const data::Element* elementPtr)
{
if (!elementPtr)
{
- throw error::ArvizReflectionError("data::Element* is null in ElementJsonSerializers::to_json().");
+ throw error::ArvizReflectionError(
+ "data::Element* is null in ElementJsonSerializers::to_json().");
}
to_json(j, *elementPtr);
}
- void ElementJsonSerializers::from_json(const nlohmann::json& j, data::ElementPtr& shapePtr)
+ void
+ ElementJsonSerializers::from_json(const nlohmann::json& j, data::ElementPtr& shapePtr)
{
getSerializer(j).from_json(j, shapePtr);
}
- std::string ElementJsonSerializers::getTypeName(const nlohmann::json& j)
+ std::string
+ ElementJsonSerializers::getTypeName(const nlohmann::json& j)
{
return json::getTypeName(j, JSON_TYPE_NAME_KEY);
}
@@ -145,9 +164,8 @@ namespace armarx::viz::json
registerElements();
}
-
- ElementJsonSerializers::ElementJsonSerializer& ElementJsonSerializers::_getSerializer(
- const std::string& demangledTypeName)
+ ElementJsonSerializers::ElementJsonSerializer&
+ ElementJsonSerializers::_getSerializer(const std::string& demangledTypeName)
{
if (auto find = _serializers.find(demangledTypeName); find != _serializers.end())
{
@@ -159,7 +177,8 @@ namespace armarx::viz::json
}
}
- std::vector<std::string> ElementJsonSerializers::_getRegisteredTypes() const
+ std::vector<std::string>
+ ElementJsonSerializers::_getRegisteredTypes() const
{
std::vector<std::string> types;
for (const auto& [typeName, serializer] : _serializers)
@@ -169,11 +188,11 @@ namespace armarx::viz::json
return types;
}
- bool ElementJsonSerializers::_isTypeRegistered(const std::string& typeName) const
+ bool
+ ElementJsonSerializers::_isTypeRegistered(const std::string& typeName) const
{
return _serializers.count(typeName) > 0;
}
-
-}
+} // namespace armarx::viz::json
diff --git a/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.h b/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.h
index d85ef14b666b71daabe5154d62eca105b7b4eabf..a2c7a24d1c6d3e7f2ebc1f90060c54f2cfb8d2e1 100644
--- a/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.h
+++ b/source/RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.h
@@ -9,7 +9,6 @@
#include "exceptions.h"
-
namespace armarx::viz::data
{
/// @see `ElementSerializers::to_json()`
@@ -24,8 +23,7 @@ namespace armarx::viz::data
/// @see `ElementSerializers::to_json()`
void to_json(nlohmann::json& j, const Element* elementPtr);
-}
-
+} // namespace armarx::viz::data
namespace armarx::viz::json
{
@@ -84,7 +82,6 @@ namespace armarx::viz::json
template <class ValueT>
using FromJsonFn = std::function<void(const nlohmann::json& j, ValueT& v)>;
-
/**
* @brief Handles serialization and deserialization of dynamic `data::Element`
* objects to and from JSON.
@@ -157,13 +154,11 @@ namespace armarx::viz::json
class ElementJsonSerializers
{
public:
-
/// JSON key under which demangled type name is stored.
static const std::string JSON_TYPE_NAME_KEY;
public:
-
// PUBLIC STATIC INTERFACE
/**
@@ -188,11 +183,14 @@ namespace armarx::viz::json
* If there already is a registered serializer for that type.
*/
template <class DerivedElement>
- static void registerSerializer(RawToJsonFn<DerivedElement> to_json,
- RawFromJsonFn<DerivedElement> from_json,
- bool overwrite = false)
+ static void
+ registerSerializer(RawToJsonFn<DerivedElement> to_json,
+ RawFromJsonFn<DerivedElement> from_json,
+ bool overwrite = false)
{
- registerSerializer<DerivedElement>(ToJsonFn<DerivedElement>(to_json), FromJsonFn<DerivedElement>(from_json), overwrite);
+ registerSerializer<DerivedElement>(ToJsonFn<DerivedElement>(to_json),
+ FromJsonFn<DerivedElement>(from_json),
+ overwrite);
}
/**
@@ -211,9 +209,10 @@ namespace armarx::viz::json
* If there already is a registered serializer for that type.
*/
template <class DerivedElement>
- static void registerSerializer(ToJsonFn<DerivedElement> to_json,
- FromJsonFn<DerivedElement> from_json,
- bool overwrite = false)
+ static void
+ registerSerializer(ToJsonFn<DerivedElement> to_json,
+ FromJsonFn<DerivedElement> from_json,
+ bool overwrite = false)
{
_instance._registerSerializer<DerivedElement>(to_json, from_json, overwrite);
}
@@ -222,12 +221,12 @@ namespace armarx::viz::json
* Remove a registered serializer for `DerivedElement`.
*/
template <class DerivedElement>
- static void removeSerializer()
+ static void
+ removeSerializer()
{
_instance._removeSerializer<DerivedElement>();
}
-
/// Get the type names for which serializers are registered.
static std::vector<std::string> getRegisteredTypes();
@@ -277,7 +276,6 @@ namespace armarx::viz::json
private:
-
// PRIVATE STATIC INTERFACE
/// A serializer for a specific derived `data::Element` type.
@@ -287,20 +285,20 @@ namespace armarx::viz::json
ElementJsonSerializer(ToJsonFn<DerivedElement> to_json,
FromJsonFn<DerivedElement> from_json)
{
- _to_json = [to_json](nlohmann::json & j, const data::Element & element)
+ _to_json = [to_json](nlohmann::json& j, const data::Element& element)
{
to_json(j, *dynamic_cast<const DerivedElement*>(&element));
- setTypeName(j, JSON_TYPE_NAME_KEY, simox::meta::get_type_name<DerivedElement>());
- };
- _from_json = [this, from_json](const nlohmann::json & j, data::Element & element)
- {
- from_json(j, *dynamic_cast<DerivedElement*>(&element));
+ setTypeName(
+ j, JSON_TYPE_NAME_KEY, simox::meta::get_type_name<DerivedElement>());
};
- _from_json_ptr = [this, from_json](const nlohmann::json & j, data::ElementPtr & elementPtr)
+ _from_json = [this, from_json](const nlohmann::json& j, data::Element& element)
+ { from_json(j, *dynamic_cast<DerivedElement*>(&element)); };
+ _from_json_ptr =
+ [this, from_json](const nlohmann::json& j, data::ElementPtr& elementPtr)
{
// Referencing this->from_json() here causes a memory access violation.
// Therefore we use the from_json argument.
- elementPtr = { new DerivedElement() };
+ elementPtr = {new DerivedElement()};
from_json(j, *dynamic_cast<DerivedElement*>(elementPtr.get()));
// this->_from_json(j, *elementPtr)); // Causes memory access violation.
};
@@ -314,14 +312,11 @@ namespace armarx::viz::json
public:
-
ToJsonFn<data::Element> _to_json;
FromJsonFn<data::Element> _from_json;
FromJsonFn<data::ElementPtr> _from_json_ptr;
-
};
-
static ElementJsonSerializer& getSerializer(const nlohmann::json& j);
static ElementJsonSerializer& getSerializer(const std::string& demangledTypeName);
@@ -333,7 +328,6 @@ namespace armarx::viz::json
private:
-
// PRIVATE NON-STATIC INTERFACE
/**
@@ -344,16 +338,16 @@ namespace armarx::viz::json
*/
ElementJsonSerializers();
-
/**
* @brief Register a serializer for `DerivedElement`.
* @throw `error::SerializerAlreadyRegisteredForType`
* If there already is a registered serializer for that type.
*/
template <class DerivedElement>
- void _registerSerializer(ToJsonFn<DerivedElement> to_json,
- FromJsonFn<DerivedElement> from_json,
- bool overwrite)
+ void
+ _registerSerializer(ToJsonFn<DerivedElement> to_json,
+ FromJsonFn<DerivedElement> from_json,
+ bool overwrite)
{
const std::string typeName = simox::meta::get_type_name<DerivedElement>();
if (!overwrite && _serializers.count(typeName))
@@ -367,12 +361,12 @@ namespace armarx::viz::json
* @brief Remove the serializer for `DerivedElement`.
*/
template <class DerivedElement>
- void _removeSerializer()
+ void
+ _removeSerializer()
{
_serializers.erase(simox::meta::get_type_name<DerivedElement>());
}
-
/**
* @brief Get the serializer for the given demangled type name.
* @throw `error::NoSerializerForType` If there is no serializer for the given name.
@@ -388,9 +382,6 @@ namespace armarx::viz::json
/// The serializers. Map of demangled type name to serializer.
std::map<std::string, ElementJsonSerializer> _serializers;
-
};
-}
-
-
+} // namespace armarx::viz::json
diff --git a/source/RobotAPI/components/ArViz/Introspection/exceptions.cpp b/source/RobotAPI/components/ArViz/Introspection/exceptions.cpp
index 9930695fa68f42ea28bb88385b29485f7d09906d..8a2aa796a168fbb6e23c01e2b562518f1f10c6cf 100644
--- a/source/RobotAPI/components/ArViz/Introspection/exceptions.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/exceptions.cpp
@@ -4,20 +4,21 @@
#include <SimoxUtility/algorithm/string.h>
-
namespace armarx::viz::error
{
ArvizReflectionError::ArvizReflectionError(const std::string& msg) : std::runtime_error(msg)
- {}
+ {
+ }
- NoTypeNameEntryInJsonObject::NoTypeNameEntryInJsonObject(
- const std::string& missingKey, const nlohmann::json& j) :
+ NoTypeNameEntryInJsonObject::NoTypeNameEntryInJsonObject(const std::string& missingKey,
+ const nlohmann::json& j) :
ArvizReflectionError(makeMsg(missingKey, j))
{
}
- static std::string getAvailableKeys(const nlohmann::json& j)
+ static std::string
+ getAvailableKeys(const nlohmann::json& j)
{
std::stringstream ss;
for (const auto& item : j.items())
@@ -27,7 +28,8 @@ namespace armarx::viz::error
return ss.str();
}
- std::string NoTypeNameEntryInJsonObject::makeMsg(const std::string& missingKey, const nlohmann::json& j)
+ std::string
+ NoTypeNameEntryInJsonObject::makeMsg(const std::string& missingKey, const nlohmann::json& j)
{
std::stringstream ss;
ss << "No type name entry with key '" << missingKey << "' in JSON object.\n";
@@ -42,15 +44,17 @@ namespace armarx::viz::error
return ss.str();
}
-
- TypeNameEntryAlreadyInJsonObject::TypeNameEntryAlreadyInJsonObject(
- const std::string& key, const std::string& typeName, const nlohmann::json& j) :
+ TypeNameEntryAlreadyInJsonObject::TypeNameEntryAlreadyInJsonObject(const std::string& key,
+ const std::string& typeName,
+ const nlohmann::json& j) :
ArvizReflectionError(makeMsg(key, typeName, j))
{
}
- std::string TypeNameEntryAlreadyInJsonObject::makeMsg(
- const std::string& key, const std::string& typeName, const nlohmann::json& j)
+ std::string
+ TypeNameEntryAlreadyInJsonObject::makeMsg(const std::string& key,
+ const std::string& typeName,
+ const nlohmann::json& j)
{
std::stringstream ss;
ss << "Key '" << key << "' already used in JSON object "
@@ -59,13 +63,14 @@ namespace armarx::viz::error
return ss.str();
}
-
- TypeNameMismatch::TypeNameMismatch(const std::string& typeInJson, const std::string& typeOfObject) :
+ TypeNameMismatch::TypeNameMismatch(const std::string& typeInJson,
+ const std::string& typeOfObject) :
ArvizReflectionError(makeMsg(typeInJson, typeOfObject))
{
}
- std::string TypeNameMismatch::makeMsg(const std::string& typeInJson, const std::string& typeOfObject)
+ std::string
+ TypeNameMismatch::makeMsg(const std::string& typeInJson, const std::string& typeOfObject)
{
std::stringstream ss;
ss << "Type stored JSON (" << typeInJson << ") does not match the type of passed object ("
@@ -73,18 +78,21 @@ namespace armarx::viz::error
return ss.str();
}
-
NoSerializerForType::NoSerializerForType(const std::string& typeName) :
ArvizReflectionError("No registered serializer for type '" + typeName + "'.")
- {}
+ {
+ }
SerializerAlreadyRegisteredForType::SerializerAlreadyRegisteredForType(
- const std::string& typeName, const std::vector<std::string>& acceptedTypes) :
+ const std::string& typeName,
+ const std::vector<std::string>& acceptedTypes) :
ArvizReflectionError(makeMsg(typeName, acceptedTypes))
- {}
+ {
+ }
- std::string SerializerAlreadyRegisteredForType::makeMsg(
- const std::string& typeName, const std::vector<std::string>& acceptedTypes)
+ std::string
+ SerializerAlreadyRegisteredForType::makeMsg(const std::string& typeName,
+ const std::vector<std::string>& acceptedTypes)
{
std::stringstream ss;
ss << "There is already a registered serializer for type '" + typeName + "'.";
@@ -95,5 +103,4 @@ namespace armarx::viz::error
return ss.str();
}
-}
-
+} // namespace armarx::viz::error
diff --git a/source/RobotAPI/components/ArViz/Introspection/exceptions.h b/source/RobotAPI/components/ArViz/Introspection/exceptions.h
index 513dc868e2404e6cffd161c89ae04a0bf32ab623..a9ee12bf5be94b03835ab59aba3d1743b65752ff 100644
--- a/source/RobotAPI/components/ArViz/Introspection/exceptions.h
+++ b/source/RobotAPI/components/ArViz/Introspection/exceptions.h
@@ -4,7 +4,6 @@
#include <SimoxUtility/json/json.hpp>
-
namespace armarx::viz::error
{
@@ -15,34 +14,35 @@ namespace armarx::viz::error
ArvizReflectionError(const std::string& msg);
};
-
/// Indicates that a JSON document did not contain the type name.
class NoTypeNameEntryInJsonObject : public ArvizReflectionError
{
public:
NoTypeNameEntryInJsonObject(const std::string& missingKey, const nlohmann::json& j);
+
private:
static std::string makeMsg(const std::string& missingKey, const nlohmann::json& j);
};
-
/// The TypeNameEntryAlreadyInJsonObject class
class TypeNameEntryAlreadyInJsonObject : public ArvizReflectionError
{
public:
- TypeNameEntryAlreadyInJsonObject(const std::string& key, const std::string& typeName,
+ TypeNameEntryAlreadyInJsonObject(const std::string& key,
+ const std::string& typeName,
const nlohmann::json& j);
+
private:
- static std::string makeMsg(const std::string& key, const std::string& typeName,
- const nlohmann::json& j);
+ static std::string
+ makeMsg(const std::string& key, const std::string& typeName, const nlohmann::json& j);
};
-
/// Indicates that the type name in a JSON object did not match the type of the passed C++ object.
class TypeNameMismatch : public ArvizReflectionError
{
public:
TypeNameMismatch(const std::string& typeInJson, const std::string& typeOfObject);
+
private:
static std::string makeMsg(const std::string& typeInJson, const std::string& typeOfObject);
};
@@ -54,7 +54,6 @@ namespace armarx::viz::error
NoSerializerForType(const std::string& typeName);
};
-
/// Indicates that there already was a serializer registered for a type when trying to
/// register a serializer
class SerializerAlreadyRegisteredForType : public ArvizReflectionError
@@ -62,8 +61,10 @@ namespace armarx::viz::error
public:
SerializerAlreadyRegisteredForType(const std::string& typeName,
const std::vector<std::string>& acceptedTypes = {});
+
private:
- static std::string makeMsg(const std::string& typeName, const std::vector<std::string>& acceptedTypes);
+ static std::string makeMsg(const std::string& typeName,
+ const std::vector<std::string>& acceptedTypes);
};
-}
+} // namespace armarx::viz::error
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_base.cpp b/source/RobotAPI/components/ArViz/Introspection/json_base.cpp
index ae5f23e56d2f3603622b1c3f0f6c6d28debc4590..3dc3a6e5740846d2dbd94b3debc477b22d86055a 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_base.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/json_base.cpp
@@ -4,36 +4,40 @@
#include "ElementJsonSerializers.h"
-
-void armarx::to_json(nlohmann::json& j, const armarx::Vector2f& value)
+void
+armarx::to_json(nlohmann::json& j, const armarx::Vector2f& value)
{
j["x"] = value.e0;
j["y"] = value.e1;
}
-void armarx::from_json(const nlohmann::json& j, armarx::Vector2f& value)
+
+void
+armarx::from_json(const nlohmann::json& j, armarx::Vector2f& value)
{
value.e0 = j.at("x").get<float>();
value.e1 = j.at("y").get<float>();
}
-
-void armarx::to_json(nlohmann::json& j, const armarx::Vector3f& value)
+void
+armarx::to_json(nlohmann::json& j, const armarx::Vector3f& value)
{
j["x"] = value.e0;
j["y"] = value.e1;
j["z"] = value.e2;
}
-void armarx::from_json(const nlohmann::json& j, armarx::Vector3f& value)
+
+void
+armarx::from_json(const nlohmann::json& j, armarx::Vector3f& value)
{
value.e0 = j.at("x").get<float>();
value.e1 = j.at("y").get<float>();
value.e2 = j.at("z").get<float>();
}
-
namespace armarx::viz
{
- void data::to_json(nlohmann::json& j, const data::GlobalPose& pose)
+ void
+ data::to_json(nlohmann::json& j, const data::GlobalPose& pose)
{
armarx::Vector3f pos;
pos.e0 = pose.x;
@@ -45,7 +49,9 @@ namespace armarx::viz
j[json::meta::KEY]["orientation"] = json::meta::ORIENTATION;
}
- void data::from_json(const nlohmann::json& j, data::GlobalPose& pose)
+
+ void
+ data::from_json(const nlohmann::json& j, data::GlobalPose& pose)
{
const armarx::Vector3f pos = j.at("position").get<armarx::Vector3f>();
const Eigen::Quaternionf ori = j.at("orientation").get<Eigen::Quaternionf>();
@@ -58,14 +64,17 @@ namespace armarx::viz
pose.qz = ori.z();
}
- void data::to_json(nlohmann::json& j, const data::Color& color)
+ void
+ data::to_json(nlohmann::json& j, const data::Color& color)
{
j["r"] = color.r;
j["g"] = color.g;
j["b"] = color.b;
j["a"] = color.a;
}
- void data::from_json(const nlohmann::json& j, data::Color& color)
+
+ void
+ data::from_json(const nlohmann::json& j, data::Color& color)
{
color.r = j.at("r").get<Ice::Byte>();
color.g = j.at("g").get<Ice::Byte>();
@@ -73,7 +82,6 @@ namespace armarx::viz
color.a = j.at("a").get<Ice::Byte>();
}
-
const std::string json::meta::KEY = "__meta__";
const std::string json::meta::HIDE = "hide";
@@ -82,8 +90,8 @@ namespace armarx::viz
const std::string json::meta::COLOR = "color";
const std::string json::meta::ORIENTATION = "orientation";
-
- void json::to_json_base(nlohmann::json& j, const data::Element& element)
+ void
+ json::to_json_base(nlohmann::json& j, const data::Element& element)
{
j["id"] = element.id;
j["pose"] = element.pose;
@@ -95,7 +103,9 @@ namespace armarx::viz
j[meta::KEY]["flags"] = meta::HIDE;
j[meta::KEY]["color"] = meta::COLOR;
}
- void json::from_json_base(const nlohmann::json& j, data::Element& element)
+
+ void
+ json::from_json_base(const nlohmann::json& j, data::Element& element)
{
element.id = j.at("id").get<std::string>();
element.pose = j.at("pose").get<data::GlobalPose>();
@@ -104,5 +114,4 @@ namespace armarx::viz
element.scale = j.at("scale").get<armarx::Vector3f>();
}
-}
-
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_base.h b/source/RobotAPI/components/ArViz/Introspection/json_base.h
index 7f5613d2bdecd8953007628d9231dc4cd3ef61c0..65c2bea3eef2073f23872e0c283b0220a93bfb83 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_base.h
+++ b/source/RobotAPI/components/ArViz/Introspection/json_base.h
@@ -4,13 +4,11 @@
#include <RobotAPI/interface/ArViz/Elements.h>
-
namespace armarx
{
}
-
namespace armarx
{
void to_json(nlohmann::json& j, const Vector2f& value);
@@ -18,8 +16,7 @@ namespace armarx
void to_json(nlohmann::json& j, const Vector3f& value);
void from_json(const nlohmann::json& j, Vector3f& value);
-}
-
+} // namespace armarx
namespace armarx::viz::data
{
@@ -28,8 +25,7 @@ namespace armarx::viz::data
void to_json(nlohmann::json& j, const data::Color& color);
void from_json(const nlohmann::json& j, data::Color& color);
-}
-
+} // namespace armarx::viz::data
namespace armarx::viz::json
{
@@ -51,11 +47,9 @@ namespace armarx::viz::json
// extern const std::string POSE;
// extern const std::string POSITION;
extern const std::string ORIENTATION;
- }
-
+ } // namespace meta
void to_json_base(nlohmann::json& j, const data::Element& element);
void from_json_base(const nlohmann::json& j, data::Element& value);
-}
-
+} // namespace armarx::viz::json
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
index bfb083d844beb8d64851cd14725f049692d1028e..edccbe4759dfc15b478352dbb525bb33ba7b53cd 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.cpp
@@ -7,26 +7,23 @@
#include "json_base.h"
-
namespace armarx::viz
{
- const simox::meta::IntEnumNames data::ElementFlags::names =
- {
- { NONE, "None" },
- { OVERRIDE_MATERIAL, "Override_Material" },
+ const simox::meta::IntEnumNames data::ElementFlags::names = {
+ {NONE, "None"},
+ {OVERRIDE_MATERIAL, "Override_Material"},
};
- const simox::meta::IntEnumNames data::ModelDrawStyle::names =
- {
- { ORIGINAL, "Original" },
- { COLLISION, "Collision" },
- { OVERRIDE_COLOR, "Override_Color" },
+ const simox::meta::IntEnumNames data::ModelDrawStyle::names = {
+ {ORIGINAL, "Original"},
+ {COLLISION, "Collision"},
+ {OVERRIDE_COLOR, "Override_Color"},
};
-
- void data::to_json(nlohmann::json& j, const ColoredPoint& coloredPoint)
+ void
+ data::to_json(nlohmann::json& j, const ColoredPoint& coloredPoint)
{
j["x"] = coloredPoint.x;
j["y"] = coloredPoint.y;
@@ -34,7 +31,9 @@ namespace armarx::viz
j["color"] = coloredPoint.color;
j[json::meta::KEY]["color"] = json::meta::COLOR;
}
- void data::from_json(const nlohmann::json& j, ColoredPoint& coloredPoint)
+
+ void
+ data::from_json(const nlohmann::json& j, ColoredPoint& coloredPoint)
{
coloredPoint.x = j.at("x");
coloredPoint.y = j.at("y");
@@ -42,29 +41,33 @@ namespace armarx::viz
coloredPoint.color = j.at("color");
}
-
- void data::to_json(nlohmann::json& j, const ElementArrow& arrow)
+ void
+ data::to_json(nlohmann::json& j, const ElementArrow& arrow)
{
json::to_json_base(j, arrow);
j["length"] = arrow.length;
j["width"] = arrow.width;
}
- void data::from_json(const nlohmann::json& j, ElementArrow& arrow)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementArrow& arrow)
{
json::from_json_base(j, arrow);
arrow.length = j.at("length");
arrow.width = j.at("width");
}
-
- void data::to_json(nlohmann::json& j, const ElementArrowCircle& arrowCircle)
+ void
+ data::to_json(nlohmann::json& j, const ElementArrowCircle& arrowCircle)
{
json::to_json_base(j, arrowCircle);
j["radius"] = arrowCircle.radius;
j["completion"] = arrowCircle.completion;
j["width"] = arrowCircle.width;
}
- void data::from_json(const nlohmann::json& j, ElementArrowCircle& arrowCircle)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementArrowCircle& arrowCircle)
{
json::from_json_base(j, arrowCircle);
arrowCircle.radius = j.at("radius");
@@ -72,41 +75,47 @@ namespace armarx::viz
arrowCircle.width = j.at("width");
}
-
- void data::to_json(nlohmann::json& j, const ElementBox& box)
+ void
+ data::to_json(nlohmann::json& j, const ElementBox& box)
{
json::to_json_base(j, box);
j["size"] = box.size;
}
- void data::from_json(const nlohmann::json& j, ElementBox& box)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementBox& box)
{
json::from_json_base(j, box);
box.size = j.at("size").get<armarx::Vector3f>();
}
-
- void data::to_json(nlohmann::json& j, const ElementCylinder& cylinder)
+ void
+ data::to_json(nlohmann::json& j, const ElementCylinder& cylinder)
{
json::to_json_base(j, cylinder);
j["height"] = cylinder.height;
j["radius"] = cylinder.radius;
}
- void data::from_json(const nlohmann::json& j, ElementCylinder& cylinder)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementCylinder& cylinder)
{
json::from_json_base(j, cylinder);
cylinder.height = j.at("height");
cylinder.radius = j.at("radius");
}
-
- void data::to_json(nlohmann::json& j, const ElementCylindroid& cylindroid)
+ void
+ data::to_json(nlohmann::json& j, const ElementCylindroid& cylindroid)
{
json::to_json_base(j, cylindroid);
j["height"] = cylindroid.height;
j["axisLengths"] = cylindroid.axisLengths;
j["curvature"] = cylindroid.curvature;
}
- void data::from_json(const nlohmann::json& j, ElementCylindroid& cylindroid)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementCylindroid& cylindroid)
{
json::from_json_base(j, cylindroid);
cylindroid.height = j.at("height");
@@ -114,15 +123,17 @@ namespace armarx::viz
cylindroid.curvature = j.at("curvature").get<armarx::Vector2f>();
}
-
- void data::to_json(nlohmann::json& j, const ElementLine& line)
+ void
+ data::to_json(nlohmann::json& j, const ElementLine& line)
{
json::to_json_base(j, line);
j["from"] = line.from;
j["to"] = line.to;
j["lineWidth"] = line.lineWidth;
}
- void data::from_json(const nlohmann::json& j, ElementLine& line)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementLine& line)
{
json::from_json_base(j, line);
line.from = j.at("from").get<armarx::Vector3f>();
@@ -130,8 +141,8 @@ namespace armarx::viz
line.lineWidth = j.at("lineWidth").get<float>();
}
-
- void data::to_json(nlohmann::json& j, const ElementMesh& mesh)
+ void
+ data::to_json(nlohmann::json& j, const ElementMesh& mesh)
{
json::to_json_base(j, mesh);
j["# Vertices"] = mesh.vertices.size();
@@ -142,19 +153,21 @@ namespace armarx::viz
j[json::meta::KEY]["# Colors"] = json::meta::READ_ONLY;
j[json::meta::KEY]["# Faces"] = json::meta::READ_ONLY;
}
- void data::from_json(const nlohmann::json& j, ElementMesh& mesh)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementMesh& mesh)
{
json::from_json_base(j, mesh);
}
-
- void data::to_json(nlohmann::json& j, const ElementPointCloud& pointCloud)
+ void
+ data::to_json(nlohmann::json& j, const ElementPointCloud& pointCloud)
{
json::to_json_base(j, pointCloud);
j["transparency"] = pointCloud.transparency;
j["pointSizeInPixels"] = pointCloud.pointSizeInPixels;
- std::size_t numPoints = pointCloud.points.size() / sizeof (ColoredPoint);
+ std::size_t numPoints = pointCloud.points.size() / sizeof(ColoredPoint);
j["# Points"] = numPoints;
j[json::meta::KEY]["# Points"] = json::meta::READ_ONLY;
@@ -162,15 +175,17 @@ namespace armarx::viz
ColoredPoint const* end = begin + std::min(std::size_t(10), numPoints);
j["Points[0:10]"] = ColoredPointList(begin, end);
}
- void data::from_json(const nlohmann::json& j, ElementPointCloud& pointCloud)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementPointCloud& pointCloud)
{
json::from_json_base(j, pointCloud);
pointCloud.transparency = j.at("transparency");
pointCloud.pointSizeInPixels = j.at("pointSizeInPixels");
}
-
- void data::to_json(nlohmann::json& j, const ElementPolygon& polygon)
+ void
+ data::to_json(nlohmann::json& j, const ElementPolygon& polygon)
{
json::to_json_base(j, polygon);
j["lineColor"] = polygon.lineColor;
@@ -178,7 +193,9 @@ namespace armarx::viz
j["lineWidth"] = polygon.lineWidth;
j["points"] = polygon.points;
}
- void data::from_json(const nlohmann::json& j, ElementPolygon& polygon)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementPolygon& polygon)
{
json::from_json_base(j, polygon);
polygon.lineColor = j.at("lineColor");
@@ -186,76 +203,85 @@ namespace armarx::viz
polygon.points = j.at("points").get<std::vector<armarx::Vector3f>>();
}
-
- void data::to_json(nlohmann::json& j, const ElementPose& pose)
+ void
+ data::to_json(nlohmann::json& j, const ElementPose& pose)
{
json::to_json_base(j, pose);
-
}
- void data::from_json(const nlohmann::json& j, ElementPose& pose)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementPose& pose)
{
json::from_json_base(j, pose);
}
- void data::to_json(nlohmann::json& j, const ElementPath& path)
+ void
+ data::to_json(nlohmann::json& j, const ElementPath& path)
{
json::to_json_base(j, path);
j["points"] = path.points;
}
- void data::from_json(const nlohmann::json& j, ElementPath& path)
+ void
+ data::from_json(const nlohmann::json& j, ElementPath& path)
{
json::from_json_base(j, path);
path.points = j.at("points").get<armarx::Vector3fSeq>();
}
-
- void data::to_json(nlohmann::json& j, const ElementSphere& sphere)
+ void
+ data::to_json(nlohmann::json& j, const ElementSphere& sphere)
{
json::to_json_base(j, sphere);
j["radius"] = sphere.radius;
}
- void data::from_json(const nlohmann::json& j, ElementSphere& sphere)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementSphere& sphere)
{
json::from_json_base(j, sphere);
sphere.radius = j.at("radius");
}
-
- void data::to_json(nlohmann::json& j, const ElementEllipsoid& ellipsoid)
+ void
+ data::to_json(nlohmann::json& j, const ElementEllipsoid& ellipsoid)
{
json::to_json_base(j, ellipsoid);
j["axisLengths"] = ellipsoid.axisLengths;
j["curvature"] = ellipsoid.curvature;
}
- void data::from_json(const nlohmann::json& j, ElementEllipsoid& ellipsoid)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementEllipsoid& ellipsoid)
{
json::from_json_base(j, ellipsoid);
ellipsoid.axisLengths = j.at("axisLengths");
ellipsoid.curvature = j.at("curvature");
}
-
- void data::to_json(nlohmann::json& j, const ElementText& text)
+ void
+ data::to_json(nlohmann::json& j, const ElementText& text)
{
json::to_json_base(j, text);
j["text"] = text.text;
}
- void data::from_json(const nlohmann::json& j, ElementText& text)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementText& text)
{
json::from_json_base(j, text);
text.text = j.at("text");
}
-
namespace data::ModelDrawStyle
{
- std::string to_flag_names(const int drawStyle)
+ std::string
+ to_flag_names(const int drawStyle)
{
std::vector<std::string> flag_names;
for (int flag : names.values())
{
- if (drawStyle == flag // Necessary if flag is 0
+ if (drawStyle == flag // Necessary if flag is 0
|| drawStyle & flag)
{
flag_names.push_back(names.to_name(flag));
@@ -264,7 +290,8 @@ namespace armarx::viz
return simox::alg::join(flag_names, " | ");
}
- int from_flag_names(const std::string& flagString)
+ int
+ from_flag_names(const std::string& flagString)
{
bool trim_elements = true;
std::vector<std::string> split = simox::alg::split(flagString, "|", trim_elements);
@@ -275,19 +302,22 @@ namespace armarx::viz
}
return flag;
}
- }
+ } // namespace data::ModelDrawStyle
-
- void data::to_json(nlohmann::json& j, const ElementObject& object)
+ void
+ data::to_json(nlohmann::json& j, const ElementObject& object)
{
json::to_json_base(j, object);
j["project"] = object.project;
j["filename"] = object.filename;
- j["drawStyle"] = ModelDrawStyle::to_flag_names(object.drawStyle) + " (" + std::to_string(object.drawStyle) + ")";
+ j["drawStyle"] = ModelDrawStyle::to_flag_names(object.drawStyle) + " (" +
+ std::to_string(object.drawStyle) + ")";
j[json::meta::KEY]["drawStyle"] = json::meta::READ_ONLY;
}
- void data::from_json(const nlohmann::json& j, ElementObject& object)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementObject& object)
{
json::from_json_base(j, object);
object.project = j.at("project");
@@ -295,8 +325,8 @@ namespace armarx::viz
object.drawStyle = ModelDrawStyle::from_flag_names(j.at("drawStyle"));
}
-
- void data::to_json(nlohmann::json& j, const ElementRobot& robot)
+ void
+ data::to_json(nlohmann::json& j, const ElementRobot& robot)
{
json::to_json_base(j, robot);
j["project"] = robot.project;
@@ -306,7 +336,9 @@ namespace armarx::viz
j["drawStyle"] = ModelDrawStyle::to_flag_names(robot.drawStyle);
j[json::meta::KEY]["drawStyle"] = json::meta::READ_ONLY;
}
- void data::from_json(const nlohmann::json& j, ElementRobot& robot)
+
+ void
+ data::from_json(const nlohmann::json& j, ElementRobot& robot)
{
json::from_json_base(j, robot);
robot.project = j.at("project");
@@ -315,4 +347,4 @@ namespace armarx::viz
robot.drawStyle = ModelDrawStyle::from_flag_names(j.at("drawStyle"));
}
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_elements.h b/source/RobotAPI/components/ArViz/Introspection/json_elements.h
index 5e41dc0df1c60d71ec11dafed535e3ec35126603..e0a57bf7a8c3761662274694c74a86356143ecd9 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_elements.h
+++ b/source/RobotAPI/components/ArViz/Introspection/json_elements.h
@@ -5,7 +5,6 @@
#include <RobotAPI/interface/ArViz/Elements.h>
-
namespace armarx::viz::data
{
@@ -19,7 +18,6 @@ namespace armarx::viz::data
extern const simox::meta::IntEnumNames names;
}
-
void to_json(nlohmann::json& j, const ColoredPoint& coloredPoint);
void from_json(const nlohmann::json& j, ColoredPoint& coloredPoint);
@@ -89,4 +87,4 @@ namespace armarx::viz::data
void to_json(nlohmann::json& j, const ElementText& text);
void from_json(const nlohmann::json& j, ElementText& text);
-}
+} // namespace armarx::viz::data
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_layer.cpp b/source/RobotAPI/components/ArViz/Introspection/json_layer.cpp
index 1ec6c10cbd4704751e173fdfe021bd9521d7f24f..7492689c50c45a6eb544d05fd01f01bb4e9efc6b 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_layer.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/json_layer.cpp
@@ -1,21 +1,22 @@
#include "json_layer.h"
-#include "json_base.h"
-
#include "ElementJsonSerializers.h"
-
+#include "json_base.h"
namespace armarx::viz
{
- void data::to_json(nlohmann::json& j, const LayerUpdate& update)
+ void
+ data::to_json(nlohmann::json& j, const LayerUpdate& update)
{
j["component"] = update.component;
j["name"] = update.name;
j["action"] = update.action;
j["elements"] = update.elements;
}
- void data::from_json(const nlohmann::json& j, LayerUpdate& update)
+
+ void
+ data::from_json(const nlohmann::json& j, LayerUpdate& update)
{
update.component = j.at("component");
update.name = j.at("name");
@@ -23,16 +24,18 @@ namespace armarx::viz
update.elements = j.at("elements").get<ElementSeq>();
}
- void data::to_json(nlohmann::json& j, const LayerUpdates& updates)
+ void
+ data::to_json(nlohmann::json& j, const LayerUpdates& updates)
{
j["updates"] = updates.updates;
j["revision"] = updates.revision;
}
- void data::from_json(const nlohmann::json& j, LayerUpdates& updates)
+
+ void
+ data::from_json(const nlohmann::json& j, LayerUpdates& updates)
{
updates.updates = j.at("updates").get<data::LayerUpdateSeq>();
updates.revision = j.at("revision").get<Ice::Long>();
}
-}
-
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Introspection/json_layer.h b/source/RobotAPI/components/ArViz/Introspection/json_layer.h
index a79c0abc7ffb43cbfaa037e492e7bf422211dd44..d85e94b616600bdc0741249d9467df09ed09042a 100644
--- a/source/RobotAPI/components/ArViz/Introspection/json_layer.h
+++ b/source/RobotAPI/components/ArViz/Introspection/json_layer.h
@@ -4,7 +4,6 @@
#include <RobotAPI/interface/ArViz/Component.h>
-
namespace armarx::viz::data
{
@@ -14,4 +13,4 @@ namespace armarx::viz::data
void to_json(nlohmann::json& j, const LayerUpdates& update);
void from_json(const nlohmann::json& j, LayerUpdates& update);
-}
+} // namespace armarx::viz::data
diff --git a/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp b/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
index 48228263d42465bf3a219488ba012437aae93001..b3f8084477f574e1e69cf5ae1e49d522a51851d0 100644
--- a/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
+++ b/source/RobotAPI/components/ArViz/Introspection/register_element_json_serializers.cpp
@@ -1,9 +1,10 @@
#include <RobotAPI/interface/ArViz/Elements.h>
+
#include "ElementJsonSerializers.h"
#include "json_elements.h"
-
-void armarx::viz::json::ElementJsonSerializers::registerElements()
+void
+armarx::viz::json::ElementJsonSerializers::registerElements()
{
registerSerializer<data::ElementArrow>(viz::data::to_json, viz::data::from_json);
registerSerializer<data::ElementArrowCircle>(viz::data::to_json, viz::data::from_json);
diff --git a/source/RobotAPI/components/ArViz/test/Client/ColorTest.cpp b/source/RobotAPI/components/ArViz/test/Client/ColorTest.cpp
index 3ba59f58d7ae224e5a28c9a0e00cd436c39d2a8f..2776da1b6960b338ac202d94ac3ce77226ecc7de 100644
--- a/source/RobotAPI/components/ArViz/test/Client/ColorTest.cpp
+++ b/source/RobotAPI/components/ArViz/test/Client/ColorTest.cpp
@@ -25,10 +25,10 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ArViz/Client/elements/Color.h>
#include <iostream>
+#include <RobotAPI/components/ArViz/Client/elements/Color.h>
namespace
{
@@ -37,7 +37,6 @@ namespace
namespace viz = armarx::viz;
-
BOOST_AUTO_TEST_CASE(test_fromRGBA_int_vs_float)
{
BOOST_CHECK_EQUAL(viz::Color::fromRGBA(0, 0, 0), viz::Color::fromRGBA(0.f, 0.f, 0.f));
@@ -45,7 +44,6 @@ BOOST_AUTO_TEST_CASE(test_fromRGBA_int_vs_float)
BOOST_CHECK_EQUAL(viz::Color::fromRGBA(255, 255, 255), viz::Color::fromRGBA(1.f, 1.f, 1.f));
}
-
BOOST_AUTO_TEST_CASE(test_fromRGBA_from_vector3)
{
const Eigen::Vector3i color(0, 127, 255);
diff --git a/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp b/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp
index 2100834633ab64dc8a2f9fe7fe23298cb510ef0c..ffa9714c8dd76b254174d820dbdf2d3faea3f272 100644
--- a/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp
+++ b/source/RobotAPI/components/ArViz/test/Client/PointCloudTest.cpp
@@ -25,13 +25,14 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ArViz/Client/elements/point_cloud_type_traits.hpp>
-#include <RobotAPI/components/ArViz/Client/elements/PointCloud.h>
#include <iostream>
#include <SimoxUtility/color/cmaps.h>
+#include <RobotAPI/components/ArViz/Client/elements/PointCloud.h>
+#include <RobotAPI/components/ArViz/Client/elements/point_cloud_type_traits.hpp>
+
namespace viz = armarx::viz;
@@ -62,38 +63,32 @@ namespace PointCloudTestNamespace
uint32_t label;
};
-
struct PointCloudFixture
{
PointCloudFixture()
{
-
}
+
~PointCloudFixture()
{
-
}
- viz::PointCloud pc {"pointcloud"};
+ viz::PointCloud pc{"pointcloud"};
- const std::vector<PointXYZRGBL> points
- {
- { 10, 20, 30, 1, 2, 3, 4, 100 },
+ const std::vector<PointXYZRGBL> points{
+ {10, 20, 30, 1, 2, 3, 4, 100},
};
- const std::vector<PointXYZRGBL> points2
- {
- { 40, 50, 60, 4, 5, 6, 7, 100 },
- { 70, 80, 90, 7, 8, 9, 10, 200 },
+ const std::vector<PointXYZRGBL> points2{
+ {40, 50, 60, 4, 5, 6, 7, 100},
+ {70, 80, 90, 7, 8, 9, 10, 200},
};
- const std::vector<int> indices
- {
+ const std::vector<int> indices{
1,
};
};
-}
-
+} // namespace PointCloudTestNamespace
BOOST_AUTO_TEST_CASE(test_has_member_rgba)
{
@@ -120,8 +115,6 @@ BOOST_AUTO_TEST_CASE(test_has_member_rgba)
BOOST_CHECK_EQUAL(true, true);
}
-
-
BOOST_AUTO_TEST_CASE(test_addPoint_color_types)
{
using namespace PointCloudTestNamespace;
@@ -129,7 +122,7 @@ BOOST_AUTO_TEST_CASE(test_addPoint_color_types)
viz::PointCloud pc("pointcloud");
viz::ColoredPoint* begin = (viz::ColoredPoint*)pc.data_->points.data();
- viz::ColoredPoint* end = begin + (pc.data_->points.size() / sizeof (viz::ColoredPoint));
+ viz::ColoredPoint* end = begin + (pc.data_->points.size() / sizeof(viz::ColoredPoint));
viz::ColoredPointList points(begin, end);
// XYZ, default color
@@ -174,14 +167,11 @@ BOOST_AUTO_TEST_CASE(test_addPoint_color_types)
// RGBL, RGBA
pc.addPoint(PointXYZRGBL{0., 0., 0., 6, 7, 8, 9, 100}, false);
BOOST_CHECK_EQUAL(points.back().color, simox::Color(6, 7, 8, 9));
-
}
BOOST_FIXTURE_TEST_SUITE(test_pointCloud_suite, PointCloudTestNamespace::PointCloudFixture)
-
-
BOOST_AUTO_TEST_CASE(test_pointCloud_colorByLabel)
{
bool colorByLabel = false;
@@ -197,81 +187,55 @@ BOOST_AUTO_TEST_CASE(test_pointCloud_colorByLabel)
BOOST_CHECK_EQUAL(ps.front().color, simox::color::GlasbeyLUT::at(points.front().label));
}
-
BOOST_AUTO_TEST_CASE(test_pointCloud_color_func)
{
using namespace PointCloudTestNamespace;
// Test returning a viz::Color.
- pc.pointCloud(points, [](const PointXYZRGBL & p)
- {
- return viz::Color::fromRGBA(p.r, 0, 0, 0);
- });
+ pc.pointCloud(points, [](const PointXYZRGBL& p) { return viz::Color::fromRGBA(p.r, 0, 0, 0); });
BOOST_CHECK_EQUAL(ps.size(), 1);
BOOST_CHECK_EQUAL(ps.front().color, simox::Color(points.front().r, 0, 0, 0));
- pc.pointCloud(points2, indices, [](const PointXYZRGBL & p)
- {
- return viz::Color::fromRGBA(p.r, 0, 0, 0);
- });
+ pc.pointCloud(
+ points2, indices, [](const PointXYZRGBL& p) { return viz::Color::fromRGBA(p.r, 0, 0, 0); });
BOOST_CHECK_EQUAL(ps.size(), 1);
BOOST_CHECK_EQUAL(ps.front().color, simox::Color(points2.back().r, 0, 0, 0));
// Test returning a simox::Color.
- pc.pointCloud(points, [](PointXYZRGBL p)
- {
- return simox::Color(0, p.g, 0, 0);
- });
+ pc.pointCloud(points, [](PointXYZRGBL p) { return simox::Color(0, p.g, 0, 0); });
BOOST_CHECK_EQUAL(ps.size(), 1);
BOOST_CHECK_EQUAL(ps.front().color, simox::Color(0, points.front().g, 0, 0));
- pc.pointCloud(points2, indices, [](const PointXYZRGBL & p)
- {
- return simox::Color(0, p.g, 0, 0);
- });
+ pc.pointCloud(
+ points2, indices, [](const PointXYZRGBL& p) { return simox::Color(0, p.g, 0, 0); });
BOOST_CHECK_EQUAL(ps.size(), 1);
BOOST_CHECK_EQUAL(ps.front().color, simox::Color(0, points2.back().g, 0, 0));
-
}
-
BOOST_AUTO_TEST_CASE(test_pointCloud_scalar_func_cmap)
{
using namespace PointCloudTestNamespace;
const simox::ColorMap cmap = simox::color::cmaps::viridis();
- pc.pointCloud(points, cmap, [](const PointXYZRGBL & p)
- {
- return p.x;
- });
+ pc.pointCloud(points, cmap, [](const PointXYZRGBL& p) { return p.x; });
BOOST_CHECK_EQUAL(ps.size(), 1);
BOOST_CHECK_EQUAL(ps.front().color, cmap(ps.front().x));
- pc.pointCloud(points2, indices, cmap, [](const PointXYZRGBL & p)
- {
- return p.x;
- });
+ pc.pointCloud(points2, indices, cmap, [](const PointXYZRGBL& p) { return p.x; });
BOOST_CHECK_EQUAL(ps.size(), 1);
BOOST_CHECK_EQUAL(ps.front().color, cmap(points2.back().x));
- pc.pointCloud(points, cmap, [](PointXYZRGBL p)
- {
- return p.y;
- });
+ pc.pointCloud(points, cmap, [](PointXYZRGBL p) { return p.y; });
BOOST_CHECK_EQUAL(ps.size(), 1);
BOOST_CHECK_EQUAL(ps.front().color, cmap(ps.front().y));
- pc.pointCloud(points2, indices, cmap, [](const PointXYZRGBL & p)
- {
- return p.y;
- });
+ pc.pointCloud(points2, indices, cmap, [](const PointXYZRGBL& p) { return p.y; });
BOOST_CHECK_EQUAL(ps.size(), 1);
BOOST_CHECK_EQUAL(ps.front().color, cmap(points2.back().y));
}
BOOST_AUTO_TEST_SUITE_END()
-
diff --git a/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp
index 1b2b3df5038078ad16a14af3cb977767129e096d..ae80637126190cc8c15f2fc120e15c4108917a5d 100644
--- a/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp
+++ b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.cpp
@@ -20,10 +20,12 @@
* GNU General Public License
*/
+#include "AronComponentConfigExample.h"
+
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
#include <RobotAPI/libraries/aron_component_config/PropertyDefinitionVisitors.h>
#include <RobotAPI/libraries/aron_component_config/RemoteGui.h>
-#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
-#include "AronComponentConfigExample.h"
namespace armarx
{
@@ -31,36 +33,35 @@ namespace armarx
void
AronComponentConfigExample::onInitComponent()
{
-
}
void
AronComponentConfigExample::onConnectComponent()
{
- remote_gui_plugin_->createOrUpdateTab(aron_component_config_plugin_->buildRemoteGui("Config"),
- [this](armarx::RemoteGui::TabProxy& prx)
- {
- prx.receiveUpdates();
- std::lock_guard lock(write_mutex);
- if (aron_component_config_plugin_->updateRemoteGui(prx))
- {
- remote_gui_plugin_->createOrUpdateTab("",
- aron_component_config_plugin_
- ->buildRemoteGui(
- "Config"));
- }
- prx.sendUpdates();
- });
- periodicTask = new SimplePeriodicTask<>([&, this]
- {
- std::lock_guard lock(write_mutex);
- ARMARX_TRACE;
- auto& config =
- aron_component_config_plugin_->config_.getWriteBuffer();
-// ARMARX_INFO << VAROUT(config.orientation) << VAROUT(config.pose) << VAROUT(config.position);
- config.intMember--;
- aron_component_config_plugin_->config_.commitWrite();
- }, 1000);
+ remote_gui_plugin_->createOrUpdateTab(
+ aron_component_config_plugin_->buildRemoteGui("Config"),
+ [this](armarx::RemoteGui::TabProxy& prx)
+ {
+ prx.receiveUpdates();
+ std::lock_guard lock(write_mutex);
+ if (aron_component_config_plugin_->updateRemoteGui(prx))
+ {
+ remote_gui_plugin_->createOrUpdateTab(
+ "", aron_component_config_plugin_->buildRemoteGui("Config"));
+ }
+ prx.sendUpdates();
+ });
+ periodicTask = new SimplePeriodicTask<>(
+ [&, this]
+ {
+ std::lock_guard lock(write_mutex);
+ ARMARX_TRACE;
+ auto& config = aron_component_config_plugin_->config_.getWriteBuffer();
+ // ARMARX_INFO << VAROUT(config.orientation) << VAROUT(config.pose) << VAROUT(config.position);
+ config.intMember--;
+ aron_component_config_plugin_->config_.commitWrite();
+ },
+ 1000);
periodicTask->start();
}
@@ -77,21 +78,18 @@ namespace armarx
armarx::PropertyDefinitionsPtr
AronComponentConfigExample::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new AronComponentConfigExamplePropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new AronComponentConfigExamplePropertyDefinitions(getConfigIdentifier());
auto& config = aron_component_config_plugin_->config_.getWriteBuffer();
config.boolMember = true;
config.floatMember = 100.0f;
config.intMember = 1000;
config.stringMember = "initial";
config.longMember = 0;
- config.subMember
- .subsubMember
- .intListMember
- .insert(config.subMember.subsubMember.intListMember.end(), {1, 2, 3, 4, 5, 6});
- config.subMember
- .subsubMember
- .stringListMember
- .insert(config.subMember.subsubMember.stringListMember.end(), {"a", "b", "c", "d", "e"});
+ config.subMember.subsubMember.intListMember.insert(
+ config.subMember.subsubMember.intListMember.end(), {1, 2, 3, 4, 5, 6});
+ config.subMember.subsubMember.stringListMember.insert(
+ config.subMember.subsubMember.stringListMember.end(), {"a", "b", "c", "d", "e"});
config.subMember.subsubMember.intDictMember.emplace("int1", 1);
config.subMember.subsubMember.intDictMember.emplace("int2", 2);
config.subMember.subsubMember.stringDictMember.emplace("string1", "blub");
@@ -112,8 +110,9 @@ namespace armarx
addPlugin(aron_component_config_plugin_, "");
}
- AronComponentConfigExamplePropertyDefinitions::AronComponentConfigExamplePropertyDefinitions(std::string prefix) :
- armarx::ComponentPropertyDefinitions(std::move(prefix))
+ AronComponentConfigExamplePropertyDefinitions::AronComponentConfigExamplePropertyDefinitions(
+ std::string prefix) :
+ armarx::ComponentPropertyDefinitions(std::move(prefix))
{
}
-}
\ No newline at end of file
+} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.h b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.h
index e466b0a6ae20979766da06f839d31f45b76c3574..739798418f522d040d8f1842726e8de688f231c2 100644
--- a/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.h
+++ b/source/RobotAPI/components/AronComponentConfigExample/AronComponentConfigExample.h
@@ -23,6 +23,7 @@
#pragma once
#include <ArmarXCore/core/Component.h>
+
#include <RobotAPI/components/AronComponentConfigExample/aron/ComponentConfig.aron.generated.h>
#include <RobotAPI/libraries/aron_component_config/ComponentPlugin.h>
@@ -30,14 +31,13 @@ namespace armarx
{
class AronComponentConfigExamplePropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
+ public armarx::ComponentPropertyDefinitions
{
public:
AronComponentConfigExamplePropertyDefinitions(std::string prefix);
};
- class AronComponentConfigExample :
- virtual public armarx::Component
+ class AronComponentConfigExample : virtual public armarx::Component
{
public:
std::string getDefaultName() const override;
@@ -58,12 +58,14 @@ namespace armarx
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
AronComponentConfigExample();
+
private:
SimplePeriodicTask<>::pointer_type periodicTask;
- std::experimental::observer_ptr<plugins::RemoteGuiComponentPlugin> remote_gui_plugin_{nullptr};
- std::experimental::observer_ptr<plugins::AronComponentConfigPlugin<armarx::component_config::aron::TestConfig>>
- aron_component_config_plugin_{nullptr};
+ std::experimental::observer_ptr<plugins::RemoteGuiComponentPlugin> remote_gui_plugin_{
+ nullptr};
+ std::experimental::observer_ptr<
+ plugins::AronComponentConfigPlugin<armarx::component_config::aron::TestConfig>>
+ aron_component_config_plugin_{nullptr};
std::mutex write_mutex;
-
};
-}
\ No newline at end of file
+} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index 56c025a1260c2398b0e25d3e9f990146d5285fdc..aedde35678b0490bd8b780f75c36cbd9d96fb030 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -68,7 +68,7 @@ namespace armarx::articulated_object
armem::client::plugins::ReaderWriterPlugin<
::armarx::armem::articulated_object::ArticulatedObjectWriter>*
articulatedObjectWriterPlugin = nullptr;
-
+
armem::client::plugins::ReaderWriterPlugin<
::armarx::armem::articulated_object::ArticulatedObjectReader>*
articulatedObjectReaderPlugin = nullptr;
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/ArticulatedObjectLocalizerExampleTest.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/ArticulatedObjectLocalizerExampleTest.cpp
index 551ba1265648103ef7d94282bc320ea486e3e3a6..2d4b8abf9fdcfb2cecf428c3d8fabe40041742a6 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/ArticulatedObjectLocalizerExampleTest.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/test/ArticulatedObjectLocalizerExampleTest.cpp
@@ -24,9 +24,10 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
#include "../ArticulatedObjectLocalizerExample.h"
+#include <RobotAPI/Test.h>
+
#include <iostream>
BOOST_AUTO_TEST_CASE(testExample)
diff --git a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
index 96b0f863c87245ddf16d84c983e08407f39e3331..8f0b9579017498242887b60b3827d49aff59ec9c 100644
--- a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
+++ b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.cpp
@@ -21,40 +21,43 @@
*/
#include "CyberGloveObserver.h"
+
#include <ArmarXCore/observers/variant/TimestampVariant.h>
namespace armarx
{
- void CyberGloveObserver::onInitObserver()
+ void
+ CyberGloveObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("CyberGloveTopicName").getValue());
}
-
- void CyberGloveObserver::onConnectObserver()
+ void
+ CyberGloveObserver::onConnectObserver()
{
lastUpdate = TimeUtil::GetTime();
}
-
//void CyberGloveObserver::onDisconnectComponent()
//{
//}
- void CyberGloveObserver::onExitObserver()
+ void
+ CyberGloveObserver::onExitObserver()
{
-
}
- PropertyDefinitionsPtr CyberGloveObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ CyberGloveObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new CyberGloveObserverPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new CyberGloveObserverPropertyDefinitions(getConfigIdentifier()));
}
- void CyberGloveObserver::reportGloveValues(const CyberGloveValues& gloveValues, const Ice::Current&)
+ void
+ CyberGloveObserver::reportGloveValues(const CyberGloveValues& gloveValues, const Ice::Current&)
{
std::unique_lock lock(dataMutex);
@@ -76,38 +79,54 @@ namespace armarx
offerOrUpdateDataField(name, "name", Variant(name), "Name of the prostesis");
- offerOrUpdateDataField(name, "thumbCMC", Variant(gloveValues.thumbCMC), "Value of thumbCMC");
- offerOrUpdateDataField(name, "thumbMCP", Variant(gloveValues.thumbMCP), "Value of thumbMCP");
+ offerOrUpdateDataField(
+ name, "thumbCMC", Variant(gloveValues.thumbCMC), "Value of thumbCMC");
+ offerOrUpdateDataField(
+ name, "thumbMCP", Variant(gloveValues.thumbMCP), "Value of thumbMCP");
offerOrUpdateDataField(name, "thumbIP", Variant(gloveValues.thumbIP), "Value of thumbIP");
- offerOrUpdateDataField(name, "thumbAbd", Variant(gloveValues.thumbAbd), "Value of thumbAbd");
- offerOrUpdateDataField(name, "indexMCP", Variant(gloveValues.indexMCP), "Value of indexMCP");
- offerOrUpdateDataField(name, "indexPIP", Variant(gloveValues.indexPIP), "Value of indexPIP");
- offerOrUpdateDataField(name, "indexDIP", Variant(gloveValues.indexDIP), "Value of indexDIP");
- offerOrUpdateDataField(name, "middleMCP", Variant(gloveValues.middleMCP), "Value of middleMCP");
- offerOrUpdateDataField(name, "middlePIP", Variant(gloveValues.middlePIP), "Value of middlePIP");
- offerOrUpdateDataField(name, "middleDIP", Variant(gloveValues.middleDIP), "Value of middleDIP");
- offerOrUpdateDataField(name, "middleAbd", Variant(gloveValues.middleAbd), "Value of middleAbd");
+ offerOrUpdateDataField(
+ name, "thumbAbd", Variant(gloveValues.thumbAbd), "Value of thumbAbd");
+ offerOrUpdateDataField(
+ name, "indexMCP", Variant(gloveValues.indexMCP), "Value of indexMCP");
+ offerOrUpdateDataField(
+ name, "indexPIP", Variant(gloveValues.indexPIP), "Value of indexPIP");
+ offerOrUpdateDataField(
+ name, "indexDIP", Variant(gloveValues.indexDIP), "Value of indexDIP");
+ offerOrUpdateDataField(
+ name, "middleMCP", Variant(gloveValues.middleMCP), "Value of middleMCP");
+ offerOrUpdateDataField(
+ name, "middlePIP", Variant(gloveValues.middlePIP), "Value of middlePIP");
+ offerOrUpdateDataField(
+ name, "middleDIP", Variant(gloveValues.middleDIP), "Value of middleDIP");
+ offerOrUpdateDataField(
+ name, "middleAbd", Variant(gloveValues.middleAbd), "Value of middleAbd");
offerOrUpdateDataField(name, "ringMCP", Variant(gloveValues.ringMCP), "Value of ringMCP");
offerOrUpdateDataField(name, "ringPIP", Variant(gloveValues.ringPIP), "Value of ringPIP");
offerOrUpdateDataField(name, "ringDIP", Variant(gloveValues.ringDIP), "Value of ringDIP");
offerOrUpdateDataField(name, "ringAbd", Variant(gloveValues.ringAbd), "Value of ringAbd");
- offerOrUpdateDataField(name, "littleMCP", Variant(gloveValues.littleMCP), "Value of littleMCP");
- offerOrUpdateDataField(name, "littlePIP", Variant(gloveValues.littlePIP), "Value of littlePIP");
- offerOrUpdateDataField(name, "littleDIP", Variant(gloveValues.littleDIP), "Value of littleDIP");
- offerOrUpdateDataField(name, "littleAbd", Variant(gloveValues.littleAbd), "Value of littleAbd");
- offerOrUpdateDataField(name, "palmArch", Variant(gloveValues.palmArch), "Value of palmArch");
- offerOrUpdateDataField(name, "wristFlex", Variant(gloveValues.wristFlex), "Value of wristFlex");
- offerOrUpdateDataField(name, "wristAbd", Variant(gloveValues.wristAbd), "Value of wristAbd");
+ offerOrUpdateDataField(
+ name, "littleMCP", Variant(gloveValues.littleMCP), "Value of littleMCP");
+ offerOrUpdateDataField(
+ name, "littlePIP", Variant(gloveValues.littlePIP), "Value of littlePIP");
+ offerOrUpdateDataField(
+ name, "littleDIP", Variant(gloveValues.littleDIP), "Value of littleDIP");
+ offerOrUpdateDataField(
+ name, "littleAbd", Variant(gloveValues.littleAbd), "Value of littleAbd");
+ offerOrUpdateDataField(
+ name, "palmArch", Variant(gloveValues.palmArch), "Value of palmArch");
+ offerOrUpdateDataField(
+ name, "wristFlex", Variant(gloveValues.wristFlex), "Value of wristFlex");
+ offerOrUpdateDataField(
+ name, "wristAbd", Variant(gloveValues.wristAbd), "Value of wristAbd");
updateChannel(name);
}
-
-
- CyberGloveValues armarx::CyberGloveObserver::getLatestValues(const Ice::Current&)
+ CyberGloveValues
+ armarx::CyberGloveObserver::getLatestValues(const Ice::Current&)
{
std::unique_lock lock(dataMutex);
return latestValues;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h
index 45eb2f52a2c86c47c8c0272a6f2c55ce9c39b865..b35f9ee8bcd8a57acc93a0dc0185d36031de22e6 100644
--- a/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h
+++ b/source/RobotAPI/components/CyberGloveObserver/CyberGloveObserver.h
@@ -23,12 +23,13 @@
#pragma once
-#include <RobotAPI/interface/units/CyberGloveObserverInterface.h>
-#include <ArmarXCore/observers/Observer.h>
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <mutex>
+
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/observers/Observer.h>
-#include <mutex>
+#include <RobotAPI/interface/units/CyberGloveObserverInterface.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
namespace armarx
{
@@ -36,15 +37,15 @@ namespace armarx
* @class CyberGloveObserverPropertyDefinitions
* @brief
*/
- class CyberGloveObserverPropertyDefinitions:
- public armarx::ObserverPropertyDefinitions
+ class CyberGloveObserverPropertyDefinitions : public armarx::ObserverPropertyDefinitions
{
public:
- CyberGloveObserverPropertyDefinitions(std::string prefix):
+ CyberGloveObserverPropertyDefinitions(std::string prefix) :
armarx::ObserverPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
- defineOptionalProperty<std::string>("CyberGloveTopicName", "CyberGloveValues", "Name of the CyberGlove Topic");
+ defineOptionalProperty<std::string>(
+ "CyberGloveTopicName", "CyberGloveValues", "Name of the CyberGlove Topic");
}
};
@@ -59,17 +60,18 @@ namespace armarx
*
* Detailed description of class CyberGloveObserver.
*/
- class CyberGloveObserver :
- virtual public Observer,
- virtual public CyberGloveObserverInterface
+ class CyberGloveObserver : virtual public Observer, virtual public CyberGloveObserverInterface
{
public:
- CyberGloveObserver() {}
+ CyberGloveObserver()
+ {
+ }
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "CyberGloveObserver";
}
@@ -101,7 +103,6 @@ namespace armarx
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
private:
-
std::mutex dataMutex;
CyberGloveValues latestValues;
IceUtil::Time lastUpdate;
@@ -110,6 +111,5 @@ namespace armarx
public:
void reportGloveValues(const CyberGloveValues& gloveValues, const Ice::Current&) override;
CyberGloveValues getLatestValues(const Ice::Current&) override;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
index 4332dd069ac78fa95f30f953db8fdbab556f8201..4c840e631525b1f0c4d71c8780a0f6c09755802b 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
+++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
@@ -28,15 +28,15 @@
#include <cmath>
#include <fstream>
#include <limits>
-#include <string>
#include <map>
#include <memory>
#include <mutex>
+#include <string>
// Simox
-#include <VirtualRobot/Robot.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/convert.h>
+#include <VirtualRobot/Robot.h>
// Ice
#include <Ice/Current.h>
@@ -54,7 +54,6 @@ namespace doa = DynamicObstacleAvoidance;
// RobotAPI
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
namespace
{
@@ -68,24 +67,21 @@ namespace
std::make_shared<doa::Ellipsoid>(obstacle.name,
doa::State(pos / 1000, ori),
Eigen::Array3d(obstacle.safetyMarginX / 1000,
- obstacle.safetyMarginY / 1000,
- obstacle.safetyMarginZ / 1000));
-
- obstacle_ptr->set_axis_lengths(Eigen::Array3d(obstacle.axisLengthX,
- obstacle.axisLengthY,
- obstacle.axisLengthZ) / 1000);
- obstacle_ptr->set_reference_position(Eigen::Vector3d(obstacle.refPosX,
- obstacle.refPosY,
- obstacle.refPosZ) / 1000);
- obstacle_ptr->set_curvature_factor(Eigen::Array3d(obstacle.curvatureX,
- obstacle.curvatureY,
- obstacle.curvatureZ));
+ obstacle.safetyMarginY / 1000,
+ obstacle.safetyMarginZ / 1000));
+
+ obstacle_ptr->set_axis_lengths(
+ Eigen::Array3d(obstacle.axisLengthX, obstacle.axisLengthY, obstacle.axisLengthZ) /
+ 1000);
+ obstacle_ptr->set_reference_position(
+ Eigen::Vector3d(obstacle.refPosX, obstacle.refPosY, obstacle.refPosZ) / 1000);
+ obstacle_ptr->set_curvature_factor(
+ Eigen::Array3d(obstacle.curvatureX, obstacle.curvatureY, obstacle.curvatureZ));
return obstacle_ptr;
}
-}
-
+} // namespace
namespace armarx::obstacledetection
{
@@ -139,12 +135,9 @@ namespace armarx::obstacledetection
}
}
-}
-
-
-const std::string
-armarx::DSObstacleAvoidance::default_name = "DSObstacleAvoidance";
+} // namespace armarx::obstacledetection
+const std::string armarx::DSObstacleAvoidance::default_name = "DSObstacleAvoidance";
armarx::DSObstacleAvoidance::DSObstacleAvoidance()
{
@@ -159,7 +152,6 @@ armarx::DSObstacleAvoidance::DSObstacleAvoidance()
m_doa.cfg.weight_power = 2.0;
}
-
void
armarx::DSObstacleAvoidance::onInitComponent()
{
@@ -201,7 +193,6 @@ armarx::DSObstacleAvoidance::onInitComponent()
ARMARX_DEBUG << "Initialized " << getName() << ".";
}
-
void
armarx::DSObstacleAvoidance::onConnectComponent()
{
@@ -210,23 +201,20 @@ armarx::DSObstacleAvoidance::onConnectComponent()
if (m_vis.enabled)
{
m_vis.task = new PeriodicTask<DSObstacleAvoidance>(
- this,
- &DSObstacleAvoidance::run_visualization, m_vis.task_interval);
+ this, &DSObstacleAvoidance::run_visualization, m_vis.task_interval);
m_vis.task->start();
}
if (m_watchdog.enabled)
{
m_watchdog.task = new PeriodicTask<DSObstacleAvoidance>(
- this,
- &DSObstacleAvoidance::run_watchdog, m_watchdog.task_interval);
+ this, &DSObstacleAvoidance::run_watchdog, m_watchdog.task_interval);
m_watchdog.task->start();
}
ARMARX_DEBUG << "Connected " << getName() << ".";
}
-
void
armarx::DSObstacleAvoidance::onDisconnectComponent()
{
@@ -245,7 +233,6 @@ armarx::DSObstacleAvoidance::onDisconnectComponent()
ARMARX_DEBUG << "Disconnected " << getName() << ".";
}
-
void
armarx::DSObstacleAvoidance::onExitComponent()
{
@@ -254,26 +241,21 @@ armarx::DSObstacleAvoidance::onExitComponent()
ARMARX_DEBUG << "Exited " << getName() << ".";
}
-
std::string
-armarx::DSObstacleAvoidance::getDefaultName()
-const
+armarx::DSObstacleAvoidance::getDefaultName() const
{
return default_name;
}
-
armarx::dsobstacleavoidance::Config
armarx::DSObstacleAvoidance::getConfig(const Ice::Current&)
{
return m_doa.cfg;
}
-
void
-armarx::DSObstacleAvoidance::updateObstacle(
- const armarx::obstacledetection::Obstacle& obstacle,
- const Ice::Current&)
+armarx::DSObstacleAvoidance::updateObstacle(const armarx::obstacledetection::Obstacle& obstacle,
+ const Ice::Current&)
{
ARMARX_DEBUG << "Adding/updating obstacle: " << obstacle.name << ".";
@@ -293,7 +275,6 @@ armarx::DSObstacleAvoidance::updateObstacle(
ARMARX_DEBUG << "Added/updated obstacle: " << obstacle.name << ".";
}
-
void
armarx::DSObstacleAvoidance::removeObstacle(const std::string& obstacle_name, const Ice::Current&)
{
@@ -312,7 +293,6 @@ armarx::DSObstacleAvoidance::removeObstacle(const std::string& obstacle_name, co
ARMARX_DEBUG << "Removed obstacle: " << obstacle_name << ".";
}
-
std::vector<armarx::obstacledetection::Obstacle>
armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&)
{
@@ -324,14 +304,17 @@ armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&)
for (auto& [doa_name, doa_obstacle] : m_doa.env)
{
ARMARX_DEBUG << "Got an obtascle";
- std::shared_ptr<doa::Ellipsoid> doa_ellipsoid = std::dynamic_pointer_cast<doa::Ellipsoid>(doa_obstacle);
+ std::shared_ptr<doa::Ellipsoid> doa_ellipsoid =
+ std::dynamic_pointer_cast<doa::Ellipsoid>(doa_obstacle);
obstacledetection::Obstacle obstacle;
obstacle.name = doa_name;
obstacle.posX = doa_ellipsoid->get_position()(0) * 1000;
obstacle.posY = doa_ellipsoid->get_position()(1) * 1000;
obstacle.posZ = doa_ellipsoid->get_position()(2) * 1000;
- obstacle.yaw = simox::math::mat3f_to_rpy(doa_ellipsoid->get_orientation().toRotationMatrix().cast<float>()).z();
+ obstacle.yaw = simox::math::mat3f_to_rpy(
+ doa_ellipsoid->get_orientation().toRotationMatrix().cast<float>())
+ .z();
obstacle.axisLengthX = doa_ellipsoid->get_axis_lengths(0) * 1000;
obstacle.axisLengthY = doa_ellipsoid->get_axis_lengths(1) * 1000;
obstacle.axisLengthZ = doa_ellipsoid->get_axis_lengths(2) * 1000;
@@ -353,11 +336,9 @@ armarx::DSObstacleAvoidance::getObstacles(const Ice::Current&)
return obstacle_list;
}
-
Eigen::Vector3f
-armarx::DSObstacleAvoidance::modulateVelocity(
- const obstacleavoidance::Agent& agent,
- const Ice::Current&)
+armarx::DSObstacleAvoidance::modulateVelocity(const obstacleavoidance::Agent& agent,
+ const Ice::Current&)
{
ARMARX_DEBUG << "Modulate velocity";
@@ -380,14 +361,13 @@ armarx::DSObstacleAvoidance::modulateVelocity(
}
// Modulate velocity given the agent, the environment, and additional parameters.
- Eigen::Vector3d modulated_vel = doa::Modulation::modulate_velocity(
- doa_agent,
- m_doa.env,
- m_doa.cfg.local_modulation,
- m_doa.cfg.repulsion,
- m_doa.cfg.planar_modulation,
- m_doa.cfg.critical_distance,
- m_doa.cfg.weight_power);
+ Eigen::Vector3d modulated_vel = doa::Modulation::modulate_velocity(doa_agent,
+ m_doa.env,
+ m_doa.cfg.local_modulation,
+ m_doa.cfg.repulsion,
+ m_doa.cfg.planar_modulation,
+ m_doa.cfg.critical_distance,
+ m_doa.cfg.weight_power);
// Convert output back to mm.
modulated_vel *= 1000;
@@ -403,7 +383,6 @@ armarx::DSObstacleAvoidance::modulateVelocity(
return modulated_vel.cast<float>();
}
-
void
armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&)
{
@@ -482,7 +461,6 @@ armarx::DSObstacleAvoidance::updateEnvironment(const Ice::Current&)
<< sw.stop() << ".";
}
-
void
armarx::DSObstacleAvoidance::writeDebugPlots(const std::string& filename, const Ice::Current&)
{
@@ -495,8 +473,8 @@ armarx::DSObstacleAvoidance::writeDebugPlots(const std::string& filename, const
const bool show = false;
try
{
- doa::PlottingTools::plot_configuration(m_doa.env.get_obstacle_list(), filename_annotated,
- show, not m_doa.cfg.only_2d);
+ doa::PlottingTools::plot_configuration(
+ m_doa.env.get_obstacle_list(), filename_annotated, show, not m_doa.cfg.only_2d);
}
catch (...)
{
@@ -505,11 +483,9 @@ armarx::DSObstacleAvoidance::writeDebugPlots(const std::string& filename, const
}
}
-
void
armarx::DSObstacleAvoidance::sanity_check_obstacle(
- const armarx::obstacledetection::Obstacle& obstacle)
-const
+ const armarx::obstacledetection::Obstacle& obstacle) const
{
ARMARX_DEBUG << "Sanity checking obstacle `" << obstacle.name << "`.";
@@ -532,7 +508,6 @@ const
}
}
-
void
armarx::DSObstacleAvoidance::run_visualization()
{
@@ -571,9 +546,8 @@ armarx::DSObstacleAvoidance::run_visualization()
const double refPosZ = m_doa.cfg.only_2d ? 1 : obstacle.refPosZ;
const double axisLengthZ = m_doa.cfg.only_2d ? 1 : obstacle.axisLengthZ;
- const Eigen::Matrix4f pose = simox::math::pos_rpy_to_mat4f(
- obstacle.posX, obstacle.posY, posZ,
- 0, 0, obstacle.yaw);
+ const Eigen::Matrix4f pose =
+ simox::math::pos_rpy_to_mat4f(obstacle.posX, obstacle.posY, posZ, 0, 0, obstacle.yaw);
const Eigen::Vector3f dim(obstacle.axisLengthX, obstacle.axisLengthY, axisLengthZ);
const Eigen::Vector3f sm(obstacle.safetyMarginX, obstacle.safetyMarginY, safetyMarginZ);
const Eigen::Vector3f curv(obstacle.curvatureX, obstacle.curvatureY, obstacle.curvatureZ);
@@ -582,43 +556,37 @@ armarx::DSObstacleAvoidance::run_visualization()
{
layer_obs.add(Cylindroid{obstacle.name}
- .pose(pose)
- .axisLengths(dim.head<2>())
- .height(dim(2))
- .curvature(curv.head<2>())
- .color(color));
+ .pose(pose)
+ .axisLengths(dim.head<2>())
+ .height(dim(2))
+ .curvature(curv.head<2>())
+ .color(color));
layer_sms.add(Cylindroid{obstacle.name + "_sm"}
- .pose(pose)
- .axisLengths((dim + sm).head<2>())
- .height(dim(2) + sm(2))
- .curvature(curv.head<2>())
- .color(color_m));
+ .pose(pose)
+ .axisLengths((dim + sm).head<2>())
+ .height(dim(2) + sm(2))
+ .curvature(curv.head<2>())
+ .color(color_m));
}
else
{
- layer_obs.add(Ellipsoid{obstacle.name}
- .pose(pose)
- .axisLengths(dim)
- .curvature(curv)
- .color(color));
+ layer_obs.add(
+ Ellipsoid{obstacle.name}.pose(pose).axisLengths(dim).curvature(curv).color(color));
layer_sms.add(Ellipsoid{obstacle.name + "_sm"}
- .pose(pose)
- .axisLengths(dim + sm)
- .curvature(curv)
- .color(color_m));
+ .pose(pose)
+ .axisLengths(dim + sm)
+ .curvature(curv)
+ .color(color_m));
}
layer_rps.add(Sphere{obstacle.name + "_rp"}
- .position(Eigen::Vector3f(obstacle.refPosX, obstacle.refPosY, refPosZ))
- .radius(35)
- .color(Color::green()));
-
- layer_bbs.add(Box{obstacle.name + "_bb"}
- .pose(pose)
- .size(dim)
- .color(color));
+ .position(Eigen::Vector3f(obstacle.refPosX, obstacle.refPosY, refPosZ))
+ .radius(35)
+ .color(Color::green()));
+
+ layer_bbs.add(Box{obstacle.name + "_bb"}.pose(pose).size(dim).color(color));
}
m_vis.needs_update = false;
@@ -627,7 +595,6 @@ armarx::DSObstacleAvoidance::run_visualization()
//arviz.commit(layer_bbs); // Do not render bounding boxes by default.
}
-
void
armarx::DSObstacleAvoidance::run_watchdog()
{
@@ -641,8 +608,8 @@ armarx::DSObstacleAvoidance::run_watchdog()
if (needs_env_update and not was_recently_updated)
{
- ARMARX_WARNING << "Environment has not been updated for over "
- << m_watchdog.threshold << ". Have you forgotten to call "
+ ARMARX_WARNING << "Environment has not been updated for over " << m_watchdog.threshold
+ << ". Have you forgotten to call "
<< "`updateEnvironment()` after adding, updating or removing obstacles? "
<< "Forcing update now.";
@@ -650,7 +617,6 @@ armarx::DSObstacleAvoidance::run_watchdog()
}
}
-
armarx::PropertyDefinitionsPtr
armarx::DSObstacleAvoidance::createPropertyDefinitions()
{
@@ -658,7 +624,8 @@ armarx::DSObstacleAvoidance::createPropertyDefinitions()
def->optional(m_vis.enabled, "visualize", "Enable/disable visualization.");
def->optional(m_watchdog.enabled, "udpate_watchdog", "Run environment update watchdog.");
- def->optional(m_doa.load_obstacles_from_file, "load_obstacles_from",
+ def->optional(m_doa.load_obstacles_from_file,
+ "load_obstacles_from",
"Path to JSON file to load initial obstacles from.");
// "doa" namespace for properties that directly configure the library.
@@ -667,8 +634,8 @@ armarx::DSObstacleAvoidance::createPropertyDefinitions()
def->optional(m_doa.cfg.agent_safety_margin, "doa.agent_safety_margin", "Agent safety margin.");
def->optional(m_doa.cfg.local_modulation, "doa.local_modulation", "Local modulation on/off.");
def->optional(m_doa.cfg.repulsion, "doa.repulsion", "Repulsion on/off.");
- def->optional(m_doa.cfg.planar_modulation, "doa.planar_modulation",
- "Planar modulation on/off.");
+ def->optional(
+ m_doa.cfg.planar_modulation, "doa.planar_modulation", "Planar modulation on/off.");
def->optional(m_doa.cfg.critical_distance, "doa.critical_distance", "Critical distance.");
def->optional(m_doa.cfg.weight_power, "doa.weight_power", "Weight power");
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h
index 2f9035f45eb97f749f17e083ae8a150abf061ada..fb40710d0e32fb0c7bfe47f902858630fa632e03 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h
+++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h
@@ -27,10 +27,10 @@
// STD/STL
#include <array>
#include <deque>
-#include <shared_mutex>
-#include <string>
#include <map>
#include <mutex>
+#include <shared_mutex>
+#include <string>
#include <vector>
// Eigen
@@ -49,9 +49,8 @@
#include <ArmarXCore/util/time.h>
// RobotAPI
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.h>
-
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
namespace armarx
{
@@ -63,105 +62,69 @@ namespace armarx
{
public:
-
DSObstacleAvoidance();
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string
- getDefaultName()
- const override;
+ std::string getDefaultName() const override;
- dsobstacleavoidance::Config
- getConfig(const Ice::Current& = Ice::emptyCurrent)
- override;
+ dsobstacleavoidance::Config getConfig(const Ice::Current& = Ice::emptyCurrent) override;
- void
- updateObstacle(
- const obstacledetection::Obstacle& obstacle,
- const Ice::Current& = Ice::emptyCurrent)
- override;
+ void updateObstacle(const obstacledetection::Obstacle& obstacle,
+ const Ice::Current& = Ice::emptyCurrent) override;
std::vector<obstacledetection::Obstacle>
- getObstacles(const Ice::Current& = Ice::emptyCurrent)
- override;
+ getObstacles(const Ice::Current& = Ice::emptyCurrent) override;
- void
- removeObstacle(
- const std::string& obstacle_name,
- const Ice::Current& = Ice::emptyCurrent)
- override;
+ void removeObstacle(const std::string& obstacle_name,
+ const Ice::Current& = Ice::emptyCurrent) override;
- Eigen::Vector3f
- modulateVelocity(
- const obstacleavoidance::Agent& agent,
- const Ice::Current& = Ice::emptyCurrent)
- override;
+ Eigen::Vector3f modulateVelocity(const obstacleavoidance::Agent& agent,
+ const Ice::Current& = Ice::emptyCurrent) override;
- void
- updateEnvironment(const Ice::Current& = Ice::emptyCurrent)
- override;
+ void updateEnvironment(const Ice::Current& = Ice::emptyCurrent) override;
- void
- writeDebugPlots(const std::string& filename, const Ice::Current& = Ice::emptyCurrent)
- override;
+ void writeDebugPlots(const std::string& filename,
+ const Ice::Current& = Ice::emptyCurrent) override;
protected:
-
/**
* @see armarx::ManagedIceObject::onInitComponent()
*/
- void
- onInitComponent()
- override;
+ void onInitComponent() override;
/**
* @see armarx::ManagedIceObject::onConnectComponent()
*/
- void
- onConnectComponent()
- override;
+ void onConnectComponent() override;
/**
* @see armarx::ManagedIceObject::onDisconnectComponent()
*/
- void
- onDisconnectComponent()
- override;
+ void onDisconnectComponent() override;
/**
* @see armarx::ManagedIceObject::onExitComponent()
*/
- void
- onExitComponent()
- override;
+ void onExitComponent() override;
/**
* @see PropertyUser::createPropertyDefinitions()
*/
- armarx::PropertyDefinitionsPtr
- createPropertyDefinitions()
- override;
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
private:
+ void sanity_check_obstacle(const obstacledetection::Obstacle& obstacle) const;
- void
- sanity_check_obstacle(const obstacledetection::Obstacle& obstacle)
- const;
-
- void
- run_watchdog();
+ void run_watchdog();
- void
- run_visualization();
+ void run_visualization();
public:
-
static const std::string default_name;
private:
-
struct visualization
{
bool enabled = true;
@@ -217,7 +180,6 @@ namespace armarx
DSObstacleAvoidance::dynamic_obstacle_avoidance m_doa;
DSObstacleAvoidance::buffer m_buf;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/main.cpp b/source/RobotAPI/components/DSObstacleAvoidance/main.cpp
index 350f59edd246a0f74e3cbabfb3f95871f4d12cf0..0a29e22fcd0b2798688551035923007cda733392 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/main.cpp
+++ b/source/RobotAPI/components/DSObstacleAvoidance/main.cpp
@@ -25,15 +25,15 @@
#include <string>
// ArmarX
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
// armar6_skills
#include <RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.h>
-
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
using namespace armarx;
const std::string name = DSObstacleAvoidance::default_name;
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 928e2d47b22cfb858ed966a77a93d6e892f7673b..a6e113c95c86aacb0c36e2571bbece60cbdd0f48 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -25,78 +25,81 @@
#include "DebugDrawerComponent.h"
-#include <RobotAPI/libraries/core/math/ColorUtils.h>
-
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
#include <VirtualRobot/Visualization/VisualizationFactory.h>
-#include <Inventor/nodes/SoUnits.h>
-#include <Inventor/nodes/SoCube.h>
-#include <Inventor/nodes/SoMaterial.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <RobotAPI/libraries/core/math/ColorUtils.h>
+
+#include <Inventor/SbVec3f.h>
+#include <Inventor/actions/SoWriteAction.h>
+#include <Inventor/fields/SoMFColor.h>
+#include <Inventor/fields/SoMFVec3f.h>
#include <Inventor/nodes/SoAnnotation.h>
-#include <Inventor/nodes/SoTransform.h>
-#include <Inventor/nodes/SoFont.h>
-#include <Inventor/nodes/SoText2.h>
-#include <Inventor/nodes/SoSphere.h>
-#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoComplexity.h>
#include <Inventor/nodes/SoCoordinate3.h>
-#include <Inventor/nodes/SoPointSet.h>
-#include <Inventor/actions/SoWriteAction.h>
-#include <Inventor/nodes/SoTranslation.h>
-#include <Inventor/nodes/SoMaterialBinding.h>
+#include <Inventor/nodes/SoCube.h>
+#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoDrawStyle.h>
+#include <Inventor/nodes/SoFont.h>
#include <Inventor/nodes/SoLineSet.h>
-#include <Inventor/SbVec3f.h>
-#include <Inventor/fields/SoMFVec3f.h>
-#include <Inventor/fields/SoMFColor.h>
-#include <Inventor/nodes/SoShapeHints.h>
+#include <Inventor/nodes/SoMaterial.h>
+#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoMatrixTransform.h>
-
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
-#include <VirtualRobot/Visualization/TriMeshModel.h>
+#include <Inventor/nodes/SoPointSet.h>
+#include <Inventor/nodes/SoShapeHints.h>
+#include <Inventor/nodes/SoSphere.h>
+#include <Inventor/nodes/SoText2.h>
+#include <Inventor/nodes/SoTransform.h>
+#include <Inventor/nodes/SoTranslation.h>
+#include <Inventor/nodes/SoUnits.h>
using namespace VirtualRobot;
-#define SELECTION_NAME_PREFIX ("selection_" + std::to_string(reinterpret_cast<std::uintptr_t>(this)))
+#define SELECTION_NAME_PREFIX \
+ ("selection_" + std::to_string(reinterpret_cast<std::uintptr_t>(this)))
#define SELECTION_NAME_SPLITTER "____"
-#define SELECTION_NAME(layerName, elementName) simox::alg::replace_all(std::string(SELECTION_NAME_PREFIX + "_" + layerName + SELECTION_NAME_SPLITTER + elementName), " ", "_").c_str()
+#define SELECTION_NAME(layerName, elementName) \
+ simox::alg::replace_all(std::string(SELECTION_NAME_PREFIX + "_" + layerName + \
+ SELECTION_NAME_SPLITTER + elementName), \
+ " ", \
+ "_") \
+ .c_str()
namespace armarx
{
std::recursive_mutex DebugDrawerComponent::selectionMutex;
- void selection_callback(void* userdata, SoPath* path)
+ void
+ selection_callback(void* userdata, SoPath* path)
{
if (userdata)
{
- ((DebugDrawerComponent*) userdata)->selectionCallback();
+ ((DebugDrawerComponent*)userdata)->selectionCallback();
}
}
- void deselection_callback(void* userdata, SoPath* path)
+ void
+ deselection_callback(void* userdata, SoPath* path)
{
if (userdata)
{
- ((DebugDrawerComponent*) userdata)->deselectionCallback();
+ ((DebugDrawerComponent*)userdata)->deselectionCallback();
}
}
- static const std::string DEBUG_LAYER_NAME
- {
- "debug"
- };
+ static const std::string DEBUG_LAYER_NAME{"debug"};
- DebugDrawerComponent::DebugDrawerComponent():
+ DebugDrawerComponent::DebugDrawerComponent() :
mutex(new RecursiveMutex()),
dataUpdateMutex(new RecursiveMutex()),
topicMutex(new RecursiveMutex()),
@@ -123,7 +126,8 @@ namespace armarx
selectionNode->addChild(layerMainNode);
}
- void DebugDrawerComponent::enableSelections(const std::string& layerName, const ::Ice::Current&)
+ void
+ DebugDrawerComponent::enableSelections(const std::string& layerName, const ::Ice::Current&)
{
auto l = getScopedVisuLock();
@@ -131,7 +135,8 @@ namespace armarx
selectableLayers.insert(layerName);
}
- void DebugDrawerComponent::disableSelections(const std::string& layerName, const ::Ice::Current&)
+ void
+ DebugDrawerComponent::disableSelections(const std::string& layerName, const ::Ice::Current&)
{
auto l = getScopedVisuLock();
@@ -146,7 +151,8 @@ namespace armarx
}
}
- void DebugDrawerComponent::clearSelections(const std::string& layerName, const Ice::Current&)
+ void
+ DebugDrawerComponent::clearSelections(const std::string& layerName, const Ice::Current&)
{
auto l = getScopedVisuLock();
std::unique_lock l2(selectionMutex);
@@ -173,14 +179,18 @@ namespace armarx
installSelectionCallbacks();
}
- void DebugDrawerComponent::select(const std::string& layerName, const std::string& elementName, const Ice::Current&)
+ void
+ DebugDrawerComponent::select(const std::string& layerName,
+ const std::string& elementName,
+ const Ice::Current&)
{
auto l = getScopedVisuLock();
std::unique_lock l2(selectionMutex);
removeSelectionCallbacks();
- ARMARX_DEBUG << "Selecting element '" << elementName << "' on layer '" << layerName << "' (internal name: " << SELECTION_NAME(layerName, elementName) << ")";
+ ARMARX_DEBUG << "Selecting element '" << elementName << "' on layer '" << layerName
+ << "' (internal name: " << SELECTION_NAME(layerName, elementName) << ")";
SoNode* n = SoSelection::getByName(SELECTION_NAME(layerName, elementName));
if (n)
@@ -198,14 +208,18 @@ namespace armarx
installSelectionCallbacks();
}
- void DebugDrawerComponent::deselect(const std::string& layerName, const std::string& elementName, const Ice::Current&)
+ void
+ DebugDrawerComponent::deselect(const std::string& layerName,
+ const std::string& elementName,
+ const Ice::Current&)
{
auto l = getScopedVisuLock();
std::unique_lock l2(selectionMutex);
removeSelectionCallbacks();
- ARMARX_DEBUG << "Deselecting element '" << elementName << "' on layer '" << layerName << "' (internal name: " << SELECTION_NAME(layerName, elementName) << ")";
+ ARMARX_DEBUG << "Deselecting element '" << elementName << "' on layer '" << layerName
+ << "' (internal name: " << SELECTION_NAME(layerName, elementName) << ")";
SoNode* n = SoSelection::getByName(SELECTION_NAME(layerName, elementName));
if (n)
@@ -223,33 +237,39 @@ namespace armarx
installSelectionCallbacks();
}
- void DebugDrawerComponent::selectionCallback()
+ void
+ DebugDrawerComponent::selectionCallback()
{
std::unique_lock l(*topicMutex);
listenerPrx->reportSelectionChanged(getSelections());
}
- void DebugDrawerComponent::deselectionCallback()
+ void
+ DebugDrawerComponent::deselectionCallback()
{
std::unique_lock l(*topicMutex);
listenerPrx->reportSelectionChanged(getSelections());
}
- void DebugDrawerComponent::installSelectionCallbacks()
+ void
+ DebugDrawerComponent::installSelectionCallbacks()
{
auto l = getScopedVisuLock();
selectionNode->addSelectionCallback(selection_callback, this);
selectionNode->addDeselectionCallback(deselection_callback, this);
}
- void DebugDrawerComponent::removeSelectionCallbacks()
+ void
+ DebugDrawerComponent::removeSelectionCallbacks()
{
auto l = getScopedVisuLock();
selectionNode->removeSelectionCallback(selection_callback, this);
selectionNode->removeDeselectionCallback(deselection_callback, this);
}
- void DebugDrawerComponent::reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const Ice::Current&)
+ void
+ DebugDrawerComponent::reportSelectionChanged(const DebugDrawerSelectionList& selectedElements,
+ const Ice::Current&)
{
auto l = getScopedVisuLock();
std::unique_lock l2(selectionMutex);
@@ -272,7 +292,8 @@ namespace armarx
installSelectionCallbacks();
}
- DebugDrawerSelectionList DebugDrawerComponent::getSelections(const ::Ice::Current&)
+ DebugDrawerSelectionList
+ DebugDrawerComponent::getSelections(const ::Ice::Current&)
{
auto l = getScopedVisuLock();
@@ -300,11 +321,16 @@ namespace armarx
for (auto& l : layers)
{
std::string layer = l.first;
- if (layers[layer].addedBoxVisualizations.find(name) != layers[layer].addedBoxVisualizations.end()
- || layers[layer].addedTextVisualizations.find(name) != layers[layer].addedTextVisualizations.end()
- || layers[layer].addedSphereVisualizations.find(name) != layers[layer].addedSphereVisualizations.end()
- || layers[layer].addedCylinderVisualizations.find(name) != layers[layer].addedCylinderVisualizations.end()
- || layers[layer].addedPolygonVisualizations.find(name) != layers[layer].addedPolygonVisualizations.end())
+ if (layers[layer].addedBoxVisualizations.find(name) !=
+ layers[layer].addedBoxVisualizations.end() ||
+ layers[layer].addedTextVisualizations.find(name) !=
+ layers[layer].addedTextVisualizations.end() ||
+ layers[layer].addedSphereVisualizations.find(name) !=
+ layers[layer].addedSphereVisualizations.end() ||
+ layers[layer].addedCylinderVisualizations.find(name) !=
+ layers[layer].addedCylinderVisualizations.end() ||
+ layers[layer].addedPolygonVisualizations.find(name) !=
+ layers[layer].addedPolygonVisualizations.end())
{
DebugDrawerSelectionElement e;
e.layerName = layer;
@@ -324,7 +350,8 @@ namespace armarx
return selectedElements;
}
- void DebugDrawerComponent::setVisuUpdateTime(float visuUpdatesPerSec)
+ void
+ DebugDrawerComponent::setVisuUpdateTime(float visuUpdatesPerSec)
{
if (timerSensor)
{
@@ -351,7 +378,8 @@ namespace armarx
}*/
}
- void DebugDrawerComponent::onInitComponent()
+ void
+ DebugDrawerComponent::onInitComponent()
{
usingTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
bool enabled = getProperty<bool>("ShowDebugDrawing").getValue();
@@ -379,7 +407,8 @@ namespace armarx
installSelectionCallbacks();
}
- void DebugDrawerComponent::updateVisualizationCB(void* data, SoSensor* sensor)
+ void
+ DebugDrawerComponent::updateVisualizationCB(void* data, SoSensor* sensor)
{
DebugDrawerComponent* drawer = static_cast<DebugDrawerComponent*>(data);
@@ -392,15 +421,17 @@ namespace armarx
drawer->updateVisualization();
}
-
- void DebugDrawerComponent::onConnectComponent()
+ void
+ DebugDrawerComponent::onConnectComponent()
{
verbose = true;
- listenerPrx = getTopic<DebugDrawerListenerPrx>(getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
+ listenerPrx = getTopic<DebugDrawerListenerPrx>(
+ getProperty<std::string>("DebugDrawerSelectionTopic").getValue());
}
- void DebugDrawerComponent::onDisconnectComponent()
+ void
+ DebugDrawerComponent::onDisconnectComponent()
{
std::cout << "onDisconnectComponent debug drawer" << std::endl;
/*verbose = false;
@@ -418,7 +449,8 @@ namespace armarx
}*/
}
- void DebugDrawerComponent::onExitComponent()
+ void
+ DebugDrawerComponent::onExitComponent()
{
ARMARX_INFO << "onExitComponent debug drawer";
verbose = false;
@@ -450,7 +482,8 @@ namespace armarx
}
}
- void DebugDrawerComponent::exportScene(const std::string& filename, const Ice::Current&)
+ void
+ DebugDrawerComponent::exportScene(const std::string& filename, const Ice::Current&)
{
auto l = getScopedVisuLock();
@@ -471,7 +504,10 @@ namespace armarx
sep->unref();
}
- void DebugDrawerComponent::exportLayer(const std::string& filename, const std::string& layerName, const Ice::Current&)
+ void
+ DebugDrawerComponent::exportLayer(const std::string& filename,
+ const std::string& layerName,
+ const Ice::Current&)
{
auto l = getScopedVisuLock();
@@ -498,7 +534,8 @@ namespace armarx
sep->unref();
}
- void DebugDrawerComponent::drawCoordSystem(const CoordData& d)
+ void
+ DebugDrawerComponent::drawCoordSystem(const CoordData& d)
{
auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawing coord system";
@@ -530,7 +567,8 @@ namespace armarx
ARMARX_DEBUG << "end";
}
- void DebugDrawerComponent::drawLine(const LineData& d)
+ void
+ DebugDrawerComponent::drawLine(const LineData& d)
{
auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawLine1" << flush;
@@ -551,7 +589,8 @@ namespace armarx
ARMARX_DEBUG << "drawLine2" << flush;
}
- void DebugDrawerComponent::drawLineSet(const DebugDrawerComponent::LineSetData& d)
+ void
+ DebugDrawerComponent::drawLineSet(const DebugDrawerComponent::LineSetData& d)
{
auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawLineSet";
@@ -573,7 +612,8 @@ namespace armarx
if (d.lineSet.points.size() != 2 * d.lineSet.intensities.size())
{
- ARMARX_WARNING << "Amount of intensities has to be half the amount of points for a line set";
+ ARMARX_WARNING
+ << "Amount of intensities has to be half the amount of points for a line set";
return;
}
@@ -581,16 +621,23 @@ namespace armarx
{
if (!std::isfinite(e.x) || !std::isfinite(e.y) || !std::isfinite(e.z))
{
- ARMARX_WARNING << deactivateSpam(10, d.layerName + d.name) << "A point of lineset " << d.name << " on layer " << d.layerName << " is not finite! this set will not be drawn!";
+ ARMARX_WARNING << deactivateSpam(10, d.layerName + d.name) << "A point of lineset "
+ << d.name << " on layer " << d.layerName
+ << " is not finite! this set will not be drawn!";
return;
}
}
// Initialize color map for affordance visualization
std::vector<VirtualRobot::VisualizationFactory::Color> colors;
- colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorNoIntensity.r, d.lineSet.colorNoIntensity.g, d.lineSet.colorNoIntensity.b));
- colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorFullIntensity.r, d.lineSet.colorFullIntensity.g, d.lineSet.colorFullIntensity.b));
- VirtualRobot::ColorMap visualizationColorMap = VirtualRobot::ColorMap::customColorMap(colors);
+ colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorNoIntensity.r,
+ d.lineSet.colorNoIntensity.g,
+ d.lineSet.colorNoIntensity.b));
+ colors.push_back(VirtualRobot::VisualizationFactory::Color(d.lineSet.colorFullIntensity.r,
+ d.lineSet.colorFullIntensity.g,
+ d.lineSet.colorFullIntensity.b));
+ VirtualRobot::ColorMap visualizationColorMap =
+ VirtualRobot::ColorMap::customColorMap(colors);
// Initialize visualization nodes
SoSeparator* sep = new SoSeparator;
@@ -623,8 +670,11 @@ namespace armarx
{
lineSetValues[i] = 2;
- coordinateValues[2 * i].setValue(d.lineSet.points[2 * i].x, d.lineSet.points[2 * i].y, d.lineSet.points[2 * i].z);
- coordinateValues[2 * i + 1].setValue(d.lineSet.points[2 * i + 1].x, d.lineSet.points[2 * i + 1].y, d.lineSet.points[2 * i + 1].z);
+ coordinateValues[2 * i].setValue(
+ d.lineSet.points[2 * i].x, d.lineSet.points[2 * i].y, d.lineSet.points[2 * i].z);
+ coordinateValues[2 * i + 1].setValue(d.lineSet.points[2 * i + 1].x,
+ d.lineSet.points[2 * i + 1].y,
+ d.lineSet.points[2 * i + 1].z);
if (d.lineSet.useHeatMap)
{
@@ -633,7 +683,8 @@ namespace armarx
}
else
{
- VirtualRobot::VisualizationFactory::Color c = visualizationColorMap.getColor(d.lineSet.intensities[i]);
+ VirtualRobot::VisualizationFactory::Color c =
+ visualizationColorMap.getColor(d.lineSet.intensities[i]);
colorValues[i].setValue(c.r, c.g, c.b);
}
}
@@ -647,7 +698,8 @@ namespace armarx
layer.mainNode->addChild(sep);
}
- void DebugDrawerComponent::drawBox(const BoxData& d)
+ void
+ DebugDrawerComponent::drawBox(const BoxData& d)
{
auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawBox";
@@ -682,7 +734,8 @@ namespace armarx
layer.mainNode->addChild(newS);
}
- void DebugDrawerComponent::drawText(const TextData& d)
+ void
+ DebugDrawerComponent::drawText(const TextData& d)
{
auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawText1";
@@ -727,8 +780,8 @@ namespace armarx
ARMARX_DEBUG << "drawText2";
}
-
- void DebugDrawerComponent::drawSphere(const SphereData& d)
+ void
+ DebugDrawerComponent::drawSphere(const SphereData& d)
{
auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawSphere";
@@ -742,9 +795,12 @@ namespace armarx
return;
}
- if (!std::isfinite(d.position(0)) || !std::isfinite(d.position(1)) || !std::isfinite(d.position(2)))
+ if (!std::isfinite(d.position(0)) || !std::isfinite(d.position(1)) ||
+ !std::isfinite(d.position(2)))
{
- ARMARX_WARNING << deactivateSpam(10, d.layerName + d.name) << "A coordinate of sphere " << d.name << " on layer " << d.layerName << " is not finite! this sphere will not be drawn!";
+ ARMARX_WARNING << deactivateSpam(10, d.layerName + d.name) << "A coordinate of sphere "
+ << d.name << " on layer " << d.layerName
+ << " is not finite! this sphere will not be drawn!";
return;
}
@@ -768,7 +824,8 @@ namespace armarx
layer.mainNode->addChild(sep);
}
- void DebugDrawerComponent::drawCylinder(const DebugDrawerComponent::CylinderData& d)
+ void
+ DebugDrawerComponent::drawCylinder(const DebugDrawerComponent::CylinderData& d)
{
auto l = getScopedVisuLock();
ARMARX_DEBUG << "drawCylinder";
@@ -792,7 +849,8 @@ namespace armarx
// The cylinder extends in y-direction, hence choose the transformation
SoTransform* tr = new SoTransform;
tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
- tr->rotation.setValue(SbRotation(SbVec3f(0, 1, 0), SbVec3f(d.direction.x(), d.direction.y(), d.direction.z())));
+ tr->rotation.setValue(SbRotation(
+ SbVec3f(0, 1, 0), SbVec3f(d.direction.x(), d.direction.y(), d.direction.z())));
sep->addChild(tr);
SoCylinder* cylinder = new SoCylinder;
@@ -805,7 +863,8 @@ namespace armarx
layer.mainNode->addChild(sep);
}
- Eigen::Vector3f GetOrthonormalVectors(Eigen::Vector3f vec, Eigen::Vector3f& dir1, Eigen::Vector3f& dir2)
+ Eigen::Vector3f
+ GetOrthonormalVectors(Eigen::Vector3f vec, Eigen::Vector3f& dir1, Eigen::Vector3f& dir2)
{
vec = vec.normalized();
@@ -817,7 +876,6 @@ namespace armarx
{
dir1 = vec.cross(Eigen::Vector3f(0, 1, 0));
-
}
dir1 = -dir1.normalized();
@@ -825,11 +883,10 @@ namespace armarx
dir2 = vec.cross(dir1).normalized();
return vec;
-
-
}
- void DebugDrawerComponent::drawCircle(const DebugDrawerComponent::CircleData& d)
+ void
+ DebugDrawerComponent::drawCircle(const DebugDrawerComponent::CircleData& d)
{
auto l = getScopedVisuLock();
@@ -843,7 +900,6 @@ namespace armarx
}
-
SoSeparator* sep = new SoSeparator;
SoMaterial* mat = new SoMaterial;
mat->ambientColor.setValue(d.color.r, d.color.g, d.color.b);
@@ -861,13 +917,11 @@ namespace armarx
// auto quat = VirtualRobot::MathTools::getRotation(Eigen::Vector3f::UnitZ(), d.direction);
// Eigen::AngleAxisf rotAA(Eigen::Quaternionf(quat.w, quat.x, quat.y, quat.z));
Eigen::AngleAxisf rotAA(rotGoal);
- tr->rotation.setValue(SbVec3f(rotAA.axis().x(), rotAA.axis().y(), rotAA.axis().z()), rotAA.angle());
+ tr->rotation.setValue(SbVec3f(rotAA.axis().x(), rotAA.axis().y(), rotAA.axis().z()),
+ rotAA.angle());
sep->addChild(tr);
- auto node = CoinVisualizationFactory().createCircleArrow(d.radius,
- d.width,
- d.circleCompletion,
- d.color,
- 16, 30);
+ auto node = CoinVisualizationFactory().createCircleArrow(
+ d.radius, d.width, d.circleCompletion, d.color, 16, 30);
SoNode* circle = dynamic_cast<CoinVisualizationNode&>(*node).getCoinVisualization();
circle->setName(SELECTION_NAME(d.layerName, d.name));
sep->addChild(circle);
@@ -876,7 +930,8 @@ namespace armarx
layer.mainNode->addChild(sep);
}
- void DebugDrawerComponent::drawPointCloud(const PointCloudData& d)
+ void
+ DebugDrawerComponent::drawPointCloud(const PointCloudData& d)
{
auto l = getScopedVisuLock();
@@ -905,13 +960,12 @@ namespace armarx
SoCoordinate3* pclCoords = new SoCoordinate3;
std::vector<SbVec3f> coords;
coords.reserve(pcl.size());
- std::transform(
- pcl.begin(), pcl.end(), std::back_inserter(coords),
- [](const DebugDrawerPointCloudElement & elem)
- {
- return SbVec3f {elem.x, elem.y, elem.z};
- }
- );
+ std::transform(pcl.begin(),
+ pcl.end(),
+ std::back_inserter(coords),
+ [](const DebugDrawerPointCloudElement& elem) {
+ return SbVec3f{elem.x, elem.y, elem.z};
+ });
pclCoords->point.setValues(0, coords.size(), coords.data());
pclSep->addChild(pclCoords);
@@ -925,7 +979,8 @@ namespace armarx
layer.mainNode->addChild(pclSep);
}
- void DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData& d)
+ void
+ DebugDrawerComponent::drawPolygon(const DebugDrawerComponent::PolygonData& d)
{
auto l = getScopedVisuLock();
@@ -946,13 +1001,15 @@ namespace armarx
hints->faceType = SoShapeHints::UNKNOWN_FACE_TYPE;
sep->addChild(hints);
- sep->addChild(VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(d.points, d.colorInner, d.colorBorder, d.lineWidth));
+ sep->addChild(VirtualRobot::CoinVisualizationFactory::CreatePolygonVisualization(
+ d.points, d.colorInner, d.colorBorder, d.lineWidth));
sep->setName(SELECTION_NAME(d.layerName, d.name));
layer.addedPolygonVisualizations[d.name] = sep;
layer.mainNode->addChild(sep);
}
- void DebugDrawerComponent::drawTriMesh(const DebugDrawerComponent::TriMeshData& d)
+ void
+ DebugDrawerComponent::drawTriMesh(const DebugDrawerComponent::TriMeshData& d)
{
auto l = getScopedVisuLock();
@@ -969,7 +1026,8 @@ namespace armarx
for (DrawColor color : d.triMesh.colors)
{
- triMesh->addColor(VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, color.a));
+ triMesh->addColor(
+ VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, color.a));
}
for (DebugDrawerVertex v : d.triMesh.vertices)
@@ -1003,10 +1061,10 @@ namespace armarx
sep->addChild(c);
layer.addedTriMeshVisualizations[d.name] = sep;
layer.mainNode->addChild(sep);
-
}
- void DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData& d)
+ void
+ DebugDrawerComponent::drawArrow(const DebugDrawerComponent::ArrowData& d)
{
auto l = getScopedVisuLock();
@@ -1025,13 +1083,15 @@ namespace armarx
tr->translation.setValue(d.position.x(), d.position.y(), d.position.z());
sep->addChild(tr);
- SoSeparator* sepArrow = VirtualRobot::CoinVisualizationFactory::CreateArrow(d.direction, d.length, d.width, d.color);
+ SoSeparator* sepArrow = VirtualRobot::CoinVisualizationFactory::CreateArrow(
+ d.direction, d.length, d.width, d.color);
sep->addChild(sepArrow);
layer.addedArrowVisualizations[d.name] = sep;
layer.mainNode->addChild(sep);
}
- void DebugDrawerComponent::ensureExistingRobotNodes(DebugDrawerComponent::RobotData& d)
+ void
+ DebugDrawerComponent::ensureExistingRobotNodes(DebugDrawerComponent::RobotData& d)
{
auto l = getScopedVisuLock();
@@ -1045,16 +1105,18 @@ namespace armarx
if (!rob)
{
- ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name << " at layer " << d.layerName;
+ ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name
+ << " at layer " << d.layerName;
return;
}
- std::map < std::string, float >::iterator i = d.configuration.begin();
+ std::map<std::string, float>::iterator i = d.configuration.begin();
while (i != d.configuration.end())
{
if (!rob->hasRobotNode(i->first))
{
- ARMARX_WARNING << deactivateSpam() << "Robot " << rob->getName() << " does not know RobotNode " << i->first;
+ ARMARX_WARNING << deactivateSpam() << "Robot " << rob->getName()
+ << " does not know RobotNode " << i->first;
i = d.configuration.erase(i);
}
else
@@ -1064,7 +1126,8 @@ namespace armarx
}
}
- void DebugDrawerComponent::drawRobot(const DebugDrawerComponent::RobotData& d)
+ void
+ DebugDrawerComponent::drawRobot(const DebugDrawerComponent::RobotData& d)
{
auto l = getScopedVisuLock();
@@ -1081,7 +1144,8 @@ namespace armarx
if (!rob)
{
- ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name << " at layer " << d.layerName;
+ ARMARX_ERROR << deactivateSpam() << "Could not determine robot " << d.name
+ << " at layer " << d.layerName;
return;
}
@@ -1124,7 +1188,6 @@ namespace armarx
{
if (d.color.r < 0 || d.color.g < 0 || d.color.b < 0)
{
-
}
m->diffuseColor = SbColor(d.color.r, d.color.g, d.color.b);
@@ -1152,7 +1215,8 @@ namespace armarx
return;
}
- std::string entryName = simox::alg::replace_all(std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
VirtualRobot::RobotPtr robot = activeRobots[entryName];
std::vector<std::string> nodeNameList;
@@ -1162,7 +1226,9 @@ namespace armarx
nodeNameList.push_back(robot->getRobotNodes()[i]->getName());
}
- for (std::map < std::string, VirtualRobot::VisualizationFactory::Color >::const_iterator it = d.nodeColors.begin();
+ for (std::map<std::string,
+ VirtualRobot::VisualizationFactory::Color>::const_iterator it =
+ d.nodeColors.begin();
it != d.nodeColors.end();
it++)
{
@@ -1175,7 +1241,8 @@ namespace armarx
if (strit == nodeNameList.end())
{
- ARMARX_WARNING << "Cannot find correct robot node name " + nodeName + " for DebugDrawer node color update !!!";
+ ARMARX_WARNING << "Cannot find correct robot node name " + nodeName +
+ " for DebugDrawer node color update !!!";
continue;
}
@@ -1213,24 +1280,26 @@ namespace armarx
nodeMat->transparency = std::max(0.0f, nodeColor.transparency);
nodeMat->setOverride(true);
}
-
}
}
}
}
- VirtualRobot::RobotPtr DebugDrawerComponent::requestRobot(const DebugDrawerComponent::RobotData& d)
+ VirtualRobot::RobotPtr
+ DebugDrawerComponent::requestRobot(const DebugDrawerComponent::RobotData& d)
{
auto l = getScopedVisuLock();
auto& layer = requestLayer(d.layerName);
- std::string entryName = simox::alg::replace_all(std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + d.layerName + "__" + d.name + "__"), " ", "_");
ARMARX_DEBUG << "Requesting robot " << entryName;
if (activeRobots.find(entryName) != activeRobots.end())
{
- ARMARX_DEBUG << "Found robot " << entryName << ":" << activeRobots[entryName]->getName();
+ ARMARX_DEBUG << "Found robot " << entryName << ":"
+ << activeRobots[entryName]->getName();
return activeRobots[entryName];
}
@@ -1238,7 +1307,8 @@ namespace armarx
if (d.robotFile.empty())
{
- ARMARX_INFO << deactivateSpam() << "No robot defined for layer " << d.layerName << ", with name " << d.name;
+ ARMARX_INFO << deactivateSpam() << "No robot defined for layer " << d.layerName
+ << ", with name " << d.name;
return result;
}
@@ -1284,7 +1354,6 @@ namespace armarx
}
catch (...)
{
-
}
if (!result)
@@ -1305,7 +1374,8 @@ namespace armarx
nodeMat->setOverride(false);
nodeSep->addChild(nodeMat);
- CoinVisualizationPtr robNodeVisu = robNode->getVisualization<CoinVisualization>(visuType);
+ CoinVisualizationPtr robNodeVisu =
+ robNode->getVisualization<CoinVisualization>(visuType);
if (robNodeVisu)
{
@@ -1321,14 +1391,18 @@ namespace armarx
}
activeRobots[entryName] = result;
- ARMARX_DEBUG << "setting robot to activeRobots, entryName:" << entryName << ", activeRobots.size():" << activeRobots.size();
+ ARMARX_DEBUG << "setting robot to activeRobots, entryName:" << entryName
+ << ", activeRobots.size():" << activeRobots.size();
layer.addedRobotVisualizations[d.name] = sep;
- ARMARX_DEBUG << "adding sep to layer.addedRobotVisualizations, d.name:" << d.name << ", layer:" << d.layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
+ ARMARX_DEBUG << "adding sep to layer.addedRobotVisualizations, d.name:" << d.name
+ << ", layer:" << d.layerName << ", layer.addedRobotVisualizations.size():"
+ << layer.addedRobotVisualizations.size();
return result;
}
- void DebugDrawerComponent::removeLine(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeLine(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1348,7 +1422,8 @@ namespace armarx
layer.addedLineVisualizations.erase(name);
}
- void DebugDrawerComponent::removeLineSet(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeLineSet(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1368,7 +1443,8 @@ namespace armarx
layer.addedLineSetVisualizations.erase(name);
}
- void DebugDrawerComponent::removeBox(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeBox(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1388,7 +1464,8 @@ namespace armarx
layer.addedBoxVisualizations.erase(name);
}
- void DebugDrawerComponent::removeText(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeText(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1408,7 +1485,8 @@ namespace armarx
layer.addedTextVisualizations.erase(name);
}
- void DebugDrawerComponent::removeSphere(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeSphere(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1428,7 +1506,8 @@ namespace armarx
layer.addedSphereVisualizations.erase(name);
}
- void DebugDrawerComponent::removeCylinder(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeCylinder(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1448,7 +1527,8 @@ namespace armarx
layer.addedCylinderVisualizations.erase(name);
}
- void DebugDrawerComponent::removeCircle(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeCircle(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1468,7 +1548,8 @@ namespace armarx
layer.addedCircleVisualizations.erase(name);
}
- void DebugDrawerComponent::removePointCloud(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removePointCloud(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1479,7 +1560,8 @@ namespace armarx
auto& layer = layers.at(layerName);
- if (layer.addedPointCloudVisualizations.find(name) == layer.addedPointCloudVisualizations.end())
+ if (layer.addedPointCloudVisualizations.find(name) ==
+ layer.addedPointCloudVisualizations.end())
{
return;
}
@@ -1488,7 +1570,8 @@ namespace armarx
layer.addedPointCloudVisualizations.erase(name);
}
- void DebugDrawerComponent::removePolygon(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removePolygon(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1508,7 +1591,8 @@ namespace armarx
layer.addedPolygonVisualizations.erase(name);
}
- void DebugDrawerComponent::removeTriMesh(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeTriMesh(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1528,7 +1612,8 @@ namespace armarx
layer.addedTriMeshVisualizations.erase(name);
}
- void DebugDrawerComponent::removeArrow(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeArrow(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1548,19 +1633,22 @@ namespace armarx
layer.addedArrowVisualizations.erase(name);
}
- void DebugDrawerComponent::removeRobot(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeRobot(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
// process active robots
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + name + "__"), " ", "_");
+ std::string entryName =
+ simox::alg::replace_all(std::string("__" + layerName + "__" + name + "__"), " ", "_");
ARMARX_DEBUG << "Removing robot " << entryName;
if (activeRobots.find(entryName) != activeRobots.end())
{
ARMARX_DEBUG << "Found robot to remove " << entryName;
activeRobots.erase(entryName);
- ARMARX_DEBUG << "after Found robot to remove, activeRobots.size() = " << activeRobots.size();
+ ARMARX_DEBUG << "after Found robot to remove, activeRobots.size() = "
+ << activeRobots.size();
}
// process visualizations
@@ -1580,7 +1668,9 @@ namespace armarx
ARMARX_DEBUG << "Removing visualization for " << entryName;
- ARMARX_DEBUG << "removing sep from layer.addedRobotVisualizations, d.name:" << name << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
+ ARMARX_DEBUG << "removing sep from layer.addedRobotVisualizations, d.name:" << name
+ << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():"
+ << layer.addedRobotVisualizations.size();
if (layer.addedRobotVisualizations.find(name) == layer.addedRobotVisualizations.end())
{
@@ -1588,7 +1678,8 @@ namespace armarx
return;
}
- ARMARX_DEBUG << "removing from layer.mainNode, layer.mainNode->getNumChildren()=" << layer.mainNode->getNumChildren();
+ ARMARX_DEBUG << "removing from layer.mainNode, layer.mainNode->getNumChildren()="
+ << layer.mainNode->getNumChildren();
if (layer.mainNode->findChild(layer.addedRobotVisualizations[name]) < 0)
{
@@ -1598,11 +1689,15 @@ namespace armarx
layer.mainNode->removeChild(layer.addedRobotVisualizations[name]);
layer.addedRobotVisualizations.erase(name);
- ARMARX_DEBUG << "after removing from layer.mainNode, layer.mainNode->getNumChildren()=" << layer.mainNode->getNumChildren();
- ARMARX_DEBUG << "after removing sep from layer.addedRobotVisualizations, d.name:" << name << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():" << layer.addedRobotVisualizations.size();
+ ARMARX_DEBUG << "after removing from layer.mainNode, layer.mainNode->getNumChildren()="
+ << layer.mainNode->getNumChildren();
+ ARMARX_DEBUG << "after removing sep from layer.addedRobotVisualizations, d.name:" << name
+ << ", layer:" << layerName << ", layer.addedRobotVisualizations.size():"
+ << layer.addedRobotVisualizations.size();
}
- void DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeCoordSystem(const std::string& layerName, const std::string& name)
{
auto l = getScopedVisuLock();
@@ -1622,7 +1717,8 @@ namespace armarx
layer.addedCoordVisualizations.erase(name);
}
- void DebugDrawerComponent::setLayerVisibility(const std::string& layerName, bool visible)
+ void
+ DebugDrawerComponent::setLayerVisibility(const std::string& layerName, bool visible)
{
auto l = getScopedVisuLock();
@@ -1650,7 +1746,8 @@ namespace armarx
}
}
- void DebugDrawerComponent::disableAllLayers(const ::Ice::Current&)
+ void
+ DebugDrawerComponent::disableAllLayers(const ::Ice::Current&)
{
auto l = getScopedVisuLock();
@@ -1660,7 +1757,8 @@ namespace armarx
}
}
- void DebugDrawerComponent::enableAllLayers(const ::Ice::Current&)
+ void
+ DebugDrawerComponent::enableAllLayers(const ::Ice::Current&)
{
auto l = getScopedVisuLock();
@@ -1670,13 +1768,19 @@ namespace armarx
}
}
- void DebugDrawerComponent::setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
+ void
+ DebugDrawerComponent::setScaledPoseVisu(const std::string& layerName,
+ const std::string& poseName,
+ const ::armarx::PoseBasePtr& globalPose,
+ const ::Ice::Float scale,
+ const ::Ice::Current&)
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(poseName);
Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
CoordData& d = accumulatedUpdateData.coord[entryName];
d.globalPose = gp;
d.layerName = layerName;
@@ -1686,26 +1790,41 @@ namespace armarx
}
}
- void DebugDrawerComponent::setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current&)
+ void
+ DebugDrawerComponent::setScaledPoseDebugLayerVisu(const std::string& poseName,
+ const ::armarx::PoseBasePtr& globalPose,
+ const ::Ice::Float scale,
+ const ::Ice::Current&)
{
setScaledPoseVisu(DEBUG_LAYER_NAME, poseName, globalPose, scale);
}
- void DebugDrawerComponent::setPoseVisu(const std::string& layerName, const std::string& poseName, const PoseBasePtr& globalPose, const Ice::Current&)
+ void
+ DebugDrawerComponent::setPoseVisu(const std::string& layerName,
+ const std::string& poseName,
+ const PoseBasePtr& globalPose,
+ const Ice::Current&)
{
setScaledPoseVisu(layerName, poseName, globalPose, 1.f);
}
- void DebugDrawerComponent::setPoseDebugLayerVisu(const std::string& poseName, const PoseBasePtr& globalPose, const Ice::Current&)
+ void
+ DebugDrawerComponent::setPoseDebugLayerVisu(const std::string& poseName,
+ const PoseBasePtr& globalPose,
+ const Ice::Current&)
{
setScaledPoseVisu(DEBUG_LAYER_NAME, poseName, globalPose, 1.f);
}
- void DebugDrawerComponent::removePoseVisu(const std::string& layerName, const std::string& poseName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removePoseVisu(const std::string& layerName,
+ const std::string& poseName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + poseName + "__"), " ", "_");
CoordData& d = accumulatedUpdateData.coord[entryName];
d.layerName = layerName;
d.name = poseName;
@@ -1713,19 +1832,28 @@ namespace armarx
}
}
- void DebugDrawerComponent::removePoseDebugLayerVisu(const std::string& poseName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removePoseDebugLayerVisu(const std::string& poseName, const Ice::Current&)
{
removePoseVisu(DEBUG_LAYER_NAME, poseName);
}
- void DebugDrawerComponent::setLineVisu(const std::string& layerName, const std::string& lineName, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, float lineWidth, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerComponent::setLineVisu(const std::string& layerName,
+ const std::string& lineName,
+ const Vector3BasePtr& globalPosition1,
+ const Vector3BasePtr& globalPosition2,
+ float lineWidth,
+ const DrawColor& color,
+ const Ice::Current&)
{
Eigen::Vector3f p1 = Vector3Ptr::dynamicCast(globalPosition1)->toEigen();
Eigen::Vector3f p2 = Vector3Ptr::dynamicCast(globalPosition2)->toEigen();
VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
LineData& d = accumulatedUpdateData.line[entryName];
d.p1 = p1;
d.p2 = p2;
@@ -1737,16 +1865,26 @@ namespace armarx
}
}
- void DebugDrawerComponent::setLineDebugLayerVisu(const std::string& lineName, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, float lineWidth, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerComponent::setLineDebugLayerVisu(const std::string& lineName,
+ const Vector3BasePtr& globalPosition1,
+ const Vector3BasePtr& globalPosition2,
+ float lineWidth,
+ const DrawColor& color,
+ const Ice::Current&)
{
setLineVisu(DEBUG_LAYER_NAME, lineName, globalPosition1, globalPosition2, lineWidth, color);
}
- void DebugDrawerComponent::removeLineVisu(const std::string& layerName, const std::string& lineName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeLineVisu(const std::string& layerName,
+ const std::string& lineName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + lineName + "__"), " ", "_");
LineData& d = accumulatedUpdateData.line[entryName];
d.layerName = layerName;
d.name = lineName;
@@ -1754,17 +1892,23 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeLineDebugLayerVisu(const std::string& lineName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeLineDebugLayerVisu(const std::string& lineName, const Ice::Current&)
{
removeLineVisu(DEBUG_LAYER_NAME, lineName);
}
- void DebugDrawerComponent::setLineSetVisu(const std::string& layerName, const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const Ice::Current&)
+ void
+ DebugDrawerComponent::setLineSetVisu(const std::string& layerName,
+ const std::string& lineSetName,
+ const DebugDrawerLineSet& lineSet,
+ const Ice::Current&)
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(lineSetName);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
LineSetData& d = accumulatedUpdateData.lineSet[entryName];
d.lineSet = lineSet;
d.layerName = layerName;
@@ -1773,16 +1917,23 @@ namespace armarx
}
}
- void DebugDrawerComponent::setLineSetDebugLayerVisu(const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const Ice::Current&)
+ void
+ DebugDrawerComponent::setLineSetDebugLayerVisu(const std::string& lineSetName,
+ const DebugDrawerLineSet& lineSet,
+ const Ice::Current&)
{
setLineSetVisu(DEBUG_LAYER_NAME, lineSetName, lineSet);
}
- void DebugDrawerComponent::removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeLineSetVisu(const std::string& layerName,
+ const std::string& lineSetName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + lineSetName + "__"), " ", "_");
LineSetData& d = accumulatedUpdateData.lineSet[entryName];
d.layerName = layerName;
d.name = lineSetName;
@@ -1790,18 +1941,27 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeLineSetDebugLayerVisu(const std::string& lineSetName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeLineSetDebugLayerVisu(const std::string& lineSetName,
+ const Ice::Current&)
{
removeLineSetVisu(DEBUG_LAYER_NAME, lineSetName);
}
- void DebugDrawerComponent::setBoxVisu(const std::string& layerName, const std::string& boxName, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerComponent::setBoxVisu(const std::string& layerName,
+ const std::string& boxName,
+ const PoseBasePtr& globalPose,
+ const Vector3BasePtr& dimensions,
+ const DrawColor& color,
+ const Ice::Current&)
{
Eigen::Matrix4f gp = PosePtr::dynamicCast(globalPose)->toEigen();
VirtualRobot::VisualizationFactory::Color c(color.r, color.g, color.b, 1 - color.a);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
BoxData& d = accumulatedUpdateData.box[entryName];
d.width = dimensions->x;
d.height = dimensions->y;
@@ -1814,16 +1974,25 @@ namespace armarx
}
}
- void DebugDrawerComponent::setBoxDebugLayerVisu(const std::string& boxName, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerComponent::setBoxDebugLayerVisu(const std::string& boxName,
+ const PoseBasePtr& globalPose,
+ const Vector3BasePtr& dimensions,
+ const DrawColor& color,
+ const Ice::Current&)
{
setBoxVisu(DEBUG_LAYER_NAME, boxName, globalPose, dimensions, color);
}
- void DebugDrawerComponent::removeBoxVisu(const std::string& layerName, const std::string& boxName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeBoxVisu(const std::string& layerName,
+ const std::string& boxName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + boxName + "__"), " ", "_");
BoxData& d = accumulatedUpdateData.box[entryName];
d.layerName = layerName;
d.name = boxName;
@@ -1831,38 +2000,58 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeBoxDebugLayerVisu(const std::string& boxName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeBoxDebugLayerVisu(const std::string& boxName, const Ice::Current&)
{
removeBoxVisu(DEBUG_LAYER_NAME, boxName);
}
- void DebugDrawerComponent::setTextVisu(const std::string& layerName, const std::string& textName, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, int size, const Ice::Current&)
+ void
+ DebugDrawerComponent::setTextVisu(const std::string& layerName,
+ const std::string& textName,
+ const std::string& text,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ int size,
+ const Ice::Current&)
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(textName);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + textName + "__"), " ", "_");
TextData& d = accumulatedUpdateData.text[entryName];
d.text = text;
d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
d.layerName = layerName;
d.name = textName;
- d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
+ d.color =
+ VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
d.size = size;
d.active = true;
}
}
- void DebugDrawerComponent::setTextDebugLayerVisu(const std::string& textName, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, int size, const Ice::Current&)
+ void
+ DebugDrawerComponent::setTextDebugLayerVisu(const std::string& textName,
+ const std::string& text,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ int size,
+ const Ice::Current&)
{
setTextVisu(DEBUG_LAYER_NAME, textName, text, globalPosition, color, size);
}
- void DebugDrawerComponent::removeTextVisu(const std::string& layerName, const std::string& textName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeTextVisu(const std::string& layerName,
+ const std::string& textName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + textName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + textName + "__"), " ", "_");
TextData& d = accumulatedUpdateData.text[entryName];
d.layerName = layerName;
d.name = textName;
@@ -1870,37 +2059,55 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeTextDebugLayerVisu(const std::string& textName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeTextDebugLayerVisu(const std::string& textName, const Ice::Current&)
{
removeTextVisu(DEBUG_LAYER_NAME, textName);
}
- void DebugDrawerComponent::setSphereVisu(const std::string& layerName, const std::string& sphereName, const Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const Ice::Current&)
+ void
+ DebugDrawerComponent::setSphereVisu(const std::string& layerName,
+ const std::string& sphereName,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ float radius,
+ const Ice::Current&)
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(sphereName);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
SphereData& d = accumulatedUpdateData.sphere[entryName];
d.radius = radius;
d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
d.layerName = layerName;
d.name = sphereName;
- d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
+ d.color =
+ VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
d.active = true;
}
}
- void DebugDrawerComponent::setSphereDebugLayerVisu(const std::string& sphereName, const Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const Ice::Current&)
+ void
+ DebugDrawerComponent::setSphereDebugLayerVisu(const std::string& sphereName,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ float radius,
+ const Ice::Current&)
{
setSphereVisu(DEBUG_LAYER_NAME, sphereName, globalPosition, color, radius);
}
- void DebugDrawerComponent::removeSphereVisu(const std::string& layerName, const std::string& sphereName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeSphereVisu(const std::string& layerName,
+ const std::string& sphereName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + sphereName + "__"), " ", "_");
SphereData& d = accumulatedUpdateData.sphere[entryName];
d.layerName = layerName;
d.name = sphereName;
@@ -1908,17 +2115,28 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeSphereDebugLayerVisu(const std::string& sphereName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeSphereDebugLayerVisu(const std::string& sphereName,
+ const Ice::Current&)
{
removeSphereVisu(DEBUG_LAYER_NAME, sphereName);
}
- void DebugDrawerComponent::setCylinderVisu(const std::string& layerName, const std::string& cylinderName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerComponent::setCylinderVisu(const std::string& layerName,
+ const std::string& cylinderName,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& direction,
+ float length,
+ float radius,
+ const DrawColor& color,
+ const Ice::Current&)
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(cylinderName);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
CylinderData& d = accumulatedUpdateData.cylinder[entryName];
d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
@@ -1926,21 +2144,34 @@ namespace armarx
d.radius = radius;
d.layerName = layerName;
d.name = cylinderName;
- d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
+ d.color =
+ VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
d.active = true;
}
}
- void DebugDrawerComponent::setCylinderDebugLayerVisu(const std::string& cylinderName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerComponent::setCylinderDebugLayerVisu(const std::string& cylinderName,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& direction,
+ float length,
+ float radius,
+ const DrawColor& color,
+ const Ice::Current&)
{
- setCylinderVisu(DEBUG_LAYER_NAME, cylinderName, globalPosition, direction, length, radius, color);
+ setCylinderVisu(
+ DEBUG_LAYER_NAME, cylinderName, globalPosition, direction, length, radius, color);
}
- void DebugDrawerComponent::removeCylinderVisu(const std::string& layerName, const std::string& cylinderName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeCylinderVisu(const std::string& layerName,
+ const std::string& cylinderName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + cylinderName + "__"), " ", "_");
CylinderData& d = accumulatedUpdateData.cylinder[entryName];
d.layerName = layerName;
d.name = cylinderName;
@@ -1948,17 +2179,24 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeCylinderDebugLayerVisu(const std::string& cylinderName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeCylinderDebugLayerVisu(const std::string& cylinderName,
+ const Ice::Current&)
{
removeCylinderVisu(DEBUG_LAYER_NAME, cylinderName);
}
- void DebugDrawerComponent::setPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const Ice::Current&)
+ void
+ DebugDrawerComponent::setPointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const DebugDrawerPointCloud& pointCloud,
+ const Ice::Current&)
{
ARMARX_DEBUG << VAROUT(layerName) << VAROUT(pointCloudName);
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
d.pointCloud = pointCloud;
d.layerName = layerName;
@@ -1967,16 +2205,23 @@ namespace armarx
}
}
- void DebugDrawerComponent::setPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const Ice::Current&)
+ void
+ DebugDrawerComponent::setPointCloudDebugLayerVisu(const std::string& pointCloudName,
+ const DebugDrawerPointCloud& pointCloud,
+ const Ice::Current&)
{
setPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
}
- void DebugDrawerComponent::removePointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removePointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
PointCloudData& d = accumulatedUpdateData.pointcloud[entryName];
d.layerName = layerName;
d.name = pointCloudName;
@@ -1984,29 +2229,44 @@ namespace armarx
}
}
- void DebugDrawerComponent::removePointCloudDebugLayerVisu(const std::string& pointCloudName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removePointCloudDebugLayerVisu(const std::string& pointCloudName,
+ const Ice::Current&)
{
removePointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
}
- void DebugDrawerComponent::setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector<Vector3BasePtr>& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const Ice::Current&)
+ void
+ DebugDrawerComponent::setPolygonVisu(const std::string& layerName,
+ const std::string& polygonName,
+ const std::vector<Vector3BasePtr>& polygonPoints,
+ const DrawColor& colorInner,
+ const DrawColor& colorBorder,
+ float lineWidth,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
PolygonData& d = accumulatedUpdateData.polygons[entryName];
- std::vector< Eigen::Vector3f > points;
+ std::vector<Eigen::Vector3f> points;
for (size_t i = 0; i < polygonPoints.size(); i++)
{
- Eigen::Vector3f p = Vector3Ptr::dynamicCast(polygonPoints.at(i))->toEigen();;
+ Eigen::Vector3f p = Vector3Ptr::dynamicCast(polygonPoints.at(i))->toEigen();
+ ;
points.push_back(p);
}
d.points = points;
- d.colorInner = VirtualRobot::VisualizationFactory::Color(colorInner.r, colorInner.g, colorInner.b, 1 - colorInner.a);;
- d.colorBorder = VirtualRobot::VisualizationFactory::Color(colorBorder.r, colorBorder.g, colorBorder.b, 1 - colorBorder.a);;
+ d.colorInner = VirtualRobot::VisualizationFactory::Color(
+ colorInner.r, colorInner.g, colorInner.b, 1 - colorInner.a);
+ ;
+ d.colorBorder = VirtualRobot::VisualizationFactory::Color(
+ colorBorder.r, colorBorder.g, colorBorder.b, 1 - colorBorder.a);
+ ;
d.lineWidth = lineWidth;
d.layerName = layerName;
@@ -2015,16 +2275,27 @@ namespace armarx
}
}
- void DebugDrawerComponent::setPolygonDebugLayerVisu(const std::string& polygonName, const std::vector<Vector3BasePtr>& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const Ice::Current&)
+ void
+ DebugDrawerComponent::setPolygonDebugLayerVisu(const std::string& polygonName,
+ const std::vector<Vector3BasePtr>& polygonPoints,
+ const DrawColor& colorInner,
+ const DrawColor& colorBorder,
+ float lineWidth,
+ const Ice::Current&)
{
- setPolygonVisu(DEBUG_LAYER_NAME, polygonName, polygonPoints, colorInner, colorBorder, lineWidth);
+ setPolygonVisu(
+ DEBUG_LAYER_NAME, polygonName, polygonPoints, colorInner, colorBorder, lineWidth);
}
- void DebugDrawerComponent::removePolygonVisu(const std::string& layerName, const std::string& polygonName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removePolygonVisu(const std::string& layerName,
+ const std::string& polygonName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + polygonName + "__"), " ", "_");
PolygonData& d = accumulatedUpdateData.polygons[entryName];
d.layerName = layerName;
d.name = polygonName;
@@ -2032,16 +2303,23 @@ namespace armarx
}
}
- void DebugDrawerComponent::removePolygonDebugLayerVisu(const std::string& polygonName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removePolygonDebugLayerVisu(const std::string& polygonName,
+ const Ice::Current&)
{
removePolygonVisu(DEBUG_LAYER_NAME, polygonName);
}
- void DebugDrawerComponent::setTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
+ void
+ DebugDrawerComponent::setTriMeshVisu(const std::string& layerName,
+ const std::string& triMeshName,
+ const DebugDrawerTriMesh& triMesh,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
d.triMesh = triMesh;
d.layerName = layerName;
@@ -2050,16 +2328,23 @@ namespace armarx
}
}
- void DebugDrawerComponent::setTriMeshDebugLayerVisu(const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
+ void
+ DebugDrawerComponent::setTriMeshDebugLayerVisu(const std::string& triMeshName,
+ const DebugDrawerTriMesh& triMesh,
+ const Ice::Current&)
{
setTriMeshVisu(DEBUG_LAYER_NAME, triMeshName, triMesh);
}
- void DebugDrawerComponent::removeTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeTriMeshVisu(const std::string& layerName,
+ const std::string& triMeshName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + triMeshName + "__"), " ", "_");
TriMeshData& d = accumulatedUpdateData.triMeshes[entryName];
d.layerName = layerName;
d.name = triMeshName;
@@ -2067,20 +2352,33 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeTriMeshDebugLayerVisu(const std::string& triMeshName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeTriMeshDebugLayerVisu(const std::string& triMeshName,
+ const Ice::Current&)
{
removeTriMeshVisu(DEBUG_LAYER_NAME, triMeshName);
}
- void DebugDrawerComponent::setArrowVisu(const std::string& layerName, const std::string& arrowName, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, float length, float width, const Ice::Current&)
+ void
+ DebugDrawerComponent::setArrowVisu(const std::string& layerName,
+ const std::string& arrowName,
+ const Vector3BasePtr& position,
+ const Vector3BasePtr& direction,
+ const DrawColor& color,
+ float length,
+ float width,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
ArrowData& d = accumulatedUpdateData.arrows[entryName];
d.position = Vector3Ptr::dynamicCast(position)->toEigen();
d.direction = Vector3Ptr::dynamicCast(direction)->toEigen();
- d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);;
+ d.color =
+ VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
+ ;
d.length = length;
d.width = width;
d.layerName = layerName;
@@ -2089,16 +2387,27 @@ namespace armarx
}
}
- void DebugDrawerComponent::setArrowDebugLayerVisu(const std::string& arrowName, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, float length, float width, const Ice::Current&)
+ void
+ DebugDrawerComponent::setArrowDebugLayerVisu(const std::string& arrowName,
+ const Vector3BasePtr& position,
+ const Vector3BasePtr& direction,
+ const DrawColor& color,
+ float length,
+ float width,
+ const Ice::Current&)
{
setArrowVisu(DEBUG_LAYER_NAME, arrowName, position, direction, color, length, width);
}
- void DebugDrawerComponent::removeArrowVisu(const std::string& layerName, const std::string& arrowName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeArrowVisu(const std::string& layerName,
+ const std::string& arrowName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + arrowName + "__"), " ", "_");
ArrowData& d = accumulatedUpdateData.arrows[entryName];
d.layerName = layerName;
d.name = arrowName;
@@ -2106,15 +2415,24 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeArrowDebugLayerVisu(const std::string& arrowName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeArrowDebugLayerVisu(const std::string& arrowName,
+ const Ice::Current&)
{
removeArrowVisu(DEBUG_LAYER_NAME, arrowName);
}
- void DebugDrawerComponent::setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const Ice::Current&)
+ void
+ DebugDrawerComponent::setRobotVisu(const std::string& layerName,
+ const std::string& robotName,
+ const std::string& robotFile,
+ const std::string& armarxProject,
+ DrawStyle drawStyle,
+ const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "setting robot visualization for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2127,10 +2445,15 @@ namespace armarx
d.active = true;
}
- void DebugDrawerComponent::updateRobotPose(const std::string& layerName, const std::string& robotName, const PoseBasePtr& globalPose, const Ice::Current&)
+ void
+ DebugDrawerComponent::updateRobotPose(const std::string& layerName,
+ const std::string& robotName,
+ const PoseBasePtr& globalPose,
+ const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot pose for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2144,10 +2467,15 @@ namespace armarx
d.active = true;
}
- void DebugDrawerComponent::updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map<std::string, float>& configuration, const Ice::Current&)
+ void
+ DebugDrawerComponent::updateRobotConfig(const std::string& layerName,
+ const std::string& robotName,
+ const std::map<std::string, float>& configuration,
+ const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot config for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2165,10 +2493,15 @@ namespace armarx
d.active = true;
}
- void DebugDrawerComponent::updateRobotColor(const std::string& layerName, const std::string& robotName, const DrawColor& c, const Ice::Current&)
+ void
+ DebugDrawerComponent::updateRobotColor(const std::string& layerName,
+ const std::string& robotName,
+ const DrawColor& c,
+ const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot color for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2190,10 +2523,16 @@ namespace armarx
d.active = true;
}
- void DebugDrawerComponent::updateRobotNodeColor(const std::string& layerName, const std::string& robotName, const std::string& robotNodeName, const DrawColor& c, const Ice::Current&)
+ void
+ DebugDrawerComponent::updateRobotNodeColor(const std::string& layerName,
+ const std::string& robotName,
+ const std::string& robotNodeName,
+ const DrawColor& c,
+ const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "updating robot color for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2208,18 +2547,21 @@ namespace armarx
}
else
{
- d.nodeColors[robotNodeName] = VirtualRobot::VisualizationFactory::Color(c.r, c.g, c.b, 1 - c.a);
+ d.nodeColors[robotNodeName] =
+ VirtualRobot::VisualizationFactory::Color(c.r, c.g, c.b, 1 - c.a);
}
d.active = true;
-
-
}
- void DebugDrawerComponent::removeRobotVisu(const std::string& layerName, const std::string& robotName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeRobotVisu(const std::string& layerName,
+ const std::string& robotName,
+ const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + robotName + "__"), " ", "_");
ARMARX_DEBUG << "removing robot visu for " << entryName;
RobotData& d = accumulatedUpdateData.robots[entryName];
@@ -2228,7 +2570,8 @@ namespace armarx
d.active = false;
}
- void DebugDrawerComponent::clearAll(const Ice::Current&)
+ void
+ DebugDrawerComponent::clearAll(const Ice::Current&)
{
for (auto& i : layers)
{
@@ -2236,7 +2579,8 @@ namespace armarx
}
}
- void DebugDrawerComponent::clearLayer(const std::string& layerName, const Ice::Current&)
+ void
+ DebugDrawerComponent::clearLayer(const std::string& layerName, const Ice::Current&)
{
auto lockData = getScopedAccumulatedDataLock();
//updates should only contain elements to add for each layer after the last clear for this layer was executed
@@ -2244,10 +2588,10 @@ namespace armarx
//=>remove all objects pending for this layer
removeAccumulatedData(layerName);
accumulatedUpdateData.clearLayers.emplace(layerName);
-
}
- void DebugDrawerComponent::clearLayerQt(const std::string& layerName)
+ void
+ DebugDrawerComponent::clearLayerQt(const std::string& layerName)
{
auto lockData = getScopedAccumulatedDataLock();
auto lockVisu = getScopedVisuLock();
@@ -2258,7 +2602,8 @@ namespace armarx
{
if (verbose)
{
- ARMARX_VERBOSE << "Layer " << layerName << " can't be cleared, because it does not exist.";
+ ARMARX_VERBOSE << "Layer " << layerName
+ << " can't be cleared, because it does not exist.";
}
return;
@@ -2354,17 +2699,20 @@ namespace armarx
}
}
- void DebugDrawerComponent::clearDebugLayer(const Ice::Current&)
+ void
+ DebugDrawerComponent::clearDebugLayer(const Ice::Current&)
{
clearLayer(DEBUG_LAYER_NAME);
}
- void DebugDrawerComponent::setMutex(RecursiveMutexPtr const& m)
+ void
+ DebugDrawerComponent::setMutex(RecursiveMutexPtr const& m)
{
mutex = m;
}
- auto DebugDrawerComponent::getScopedVisuLock() -> RecursiveMutexLockPtr
+ auto
+ DebugDrawerComponent::getScopedVisuLock() -> RecursiveMutexLockPtr
{
RecursiveMutexLockPtr l;
@@ -2381,18 +2729,21 @@ namespace armarx
return l;
}
- auto DebugDrawerComponent::getScopedAccumulatedDataLock() -> RecursiveMutexLockPtr
+ auto
+ DebugDrawerComponent::getScopedAccumulatedDataLock() -> RecursiveMutexLockPtr
{
RecursiveMutexLockPtr l(new RecursiveMutexLock(*dataUpdateMutex));
return l;
}
- SoSeparator* DebugDrawerComponent::getVisualization()
+ SoSeparator*
+ DebugDrawerComponent::getVisualization()
{
return coinVisu;
}
- DebugDrawerComponent::Layer& DebugDrawerComponent::requestLayer(const std::string& layerName)
+ DebugDrawerComponent::Layer&
+ DebugDrawerComponent::requestLayer(const std::string& layerName)
{
auto l = getScopedVisuLock();
@@ -2403,7 +2754,7 @@ namespace armarx
ARMARX_VERBOSE << "Created layer " << layerName;
- SoSeparator* mainNode = new SoSeparator {};
+ SoSeparator* mainNode = new SoSeparator{};
mainNode->ref();
layers[layerName] = Layer();
layers.at(layerName).mainNode = mainNode;
@@ -2415,7 +2766,8 @@ namespace armarx
return layers.at(layerName);
}
- void DebugDrawerComponent::updateVisualization()
+ void
+ DebugDrawerComponent::updateVisualization()
{
auto lockData = getScopedAccumulatedDataLock();
auto lockVisu = getScopedVisuLock();
@@ -2532,13 +2884,15 @@ namespace armarx
onUpdateVisualization();
}
- bool DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
+ bool
+ DebugDrawerComponent::hasLayer(const std::string& layerName, const ::Ice::Current&)
{
auto l = getScopedVisuLock();
return layers.find(layerName) != layers.end();
}
- void DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice::Current&)
+ void
+ DebugDrawerComponent::removeLayer(const std::string& layerName, const ::Ice::Current&)
{
auto lockData = getScopedAccumulatedDataLock();
//updates should only contain elements to add for each layer after the last clear/remove for this layer was executed
@@ -2548,7 +2902,8 @@ namespace armarx
accumulatedUpdateData.removeLayers.emplace(layerName);
}
- void DebugDrawerComponent::removeLayerQt(const std::string& layerName)
+ void
+ DebugDrawerComponent::removeLayerQt(const std::string& layerName)
{
auto lockData = getScopedAccumulatedDataLock();
auto lockVisu = getScopedVisuLock();
@@ -2556,7 +2911,8 @@ namespace armarx
{
if (verbose)
{
- ARMARX_VERBOSE << "Layer " << layerName << " can't be removed, because it does not exist.";
+ ARMARX_VERBOSE << "Layer " << layerName
+ << " can't be removed, because it does not exist.";
}
return;
@@ -2576,8 +2932,10 @@ namespace armarx
}
}
- template<class UpdateDataType>
- void removeUpdateDataFromMap(const std::string& layerName, std::map<std::string, UpdateDataType>& map)
+ template <class UpdateDataType>
+ void
+ removeUpdateDataFromMap(const std::string& layerName,
+ std::map<std::string, UpdateDataType>& map)
{
auto it = map.begin();
while (it != map.end())
@@ -2590,7 +2948,8 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
+ void
+ DebugDrawerComponent::removeAccumulatedData(const std::string& layerName)
{
auto lockData = getScopedAccumulatedDataLock();
@@ -2610,23 +2969,27 @@ namespace armarx
removeUpdateDataFromMap(layerName, accumulatedUpdateData.triMeshes);
onRemoveAccumulatedData(layerName);
-
}
- void DebugDrawerComponent::enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current&)
+ void
+ DebugDrawerComponent::enableLayerVisu(const std::string& layerName,
+ bool visible,
+ const ::Ice::Current&)
{
setLayerVisibility(layerName, visible);
}
- void DebugDrawerComponent::enableDebugLayerVisu(bool visible, const ::Ice::Current&)
+ void
+ DebugDrawerComponent::enableDebugLayerVisu(bool visible, const ::Ice::Current&)
{
enableLayerVisu(DEBUG_LAYER_NAME, visible);
}
- Ice::StringSeq DebugDrawerComponent::layerNames(const ::Ice::Current&)
+ Ice::StringSeq
+ DebugDrawerComponent::layerNames(const ::Ice::Current&)
{
auto l = getScopedVisuLock();
- Ice::StringSeq seq {};
+ Ice::StringSeq seq{};
for (const auto& layer : layers)
{
@@ -2636,9 +2999,10 @@ namespace armarx
return seq;
}
- ::armarx::LayerInformationSequence DebugDrawerComponent::layerInformation(const ::Ice::Current&)
+ ::armarx::LayerInformationSequence
+ DebugDrawerComponent::layerInformation(const ::Ice::Current&)
{
- ::armarx::LayerInformationSequence seq {};
+ ::armarx::LayerInformationSequence seq{};
auto l = getScopedVisuLock();
for (const auto& layer : layers)
@@ -2666,7 +3030,8 @@ namespace armarx
return seq;
}
- void DebugDrawerComponent::drawColoredPointCloud(const ColoredPointCloudData& d)
+ void
+ DebugDrawerComponent::drawColoredPointCloud(const ColoredPointCloudData& d)
{
auto l = getScopedVisuLock();
@@ -2686,13 +3051,12 @@ namespace armarx
SoMaterial* pclMat = new SoMaterial;
std::vector<SbColor> colors;
colors.reserve(pcl.size());
- std::transform(
- pcl.begin(), pcl.end(), std::back_inserter(colors),
- [](const DebugDrawerColoredPointCloudElement & elem)
- {
- return SbColor {elem.color.r, elem.color.g, elem.color.b};
- }
- );
+ std::transform(pcl.begin(),
+ pcl.end(),
+ std::back_inserter(colors),
+ [](const DebugDrawerColoredPointCloudElement& elem) {
+ return SbColor{elem.color.r, elem.color.g, elem.color.b};
+ });
pclMat->diffuseColor.setValues(0, colors.size(), colors.data());
pclMat->ambientColor.setValues(0, colors.size(), colors.data());
pclSep->addChild(pclMat);
@@ -2704,13 +3068,12 @@ namespace armarx
SoCoordinate3* pclCoords = new SoCoordinate3;
std::vector<SbVec3f> coords;
coords.reserve(pcl.size());
- std::transform(
- pcl.begin(), pcl.end(), std::back_inserter(coords),
- [](const DebugDrawerColoredPointCloudElement & elem)
- {
- return SbVec3f {elem.x, elem.y, elem.z};
- }
- );
+ std::transform(pcl.begin(),
+ pcl.end(),
+ std::back_inserter(coords),
+ [](const DebugDrawerColoredPointCloudElement& elem) {
+ return SbVec3f{elem.x, elem.y, elem.z};
+ });
pclCoords->point.setValues(0, coords.size(), coords.data());
pclSep->addChild(pclCoords);
@@ -2726,7 +3089,9 @@ namespace armarx
layer.mainNode->addChild(pclSep);
}
- void DebugDrawerComponent::removeColoredPointCloud(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::removeColoredPointCloud(const std::string& layerName,
+ const std::string& name)
{
auto l = getScopedVisuLock();
@@ -2737,7 +3102,8 @@ namespace armarx
auto& layer = layers.at(layerName);
- if (layer.addedColoredPointCloudVisualizations.find(name) == layer.addedColoredPointCloudVisualizations.end())
+ if (layer.addedColoredPointCloudVisualizations.find(name) ==
+ layer.addedColoredPointCloudVisualizations.end())
{
return;
}
@@ -2746,11 +3112,16 @@ namespace armarx
layer.addedColoredPointCloudVisualizations.erase(name);
}
- void DebugDrawerComponent::setColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current&)
+ void
+ DebugDrawerComponent::setColoredPointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const DebugDrawerColoredPointCloud& pointCloud,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
d.pointCloud = pointCloud;
d.layerName = layerName;
@@ -2759,16 +3130,24 @@ namespace armarx
}
}
- void DebugDrawerComponent::setColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current&)
+ void
+ DebugDrawerComponent::setColoredPointCloudDebugLayerVisu(
+ const std::string& pointCloudName,
+ const DebugDrawerColoredPointCloud& pointCloud,
+ const Ice::Current&)
{
setColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
}
- void DebugDrawerComponent::removeColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeColoredPointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
ColoredPointCloudData& d = accumulatedUpdateData.coloredpointcloud[entryName];
d.layerName = layerName;
d.name = pointCloudName;
@@ -2776,12 +3155,15 @@ namespace armarx
}
}
- void DebugDrawerComponent::removeColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeColoredPointCloudDebugLayerVisu(const std::string& pointCloudName,
+ const Ice::Current&)
{
removeColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
}
- void DebugDrawerComponent::draw24BitColoredPointCloud(const Colored24BitPointCloudData& d)
+ void
+ DebugDrawerComponent::draw24BitColoredPointCloud(const Colored24BitPointCloudData& d)
{
auto l = getScopedVisuLock();
@@ -2801,13 +3183,15 @@ namespace armarx
SoMaterial* pclMat = new SoMaterial;
std::vector<SbColor> colors;
colors.reserve(pcl.size());
- std::transform(
- pcl.begin(), pcl.end(), std::back_inserter(colors),
- [](const DebugDrawer24BitColoredPointCloudElement & elem)
- {
- return SbColor {static_cast<float>(elem.color.r) / 255.f, static_cast<float>(elem.color.g) / 255.f, static_cast<float>(elem.color.b) / 255.f};
- }
- );
+ std::transform(pcl.begin(),
+ pcl.end(),
+ std::back_inserter(colors),
+ [](const DebugDrawer24BitColoredPointCloudElement& elem)
+ {
+ return SbColor{static_cast<float>(elem.color.r) / 255.f,
+ static_cast<float>(elem.color.g) / 255.f,
+ static_cast<float>(elem.color.b) / 255.f};
+ });
pclMat->diffuseColor.setValues(0, colors.size(), colors.data());
pclMat->ambientColor.setValues(0, colors.size(), colors.data());
pclMat->transparency = d.pointCloud.transparency;
@@ -2820,13 +3204,12 @@ namespace armarx
SoCoordinate3* pclCoords = new SoCoordinate3;
std::vector<SbVec3f> coords;
coords.reserve(pcl.size());
- std::transform(
- pcl.begin(), pcl.end(), std::back_inserter(coords),
- [](const DebugDrawer24BitColoredPointCloudElement & elem)
- {
- return SbVec3f {elem.x, elem.y, elem.z};
- }
- );
+ std::transform(pcl.begin(),
+ pcl.end(),
+ std::back_inserter(coords),
+ [](const DebugDrawer24BitColoredPointCloudElement& elem) {
+ return SbVec3f{elem.x, elem.y, elem.z};
+ });
pclCoords->point.setValues(0, coords.size(), coords.data());
pclSep->addChild(pclCoords);
@@ -2842,7 +3225,9 @@ namespace armarx
layer.mainNode->addChild(pclSep);
}
- void DebugDrawerComponent::remove24BitColoredPointCloud(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerComponent::remove24BitColoredPointCloud(const std::string& layerName,
+ const std::string& name)
{
auto l = getScopedVisuLock();
@@ -2853,7 +3238,8 @@ namespace armarx
auto& layer = layers.at(layerName);
- if (layer.added24BitColoredPointCloudVisualizations.find(name) == layer.added24BitColoredPointCloudVisualizations.end())
+ if (layer.added24BitColoredPointCloudVisualizations.find(name) ==
+ layer.added24BitColoredPointCloudVisualizations.end())
{
return;
}
@@ -2862,11 +3248,17 @@ namespace armarx
layer.added24BitColoredPointCloudVisualizations.erase(name);
}
- void DebugDrawerComponent::set24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
+ void
+ DebugDrawerComponent::set24BitColoredPointCloudVisu(
+ const std::string& layerName,
+ const std::string& pointCloudName,
+ const DebugDrawer24BitColoredPointCloud& pointCloud,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
d.pointCloud = pointCloud;
d.layerName = layerName;
@@ -2875,16 +3267,24 @@ namespace armarx
}
}
- void DebugDrawerComponent::set24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
+ void
+ DebugDrawerComponent::set24BitColoredPointCloudDebugLayerVisu(
+ const std::string& pointCloudName,
+ const DebugDrawer24BitColoredPointCloud& pointCloud,
+ const Ice::Current&)
{
set24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName, pointCloud);
}
- void DebugDrawerComponent::remove24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const Ice::Current&)
+ void
+ DebugDrawerComponent::remove24BitColoredPointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const Ice::Current&)
{
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + pointCloudName + "__"), " ", "_");
Colored24BitPointCloudData& d = accumulatedUpdateData.colored24Bitpointcloud[entryName];
d.layerName = layerName;
d.name = pointCloudName;
@@ -2892,16 +3292,29 @@ namespace armarx
}
}
- void DebugDrawerComponent::remove24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const Ice::Current&)
+ void
+ DebugDrawerComponent::remove24BitColoredPointCloudDebugLayerVisu(
+ const std::string& pointCloudName,
+ const Ice::Current&)
{
remove24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, pointCloudName);
}
- void DebugDrawerComponent::setCircleArrowVisu(const std::string& layerName, const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerComponent::setCircleArrowVisu(const std::string& layerName,
+ const std::string& circleName,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& directionVec,
+ Ice::Float radius,
+ Ice::Float circleCompletion,
+ Ice::Float width,
+ const DrawColor& color,
+ const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
CircleData& d = accumulatedUpdateData.circle[entryName];
d.position = Vector3Ptr::dynamicCast(globalPosition)->toEigen();
d.direction = Vector3Ptr::dynamicCast(directionVec)->toEigen();
@@ -2912,28 +3325,48 @@ namespace armarx
d.name = circleName;
d.color = VirtualRobot::VisualizationFactory::Color(color.r, color.g, color.b, 1 - color.a);
d.active = true;
-
}
- void DebugDrawerComponent::setCircleDebugLayerVisu(const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& c)
+ void
+ DebugDrawerComponent::setCircleDebugLayerVisu(const std::string& circleName,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& directionVec,
+ Ice::Float radius,
+ Ice::Float circleCompletion,
+ Ice::Float width,
+ const DrawColor& color,
+ const Ice::Current& c)
{
- setCircleArrowVisu(DEBUG_LAYER_NAME, circleName, globalPosition, directionVec, radius, circleCompletion, width, color, c);
+ setCircleArrowVisu(DEBUG_LAYER_NAME,
+ circleName,
+ globalPosition,
+ directionVec,
+ radius,
+ circleCompletion,
+ width,
+ color,
+ c);
}
- void DebugDrawerComponent::removeCircleVisu(const std::string& layerName, const std::string& circleName, const Ice::Current&)
+ void
+ DebugDrawerComponent::removeCircleVisu(const std::string& layerName,
+ const std::string& circleName,
+ const Ice::Current&)
{
auto l = getScopedAccumulatedDataLock();
- std::string entryName = simox::alg::replace_all(std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
+ std::string entryName = simox::alg::replace_all(
+ std::string("__" + layerName + "__" + circleName + "__"), " ", "_");
CircleData& d = accumulatedUpdateData.circle[entryName];
d.layerName = layerName;
d.name = circleName;
d.active = false;
-
}
- void DebugDrawerComponent::removeCircleDebugLayerVisu(const std::string& circleName, const Ice::Current& c)
+ void
+ DebugDrawerComponent::removeCircleDebugLayerVisu(const std::string& circleName,
+ const Ice::Current& c)
{
removeCircleVisu(DEBUG_LAYER_NAME, circleName, c);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
index 7b2e5af08f2b9399cb5a2b482af3870db0689820..12cfd06a68a849c7327baf996b0184fdacf1c64e 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.h
@@ -28,22 +28,22 @@
// Coin3D & SoQt
#include <Inventor/nodes/SoNode.h>
-#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoSelection.h>
+#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/sensors/SoTimerSensor.h>
// ArmarX
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
-
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/Visualization/VisualizationFactory.h>
-
//std
#include <memory>
#include <mutex>
@@ -55,16 +55,21 @@ namespace armarx
* \class DebugDrawerPropertyDefinitions
* \brief
*/
- class DebugDrawerPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class DebugDrawerPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- DebugDrawerPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ DebugDrawerPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<bool>("ShowDebugDrawing", true, "The simulator implements the DebugDrawerInterface. The debug visualizations (e.g. coordinate systems) can enabled/disbaled with this flag.");
- defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
- defineOptionalProperty<std::string>("DebugDrawerSelectionTopic", "DebugDrawerSelections", "Name of the DebugDrawerSelectionTopic");
+ defineOptionalProperty<bool>(
+ "ShowDebugDrawing",
+ true,
+ "The simulator implements the DebugDrawerInterface. The debug visualizations (e.g. "
+ "coordinate systems) can enabled/disbaled with this flag.");
+ defineOptionalProperty<std::string>(
+ "DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ defineOptionalProperty<std::string>("DebugDrawerSelectionTopic",
+ "DebugDrawerSelections",
+ "Name of the DebugDrawerSelectionTopic");
}
};
@@ -110,7 +115,6 @@ namespace armarx
virtual public Component
{
public:
-
/*!
* \brief DebugDrawerComponent Constructs a debug drawer
*/
@@ -123,102 +127,281 @@ namespace armarx
void setVisuUpdateTime(float visuUpdatesPerSec);
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "DebugDrawer";
}
+
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
void onExitComponent() override;
-
-
/*!
* \see PropertyUser::createPropertyDefinitions()
*/
- PropertyDefinitionsPtr createPropertyDefinitions() override
+ PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return PropertyDefinitionsPtr(new DebugDrawerPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new DebugDrawerPropertyDefinitions(getConfigIdentifier()));
}
/* Inherited from DebugDrawerInterface. */
- void exportScene(const std::string& filename, const ::Ice::Current& = Ice::emptyCurrent) override;
- void exportLayer(const std::string& filename, const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setScaledPoseVisu(const std::string& layerName, const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setScaledPoseDebugLayerVisu(const std::string& poseName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Float scale, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removePoseVisu(const std::string& layerName, const std::string& poseName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removePoseDebugLayerVisu(const std::string& poseName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setLineVisu(const std::string& layerName, const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setLineDebugLayerVisu(const std::string& lineName, const ::armarx::Vector3BasePtr& globalPosition1, const ::armarx::Vector3BasePtr& globalPosition2, float lineWidth, const ::armarx::DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeLineVisu(const std::string& layerName, const std::string& lineName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeLineDebugLayerVisu(const std::string& lineName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setLineSetVisu(const std::string& layerName, const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setLineSetDebugLayerVisu(const std::string& lineSetName, const DebugDrawerLineSet& lineSet, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeLineSetVisu(const std::string& layerName, const std::string& lineSetName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeLineSetDebugLayerVisu(const std::string& lineSetName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setBoxVisu(const std::string& layerName, const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setBoxDebugLayerVisu(const std::string& boxName, const ::armarx::PoseBasePtr& globalPose, const ::armarx::Vector3BasePtr& dimensions, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeBoxVisu(const std::string& layerName, const std::string& boxName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeBoxDebugLayerVisu(const std::string& boxName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setTextVisu(const std::string& layerName, const std::string& textName, const std::string& text, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, int size, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setTextDebugLayerVisu(const std::string& textName, const std::string& text, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, int size, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeTextVisu(const std::string& layerName, const std::string& textName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeTextDebugLayerVisu(const std::string& textName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setSphereVisu(const std::string& layerName, const std::string& sphereName, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setSphereDebugLayerVisu(const std::string& sphereName, const ::armarx::Vector3BasePtr& globalPosition, const DrawColor& color, float radius, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeSphereVisu(const std::string& layerName, const std::string& sphereName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeSphereDebugLayerVisu(const std::string& sphereName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setCylinderVisu(const std::string& layerName, const std::string& cylinderName, const ::armarx::Vector3BasePtr& globalPosition, const ::armarx::Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setCylinderDebugLayerVisu(const std::string& cylinderName, const ::armarx::Vector3BasePtr& globalPosition, const ::armarx::Vector3BasePtr& direction, float length, float radius, const DrawColor& color, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeCylinderVisu(const std::string& layerName, const std::string& cylinderName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeCylinderDebugLayerVisu(const std::string& cylinderName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removePointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removePointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- virtual void setColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent);
- void setColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawerColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void set24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
- void set24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const DebugDrawer24BitColoredPointCloud& pointCloud, const ::Ice::Current& = Ice::emptyCurrent) override;
- void remove24BitColoredPointCloudVisu(const std::string& layerName, const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void remove24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
-
- void setPolygonVisu(const std::string& layerName, const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setPolygonDebugLayerVisu(const std::string& polygonName, const std::vector< ::armarx::Vector3BasePtr >& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, float lineWidth, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removePolygonVisu(const std::string& layerName, const std::string& polygonName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removePolygonDebugLayerVisu(const std::string& polygonName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setTriMeshDebugLayerVisu(const std::string& triMeshName, const DebugDrawerTriMesh& triMesh, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeTriMeshVisu(const std::string& layerName, const std::string& triMeshName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeTriMeshDebugLayerVisu(const std::string& triMeshName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setArrowVisu(const std::string& layerName, const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = Ice::emptyCurrent) override;
- void setArrowDebugLayerVisu(const std::string& arrowName, const ::armarx::Vector3BasePtr& position, const ::armarx::Vector3BasePtr& direction, const DrawColor& color, float length, float width, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeArrowVisu(const std::string& layerName, const std::string& arrowName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeArrowDebugLayerVisu(const std::string& arrowName, const ::Ice::Current& = Ice::emptyCurrent) override;
-
- void setCircleArrowVisu(const std::string& layerName, const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
- void setCircleDebugLayerVisu(const std::string& circleName, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
- void removeCircleVisu(const std::string& layerName, const std::string& circleName, const Ice::Current& = Ice::emptyCurrent) override;
- void removeCircleDebugLayerVisu(const std::string& circleName, const Ice::Current& = Ice::emptyCurrent) override;
+ void exportScene(const std::string& filename,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void exportLayer(const std::string& filename,
+ const std::string& layerName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setPoseVisu(const std::string& layerName,
+ const std::string& poseName,
+ const ::armarx::PoseBasePtr& globalPose,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setScaledPoseVisu(const std::string& layerName,
+ const std::string& poseName,
+ const ::armarx::PoseBasePtr& globalPose,
+ const ::Ice::Float scale,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setPoseDebugLayerVisu(const std::string& poseName,
+ const ::armarx::PoseBasePtr& globalPose,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setScaledPoseDebugLayerVisu(const std::string& poseName,
+ const ::armarx::PoseBasePtr& globalPose,
+ const ::Ice::Float scale,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePoseVisu(const std::string& layerName,
+ const std::string& poseName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePoseDebugLayerVisu(const std::string& poseName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setLineVisu(const std::string& layerName,
+ const std::string& lineName,
+ const ::armarx::Vector3BasePtr& globalPosition1,
+ const ::armarx::Vector3BasePtr& globalPosition2,
+ float lineWidth,
+ const ::armarx::DrawColor& color,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setLineDebugLayerVisu(const std::string& lineName,
+ const ::armarx::Vector3BasePtr& globalPosition1,
+ const ::armarx::Vector3BasePtr& globalPosition2,
+ float lineWidth,
+ const ::armarx::DrawColor& color,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineVisu(const std::string& layerName,
+ const std::string& lineName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineDebugLayerVisu(const std::string& lineName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setLineSetVisu(const std::string& layerName,
+ const std::string& lineSetName,
+ const DebugDrawerLineSet& lineSet,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setLineSetDebugLayerVisu(const std::string& lineSetName,
+ const DebugDrawerLineSet& lineSet,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineSetVisu(const std::string& layerName,
+ const std::string& lineSetName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineSetDebugLayerVisu(const std::string& lineSetName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setBoxVisu(const std::string& layerName,
+ const std::string& boxName,
+ const ::armarx::PoseBasePtr& globalPose,
+ const ::armarx::Vector3BasePtr& dimensions,
+ const DrawColor& color,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setBoxDebugLayerVisu(const std::string& boxName,
+ const ::armarx::PoseBasePtr& globalPose,
+ const ::armarx::Vector3BasePtr& dimensions,
+ const DrawColor& color,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeBoxVisu(const std::string& layerName,
+ const std::string& boxName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeBoxDebugLayerVisu(const std::string& boxName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setTextVisu(const std::string& layerName,
+ const std::string& textName,
+ const std::string& text,
+ const ::armarx::Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ int size,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setTextDebugLayerVisu(const std::string& textName,
+ const std::string& text,
+ const ::armarx::Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ int size,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeTextVisu(const std::string& layerName,
+ const std::string& textName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeTextDebugLayerVisu(const std::string& textName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setSphereVisu(const std::string& layerName,
+ const std::string& sphereName,
+ const ::armarx::Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ float radius,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setSphereDebugLayerVisu(const std::string& sphereName,
+ const ::armarx::Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ float radius,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeSphereVisu(const std::string& layerName,
+ const std::string& sphereName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeSphereDebugLayerVisu(const std::string& sphereName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setCylinderVisu(const std::string& layerName,
+ const std::string& cylinderName,
+ const ::armarx::Vector3BasePtr& globalPosition,
+ const ::armarx::Vector3BasePtr& direction,
+ float length,
+ float radius,
+ const DrawColor& color,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setCylinderDebugLayerVisu(const std::string& cylinderName,
+ const ::armarx::Vector3BasePtr& globalPosition,
+ const ::armarx::Vector3BasePtr& direction,
+ float length,
+ float radius,
+ const DrawColor& color,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeCylinderVisu(const std::string& layerName,
+ const std::string& cylinderName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeCylinderDebugLayerVisu(const std::string& cylinderName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setPointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const DebugDrawerPointCloud& pointCloud,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setPointCloudDebugLayerVisu(const std::string& pointCloudName,
+ const DebugDrawerPointCloud& pointCloud,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePointCloudDebugLayerVisu(const std::string& pointCloudName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ virtual void
+ setColoredPointCloudDebugLayerVisu(const std::string& pointCloudName,
+ const DebugDrawerColoredPointCloud& pointCloud,
+ const ::Ice::Current& = Ice::emptyCurrent);
+ void setColoredPointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const DebugDrawerColoredPointCloud& pointCloud,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeColoredPointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void
+ removeColoredPointCloudDebugLayerVisu(const std::string& pointCloudName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void
+ set24BitColoredPointCloudDebugLayerVisu(const std::string& pointCloudName,
+ const DebugDrawer24BitColoredPointCloud& pointCloud,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void set24BitColoredPointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const DebugDrawer24BitColoredPointCloud& pointCloud,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void remove24BitColoredPointCloudVisu(const std::string& layerName,
+ const std::string& pointCloudName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void remove24BitColoredPointCloudDebugLayerVisu(
+ const std::string& pointCloudName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+
+ void setPolygonVisu(const std::string& layerName,
+ const std::string& polygonName,
+ const std::vector<::armarx::Vector3BasePtr>& polygonPoints,
+ const DrawColor& colorInner,
+ const DrawColor& colorBorder,
+ float lineWidth,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setPolygonDebugLayerVisu(const std::string& polygonName,
+ const std::vector<::armarx::Vector3BasePtr>& polygonPoints,
+ const DrawColor& colorInner,
+ const DrawColor& colorBorder,
+ float lineWidth,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePolygonVisu(const std::string& layerName,
+ const std::string& polygonName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removePolygonDebugLayerVisu(const std::string& polygonName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setTriMeshVisu(const std::string& layerName,
+ const std::string& triMeshName,
+ const DebugDrawerTriMesh& triMesh,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setTriMeshDebugLayerVisu(const std::string& triMeshName,
+ const DebugDrawerTriMesh& triMesh,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeTriMeshVisu(const std::string& layerName,
+ const std::string& triMeshName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeTriMeshDebugLayerVisu(const std::string& triMeshName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setArrowVisu(const std::string& layerName,
+ const std::string& arrowName,
+ const ::armarx::Vector3BasePtr& position,
+ const ::armarx::Vector3BasePtr& direction,
+ const DrawColor& color,
+ float length,
+ float width,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setArrowDebugLayerVisu(const std::string& arrowName,
+ const ::armarx::Vector3BasePtr& position,
+ const ::armarx::Vector3BasePtr& direction,
+ const DrawColor& color,
+ float length,
+ float width,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeArrowVisu(const std::string& layerName,
+ const std::string& arrowName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeArrowDebugLayerVisu(const std::string& arrowName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+
+ void setCircleArrowVisu(const std::string& layerName,
+ const std::string& circleName,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& directionVec,
+ Ice::Float radius,
+ Ice::Float circleCompletion,
+ Ice::Float width,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setCircleDebugLayerVisu(const std::string& circleName,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& directionVec,
+ Ice::Float radius,
+ Ice::Float circleCompletion,
+ Ice::Float width,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCircleVisu(const std::string& layerName,
+ const std::string& circleName,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCircleDebugLayerVisu(const std::string& circleName,
+ const Ice::Current& = Ice::emptyCurrent) override;
/*!
@@ -229,41 +412,74 @@ namespace armarx
* \param DrawStyle Either FullModel ior CollisionModel.
* \param armarxProject Additional armarx project that should be used to search the robot. Must be locally available and accessible through the armarx cmake search procedure.
*/
- void setRobotVisu(const std::string& layerName, const std::string& robotName, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyle, const ::Ice::Current& = Ice::emptyCurrent) override;
- void updateRobotPose(const std::string& layerName, const std::string& robotName, const ::armarx::PoseBasePtr& globalPose, const ::Ice::Current& = Ice::emptyCurrent) override;
- void updateRobotConfig(const std::string& layerName, const std::string& robotName, const std::map< std::string, float>& configuration, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setRobotVisu(const std::string& layerName,
+ const std::string& robotName,
+ const std::string& robotFile,
+ const std::string& armarxProject,
+ DrawStyle drawStyle,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotPose(const std::string& layerName,
+ const std::string& robotName,
+ const ::armarx::PoseBasePtr& globalPose,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotConfig(const std::string& layerName,
+ const std::string& robotName,
+ const std::map<std::string, float>& configuration,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
/*!
* \brief updateRobotColor Colorizes the robot visualization
* \param layerName The layer
* \param robotName The robot identifyer
* \param c The draw color, if all is set to 0, the colorization is disabled (i.e. the original vizaulization shows up)
*/
- void updateRobotColor(const std::string& layerName, const std::string& robotName, const armarx::DrawColor& c, const ::Ice::Current& = Ice::emptyCurrent) override;
- void updateRobotNodeColor(const std::string& layerName, const std::string& robotName, const std::string& robotNodeName, const armarx::DrawColor& c, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeRobotVisu(const std::string& layerName, const std::string& robotName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotColor(const std::string& layerName,
+ const std::string& robotName,
+ const armarx::DrawColor& c,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotNodeColor(const std::string& layerName,
+ const std::string& robotName,
+ const std::string& robotNodeName,
+ const armarx::DrawColor& c,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeRobotVisu(const std::string& layerName,
+ const std::string& robotName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
void clearAll(const ::Ice::Current& = Ice::emptyCurrent) override;
- void clearLayer(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void clearLayer(const std::string& layerName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
void clearDebugLayer(const ::Ice::Current& = Ice::emptyCurrent) override;
- bool hasLayer(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void removeLayer(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ bool hasLayer(const std::string& layerName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void removeLayer(const std::string& layerName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- void enableLayerVisu(const std::string& layerName, bool visible, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void enableLayerVisu(const std::string& layerName,
+ bool visible,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
void enableDebugLayerVisu(bool visible, const ::Ice::Current& = Ice::emptyCurrent) override;
::Ice::StringSeq layerNames(const ::Ice::Current& = Ice::emptyCurrent) override;
- ::armarx::LayerInformationSequence layerInformation(const ::Ice::Current& = Ice::emptyCurrent) override;
+ ::armarx::LayerInformationSequence
+ layerInformation(const ::Ice::Current& = Ice::emptyCurrent) override;
void disableAllLayers(const ::Ice::Current& = Ice::emptyCurrent) override;
void enableAllLayers(const ::Ice::Current& = Ice::emptyCurrent) override;
// Methods for selection management
- void enableSelections(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void disableSelections(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void clearSelections(const std::string& layerName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void select(const std::string& layerName, const std::string& elementName, const ::Ice::Current& = Ice::emptyCurrent) override;
- void deselect(const std::string& layerName, const std::string& elementName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void enableSelections(const std::string& layerName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void disableSelections(const std::string& layerName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void clearSelections(const std::string& layerName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void select(const std::string& layerName,
+ const std::string& elementName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void deselect(const std::string& layerName,
+ const std::string& elementName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
DebugDrawerSelectionList getSelections(const ::Ice::Current& = Ice::emptyCurrent) override;
@@ -272,7 +488,8 @@ namespace armarx
void installSelectionCallbacks();
void removeSelectionCallbacks();
- void reportSelectionChanged(const DebugDrawerSelectionList& selectedElements, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void reportSelectionChanged(const DebugDrawerSelectionList& selectedElements,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
// This is all total madness. Why do we put mutexes and locks into shared pointers?
using RecursiveMutex = std::recursive_mutex;
@@ -296,23 +513,32 @@ namespace armarx
~DebugDrawerComponent() override;
- void setDefaultLayerVisibility(bool defaultLayerVisibility)
+ void
+ setDefaultLayerVisibility(bool defaultLayerVisibility)
{
this->defaultLayerVisibility = defaultLayerVisibility;
}
- void setDefaultLayerVisibility(const std::string& layerName, bool defaultLayerVisibility)
+
+ void
+ setDefaultLayerVisibility(const std::string& layerName, bool defaultLayerVisibility)
{
defaultLayerVisibilityPerLayer[layerName] = defaultLayerVisibility;
}
- void removeDefaultLayerVisibility(const std::string& layerName)
+
+ void
+ removeDefaultLayerVisibility(const std::string& layerName)
{
defaultLayerVisibilityPerLayer.erase(layerName);
}
- bool getDefaultLayerVisibility() const
+
+ bool
+ getDefaultLayerVisibility() const
{
return defaultLayerVisibility;
}
- bool getDefaultLayerVisibility(const std::string& layerName) const
+
+ bool
+ getDefaultLayerVisibility(const std::string& layerName) const
{
auto elem = defaultLayerVisibilityPerLayer.find(layerName);
if (elem != defaultLayerVisibilityPerLayer.end())
@@ -321,12 +547,14 @@ namespace armarx
}
return defaultLayerVisibility;
}
- std::map<std::string, bool> getAllDefaultLayerVisibilities() const
+
+ std::map<std::string, bool>
+ getAllDefaultLayerVisibilities() const
{
return defaultLayerVisibilityPerLayer;
}
- protected:
+ protected:
static void updateVisualizationCB(void* data, SoSensor* sensor);
// It turned out, that heavy data traffic (resulting in heavy mutex locking) somehow interferes with coin rendering: the
@@ -341,6 +569,7 @@ namespace armarx
active = false;
update = false;
}
+
std::string layerName;
std::string name;
bool active;
@@ -355,6 +584,7 @@ namespace armarx
Eigen::Matrix4f globalPose;
float scale;
};
+
struct LineData : public DrawData
{
Eigen::Vector3f p1;
@@ -362,10 +592,12 @@ namespace armarx
float scale;
VirtualRobot::VisualizationFactory::Color color;
};
+
struct LineSetData : public DrawData
{
DebugDrawerLineSet lineSet;
};
+
struct BoxData : public DrawData
{
Eigen::Matrix4f globalPose;
@@ -374,6 +606,7 @@ namespace armarx
float depth;
VirtualRobot::VisualizationFactory::Color color;
};
+
struct TextData : public DrawData
{
std::string text;
@@ -381,12 +614,14 @@ namespace armarx
VirtualRobot::VisualizationFactory::Color color;
int size;
};
+
struct SphereData : public DrawData
{
Eigen::Vector3f position;
VirtualRobot::VisualizationFactory::Color color;
float radius;
};
+
struct CylinderData : public DrawData
{
Eigen::Vector3f position;
@@ -395,6 +630,7 @@ namespace armarx
float radius;
VirtualRobot::VisualizationFactory::Color color;
};
+
struct CircleData : public DrawData
{
Eigen::Vector3f position;
@@ -404,25 +640,30 @@ namespace armarx
float circleCompletion;
VirtualRobot::VisualizationFactory::Color color;
};
+
struct PointCloudData : public DrawData
{
DebugDrawerPointCloud pointCloud;
};
+
struct ColoredPointCloudData : public DrawData
{
DebugDrawerColoredPointCloud pointCloud;
};
+
struct Colored24BitPointCloudData : public DrawData
{
DebugDrawer24BitColoredPointCloud pointCloud;
};
+
struct PolygonData : public DrawData
{
- std::vector< Eigen::Vector3f > points;
+ std::vector<Eigen::Vector3f> points;
float lineWidth;
VirtualRobot::VisualizationFactory::Color colorInner;
VirtualRobot::VisualizationFactory::Color colorBorder;
};
+
struct TriMeshData : public DrawData
{
DebugDrawerTriMesh triMesh;
@@ -436,6 +677,7 @@ namespace armarx
float width;
VirtualRobot::VisualizationFactory::Color color;
};
+
struct RobotData : public DrawData
{
RobotData()
@@ -444,6 +686,7 @@ namespace armarx
updatePose = false;
updateConfig = false;
}
+
std::string robotFile;
std::string armarxProject;
@@ -451,13 +694,13 @@ namespace armarx
VirtualRobot::VisualizationFactory::Color color;
bool updateNodeColor;
- std::map < std::string, VirtualRobot::VisualizationFactory::Color > nodeColors;
+ std::map<std::string, VirtualRobot::VisualizationFactory::Color> nodeColors;
bool updatePose;
Eigen::Matrix4f globalPose;
bool updateConfig;
- std::map < std::string, float > configuration;
+ std::map<std::string, float> configuration;
armarx::DrawStyle drawStyle;
};
@@ -480,8 +723,8 @@ namespace armarx
std::map<std::string, ArrowData> arrows;
std::map<std::string, RobotData> robots;
- std::set< std::string > clearLayers;
- std::set< std::string > removeLayers;
+ std::set<std::string> clearLayers;
+ std::set<std::string> removeLayers;
};
UpdateData accumulatedUpdateData;
@@ -492,10 +735,20 @@ namespace armarx
/*!
* \brief onUpdateVisualization derived methods can overwrite this method to update custom visualizations.
*/
- virtual void onUpdateVisualization() {}
- virtual void onRemoveAccumulatedData(const std::string& layerName) {}
+ virtual void
+ onUpdateVisualization()
+ {
+ }
- virtual void removeCustomVisu(const std::string& layerName, const std::string& name) {}
+ virtual void
+ onRemoveAccumulatedData(const std::string& layerName)
+ {
+ }
+
+ virtual void
+ removeCustomVisu(const std::string& layerName, const std::string& name)
+ {
+ }
void drawCoordSystem(const CoordData& d);
void drawLine(const LineData& d);
@@ -530,7 +783,6 @@ namespace armarx
void removeRobot(const std::string& layerName, const std::string& name);
-
/*!
* QT safe version, should not be called from an ICE thread
*/
@@ -560,7 +812,8 @@ namespace armarx
std::map<std::string, SoSeparator*> addedTriMeshVisualizations;
std::map<std::string, SoSeparator*> addedArrowVisualizations;
std::map<std::string, SoSeparator*> addedRobotVisualizations;
- std::map<std::string, SoSeparator*> addedCustomVisualizations; // these separators are only used by derived classes and have to start with a material node
+ std::map<std::string, SoSeparator*>
+ addedCustomVisualizations; // these separators are only used by derived classes and have to start with a material node
bool visible;
};
@@ -579,7 +832,7 @@ namespace armarx
*/
void ensureExistingRobotNodes(DebugDrawerComponent::RobotData& d);
- std::map < std::string, VirtualRobot::RobotPtr > activeRobots;
+ std::map<std::string, VirtualRobot::RobotPtr> activeRobots;
SoSeparator* coinVisu;
SoSelection* selectionNode;
@@ -620,9 +873,8 @@ namespace armarx
bool defaultLayerVisibility;
std::map<std::string, bool> defaultLayerVisibilityPerLayer;
-
};
using DebugDrawerComponentPtr = IceInternal::Handle<DebugDrawerComponent>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
index e0293363b3b56891468b8c1a88950e6c7e09ae6e..addc948fa2c4ab1fb813e8ea800cb6ec2aaf30a3 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -27,8 +27,8 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/math/Helpers.h>
-#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -37,15 +37,18 @@ using namespace math;
namespace armarx::detail::DebugDrawerHelper
{
FrameView::FrameView(class DebugDrawerHelper& helper, const Eigen::Matrix4f frame) :
- _helper{&helper},
- _fallbackFrame{frame}
- {}
+ _helper{&helper}, _fallbackFrame{frame}
+ {
+ }
+
FrameView::FrameView(class DebugDrawerHelper& helper, const VirtualRobot::RobotNodePtr& rn) :
- _helper{&helper},
- _rn{rn}
- {}
+ _helper{&helper}, _rn{rn}
+ {
+ }
+
//make global
- Eigen::Matrix4f FrameView::makeGlobalEigen(const Eigen::Matrix4f& pose) const
+ Eigen::Matrix4f
+ FrameView::makeGlobalEigen(const Eigen::Matrix4f& pose) const
{
ARMARX_TRACE;
if (_rn)
@@ -54,7 +57,9 @@ namespace armarx::detail::DebugDrawerHelper
}
return _fallbackFrame * pose;
}
- Eigen::Vector3f FrameView::makeGlobalEigen(const Eigen::Vector3f& position) const
+
+ Eigen::Vector3f
+ FrameView::makeGlobalEigen(const Eigen::Vector3f& position) const
{
ARMARX_TRACE;
if (_rn)
@@ -63,7 +68,9 @@ namespace armarx::detail::DebugDrawerHelper
}
return (_fallbackFrame * position.homogeneous()).eval().hnormalized();
}
- Eigen::Vector3f FrameView::makeGlobalDirectionEigen(const Eigen::Vector3f& direction) const
+
+ Eigen::Vector3f
+ FrameView::makeGlobalDirectionEigen(const Eigen::Vector3f& direction) const
{
ARMARX_TRACE;
if (_rn)
@@ -72,17 +79,23 @@ namespace armarx::detail::DebugDrawerHelper
}
return ::math::Helpers::TransformDirection(_fallbackFrame, direction);
}
- PosePtr FrameView::makeGlobal(const Eigen::Matrix4f& pose) const
+
+ PosePtr
+ FrameView::makeGlobal(const Eigen::Matrix4f& pose) const
{
ARMARX_TRACE;
return new Pose(makeGlobalEigen(pose));
}
- Vector3Ptr FrameView::makeGlobal(const Eigen::Vector3f& position) const
+
+ Vector3Ptr
+ FrameView::makeGlobal(const Eigen::Vector3f& position) const
{
ARMARX_TRACE;
return new Vector3(makeGlobalEigen(position));
}
- Vector3Ptr FrameView::makeGlobalDirection(const Eigen::Vector3f& direction) const
+
+ Vector3Ptr
+ FrameView::makeGlobalDirection(const Eigen::Vector3f& direction) const
{
ARMARX_TRACE;
return new Vector3(makeGlobalDirectionEigen(direction));
@@ -90,57 +103,103 @@ namespace armarx::detail::DebugDrawerHelper
//1st order
using DrawElementType = ::armarx::DebugDrawerHelper::DrawElementType;
-#define CHECK_AND_ADD(name,type) if (!_helper->enableVisu) {return;} _helper->addNewElement(name,type);
+#define CHECK_AND_ADD(name, type) \
+ if (!_helper->enableVisu) \
+ { \
+ return; \
+ } \
+ _helper->addNewElement(name, type);
- void FrameView::drawPose(const std::string& name, const Eigen::Matrix4f& pose)
+ void
+ FrameView::drawPose(const std::string& name, const Eigen::Matrix4f& pose)
{
ARMARX_TRACE;
CHECK_AND_ADD(name, DrawElementType::Pose)
_helper->debugDrawerPrx->setPoseVisu(_helper->layerName, name, makeGlobal(pose));
}
- void FrameView::drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale)
+ void
+ FrameView::drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale)
{
ARMARX_TRACE;
CHECK_AND_ADD(name, DrawElementType::Pose)
- _helper->debugDrawerPrx->setScaledPoseVisu(_helper->layerName, name, makeGlobal(pose), scale);
+ _helper->debugDrawerPrx->setScaledPoseVisu(
+ _helper->layerName, name, makeGlobal(pose), scale);
}
- void FrameView::drawBox(const std::string& name, const Eigen::Matrix4f& pose, const Eigen::Vector3f& size, const DrawColor& color)
+ void
+ FrameView::drawBox(const std::string& name,
+ const Eigen::Matrix4f& pose,
+ const Eigen::Vector3f& size,
+ const DrawColor& color)
{
ARMARX_TRACE;
CHECK_AND_ADD(name, DrawElementType::Box)
- _helper->debugDrawerPrx->setBoxVisu(_helper->layerName, name, makeGlobal(pose), new Vector3(size), color);
+ _helper->debugDrawerPrx->setBoxVisu(
+ _helper->layerName, name, makeGlobal(pose), new Vector3(size), color);
}
- void FrameView::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float width, const DrawColor& color)
+ void
+ FrameView::drawLine(const std::string& name,
+ const Eigen::Vector3f& p1,
+ const Eigen::Vector3f& p2,
+ float width,
+ const DrawColor& color)
{
ARMARX_TRACE;
CHECK_AND_ADD(name, DrawElementType::Line)
- _helper->debugDrawerPrx->setLineVisu(_helper->layerName, name, makeGlobal(p1), makeGlobal(p2), width, color);
+ _helper->debugDrawerPrx->setLineVisu(
+ _helper->layerName, name, makeGlobal(p1), makeGlobal(p2), width, color);
}
- void FrameView::drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size)
+ void
+ FrameView::drawText(const std::string& name,
+ const Eigen::Vector3f& p1,
+ const std::string& text,
+ const DrawColor& color,
+ int size)
{
ARMARX_TRACE;
CHECK_AND_ADD(name, DrawElementType::Text)
- _helper->debugDrawerPrx->setTextVisu(_helper->layerName, name, text, makeGlobal(p1), color, size);
+ _helper->debugDrawerPrx->setTextVisu(
+ _helper->layerName, name, text, makeGlobal(p1), color, size);
}
- void FrameView::drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width)
+ void
+ FrameView::drawArrow(const std::string& name,
+ const Eigen::Vector3f& pos,
+ const Eigen::Vector3f& direction,
+ const DrawColor& color,
+ float length,
+ float width)
{
ARMARX_TRACE;
CHECK_AND_ADD(name, DrawElementType::Arrow)
- _helper->debugDrawerPrx->setArrowVisu(_helper->layerName, name, makeGlobal(pos), makeGlobalDirection(direction), color, length, width);
+ _helper->debugDrawerPrx->setArrowVisu(_helper->layerName,
+ name,
+ makeGlobal(pos),
+ makeGlobalDirection(direction),
+ color,
+ length,
+ width);
}
- void FrameView::drawSphere(const std::string& name, const Eigen::Vector3f& position, float size, const DrawColor& color)
+ void
+ FrameView::drawSphere(const std::string& name,
+ const Eigen::Vector3f& position,
+ float size,
+ const DrawColor& color)
{
ARMARX_TRACE;
- _helper->debugDrawerPrx->setSphereVisu(_helper->layerName, name, makeGlobal(position), color, size);
+ _helper->debugDrawerPrx->setSphereVisu(
+ _helper->layerName, name, makeGlobal(position), color, size);
}
- void FrameView::drawPointCloud(const std::string& name, const std::vector<Eigen::Vector3f>& points, float pointSize, const DrawColor& color)
+ void
+ FrameView::drawPointCloud(const std::string& name,
+ const std::vector<Eigen::Vector3f>& points,
+ float pointSize,
+ const DrawColor& color)
{
ARMARX_TRACE;
CHECK_AND_ADD(name, DrawElementType::ColoredPointCloud)
@@ -153,40 +212,54 @@ namespace armarx::detail::DebugDrawerHelper
e.x = pg(0);
e.y = pg(1);
e.z = pg(2);
- e.color = color; //DrawColor24Bit {(Ice::Byte)(color.r * 255), (Ice::Byte)(color.g * 255), (Ice::Byte)(color.b * 255)};
+ e.color =
+ color; //DrawColor24Bit {(Ice::Byte)(color.r * 255), (Ice::Byte)(color.g * 255), (Ice::Byte)(color.b * 255)};
pc.points.push_back(e);
}
_helper->debugDrawerPrx->setColoredPointCloudVisu(_helper->layerName, name, pc);
}
- void FrameView::drawRobot(const std::string& name, const std::string& robotFile, const std::string& armarxProject, const Eigen::Matrix4f& pose, const DrawColor& color)
+ void
+ FrameView::drawRobot(const std::string& name,
+ const std::string& robotFile,
+ const std::string& armarxProject,
+ const Eigen::Matrix4f& pose,
+ const DrawColor& color)
{
ARMARX_TRACE;
CHECK_AND_ADD(name, DrawElementType::Robot)
- _helper->debugDrawerPrx->setRobotVisu(_helper->layerName, name, robotFile, armarxProject, DrawStyle::FullModel);
+ _helper->debugDrawerPrx->setRobotVisu(
+ _helper->layerName, name, robotFile, armarxProject, DrawStyle::FullModel);
_helper->debugDrawerPrx->updateRobotPose(_helper->layerName, name, makeGlobal(pose));
_helper->debugDrawerPrx->updateRobotColor(_helper->layerName, name, color);
}
- void FrameView::setRobotConfig(const std::string& name, const std::map<std::string, float>& config)
+ void
+ FrameView::setRobotConfig(const std::string& name, const std::map<std::string, float>& config)
{
ARMARX_TRACE;
_helper->debugDrawerPrx->updateRobotConfig(_helper->layerName, name, config);
}
- void FrameView::setRobotColor(const std::string& name, const DrawColor& color)
+
+ void
+ FrameView::setRobotColor(const std::string& name, const DrawColor& color)
{
ARMARX_TRACE;
_helper->debugDrawerPrx->updateRobotColor(_helper->layerName, name, color);
}
- void FrameView::setRobotPose(const std::string& name, const Eigen::Matrix4f& pose)
+
+ void
+ FrameView::setRobotPose(const std::string& name, const Eigen::Matrix4f& pose)
{
ARMARX_TRACE;
_helper->debugDrawerPrx->updateRobotPose(_helper->layerName, name, makeGlobal(pose));
}
+
#undef CHECK_AND_ADD
//2nd order
- void FrameView::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses)
+ void
+ FrameView::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses)
{
ARMARX_TRACE;
for (std::size_t idx = 0; idx < poses.size(); ++idx)
@@ -195,7 +268,11 @@ namespace armarx::detail::DebugDrawerHelper
drawPose(prefix + std::to_string(idx), pose);
}
}
- void FrameView::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses, float scale)
+
+ void
+ FrameView::drawPoses(const std::string& prefix,
+ const std::vector<Eigen::Matrix4f>& poses,
+ float scale)
{
ARMARX_TRACE;
for (std::size_t idx = 0; idx < poses.size(); ++idx)
@@ -205,25 +282,43 @@ namespace armarx::detail::DebugDrawerHelper
}
}
- void FrameView::drawBox(const std::string& name, const Eigen::Vector3f& position, float size, const DrawColor& color)
+ void
+ FrameView::drawBox(const std::string& name,
+ const Eigen::Vector3f& position,
+ float size,
+ const DrawColor& color)
{
ARMARX_TRACE;
- drawBox(name, Helpers::CreatePose(position, Eigen::Matrix3f::Identity()), Eigen::Vector3f(size, size, size), color);
+ drawBox(name,
+ Helpers::CreatePose(position, Eigen::Matrix3f::Identity()),
+ Eigen::Vector3f(size, size, size),
+ color);
}
- void FrameView::drawBox(const std::string& name, const Eigen::Matrix4f& pose, float size, const DrawColor& color)
+ void
+ FrameView::drawBox(const std::string& name,
+ const Eigen::Matrix4f& pose,
+ float size,
+ const DrawColor& color)
{
ARMARX_TRACE;
drawBox(name, pose, Eigen::Vector3f(size, size, size), color);
}
- void FrameView::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2)
+ void
+ FrameView::drawLine(const std::string& name,
+ const Eigen::Vector3f& p1,
+ const Eigen::Vector3f& p2)
{
ARMARX_TRACE;
drawLine(name, p1, p2, _helper->defaults.lineWidth, _helper->defaults.lineColor);
}
- void FrameView::drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps, float width, const DrawColor& color)
+ void
+ FrameView::drawLines(const std::string& prefix,
+ const std::vector<Eigen::Vector3f>& ps,
+ float width,
+ const DrawColor& color)
{
ARMARX_TRACE;
for (std::size_t idx = 1; idx < ps.size(); ++idx)
@@ -231,7 +326,9 @@ namespace armarx::detail::DebugDrawerHelper
drawLine(prefix + std::to_string(idx), ps.at(idx - 1), ps.at(idx), width, color);
}
}
- void FrameView::drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps)
+
+ void
+ FrameView::drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps)
{
ARMARX_TRACE;
for (std::size_t idx = 1; idx < ps.size(); ++idx)
@@ -239,7 +336,12 @@ namespace armarx::detail::DebugDrawerHelper
drawLine(prefix + std::to_string(idx), ps.at(idx - 1), ps.at(idx));
}
}
- void FrameView::drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps, float width, const DrawColor& color)
+
+ void
+ FrameView::drawLines(const std::string& prefix,
+ const std::vector<Eigen::Matrix4f>& ps,
+ float width,
+ const DrawColor& color)
{
ARMARX_TRACE;
for (std::size_t idx = 1; idx < ps.size(); ++idx)
@@ -247,10 +349,13 @@ namespace armarx::detail::DebugDrawerHelper
drawLine(prefix + std::to_string(idx),
ps.at(idx - 1).topRightCorner<3, 1>(),
ps.at(idx).topRightCorner<3, 1>(),
- width, color);
+ width,
+ color);
}
}
- void FrameView::drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps)
+
+ void
+ FrameView::drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps)
{
ARMARX_TRACE;
for (std::size_t idx = 1; idx < ps.size(); ++idx)
@@ -260,27 +365,29 @@ namespace armarx::detail::DebugDrawerHelper
ps.at(idx).topRightCorner<3, 1>());
}
}
-}
+} // namespace armarx::detail::DebugDrawerHelper
namespace armarx
{
- DebugDrawerHelper::DebugDrawerHelper(
- const DebugDrawerInterfacePrx& debugDrawerPrx,
- const std::string& layerName,
- const VirtualRobot::RobotPtr& robot
- ) :
+ DebugDrawerHelper::DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx,
+ const std::string& layerName,
+ const VirtualRobot::RobotPtr& robot) :
FrameView(*this, robot ? robot->getRootNode() : nullptr),
- debugDrawerPrx(debugDrawerPrx), layerName(layerName), robot(robot)
+ debugDrawerPrx(debugDrawerPrx),
+ layerName(layerName),
+ robot(robot)
{
ARMARX_CHECK_NOT_NULL(debugDrawerPrx);
}
DebugDrawerHelper::DebugDrawerHelper(const std::string& layerName) :
- FrameView(*this, nullptr),
- layerName{layerName}
- {}
+ FrameView(*this, nullptr), layerName{layerName}
+ {
+ }
+
//utility
- void DebugDrawerHelper::clearLayer()
+ void
+ DebugDrawerHelper::clearLayer()
{
ARMARX_TRACE;
debugDrawerPrx->clearLayer(layerName);
@@ -288,7 +395,8 @@ namespace armarx
newElements.clear();
}
- void DebugDrawerHelper::cyclicCleanup()
+ void
+ DebugDrawerHelper::cyclicCleanup()
{
ARMARX_TRACE;
/*
@@ -318,12 +426,15 @@ namespace armarx
newElements.clear();
}
- void DebugDrawerHelper::setVisuEnabled(bool enableVisu)
+ void
+ DebugDrawerHelper::setVisuEnabled(bool enableVisu)
{
this->enableVisu = enableVisu;
}
- void DebugDrawerHelper::removeElement(const std::string& name, DebugDrawerHelper::DrawElementType type)
+ void
+ DebugDrawerHelper::removeElement(const std::string& name,
+ DebugDrawerHelper::DrawElementType type)
{
ARMARX_TRACE;
switch (type)
@@ -352,69 +463,87 @@ namespace armarx
}
}
- const VirtualRobot::RobotPtr& DebugDrawerHelper::getRobot() const
+ const VirtualRobot::RobotPtr&
+ DebugDrawerHelper::getRobot() const
{
return robot;
}
- const DebugDrawerInterfacePrx& DebugDrawerHelper::getDebugDrawer() const
+ const DebugDrawerInterfacePrx&
+ DebugDrawerHelper::getDebugDrawer() const
{
return debugDrawerPrx;
}
- void DebugDrawerHelper::setDebugDrawer(const DebugDrawerInterfacePrx& drawer)
+ void
+ DebugDrawerHelper::setDebugDrawer(const DebugDrawerInterfacePrx& drawer)
{
ARMARX_CHECK_NOT_NULL(drawer);
debugDrawerPrx = drawer;
}
- void DebugDrawerHelper::setRobot(const VirtualRobot::RobotPtr& rob)
+ void
+ DebugDrawerHelper::setRobot(const VirtualRobot::RobotPtr& rob)
{
robot = rob;
}
- DebugDrawerHelper::FrameView DebugDrawerHelper::inFrame(const Eigen::Matrix4f& frame)
+ DebugDrawerHelper::FrameView
+ DebugDrawerHelper::inFrame(const Eigen::Matrix4f& frame)
{
return {*this, frame};
}
- DebugDrawerHelper::FrameView DebugDrawerHelper::inGlobalFrame()
+
+ DebugDrawerHelper::FrameView
+ DebugDrawerHelper::inGlobalFrame()
{
return {*this, Eigen::Matrix4f::Identity()};
}
- DebugDrawerHelper::FrameView DebugDrawerHelper::inFrame(const std::string& nodeName)
+
+ DebugDrawerHelper::FrameView
+ DebugDrawerHelper::inFrame(const std::string& nodeName)
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
ARMARX_CHECK_EXPRESSION(robot->hasRobotNode(nodeName));
return {*this, robot->getRobotNode(nodeName)};
}
- DebugDrawerHelper::FrameView DebugDrawerHelper::inFrame(const VirtualRobot::RobotNodePtr& node)
+
+ DebugDrawerHelper::FrameView
+ DebugDrawerHelper::inFrame(const VirtualRobot::RobotNodePtr& node)
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(node);
return {*this, node};
}
- void DebugDrawerHelper::setDefaultFrame(const Eigen::Matrix4f& frame)
+ void
+ DebugDrawerHelper::setDefaultFrame(const Eigen::Matrix4f& frame)
{
ARMARX_TRACE;
_rn = nullptr;
_fallbackFrame = frame;
}
- void DebugDrawerHelper::setDefaultFrameToGlobal()
+
+ void
+ DebugDrawerHelper::setDefaultFrameToGlobal()
{
ARMARX_TRACE;
_rn = nullptr;
_fallbackFrame = Eigen::Matrix4f::Identity();
}
- void DebugDrawerHelper::setDefaultFrame(const std::string& nodeName)
+
+ void
+ DebugDrawerHelper::setDefaultFrame(const std::string& nodeName)
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
ARMARX_CHECK_EXPRESSION(robot->hasRobotNode(nodeName));
_rn = robot->getRobotNode(nodeName);
}
- void DebugDrawerHelper::setDefaultFrame(const VirtualRobot::RobotNodePtr& node)
+
+ void
+ DebugDrawerHelper::setDefaultFrame(const VirtualRobot::RobotNodePtr& node)
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
@@ -422,26 +551,26 @@ namespace armarx
_rn = node;
}
- void DebugDrawerHelper::addNewElement(const std::string& name, DebugDrawerHelper::DrawElementType type)
+ void
+ DebugDrawerHelper::addNewElement(const std::string& name,
+ DebugDrawerHelper::DrawElementType type)
{
ARMARX_TRACE;
- newElements.insert(DrawElement {name, type});
+ newElements.insert(DrawElement{name, type});
}
- const DrawColor DebugDrawerHelper::Colors::Red = DrawColor {1, 0, 0, 1};
- const DrawColor DebugDrawerHelper::Colors::Green = DrawColor {0, 1, 0, 1};
- const DrawColor DebugDrawerHelper::Colors::Blue = DrawColor {0, 0, 1, 1};
- const DrawColor DebugDrawerHelper::Colors::Yellow = DrawColor {1, 1, 0, 1};
- const DrawColor DebugDrawerHelper::Colors::Orange = DrawColor {1, 0.5f, 0, 1};
+ const DrawColor DebugDrawerHelper::Colors::Red = DrawColor{1, 0, 0, 1};
+ const DrawColor DebugDrawerHelper::Colors::Green = DrawColor{0, 1, 0, 1};
+ const DrawColor DebugDrawerHelper::Colors::Blue = DrawColor{0, 0, 1, 1};
+ const DrawColor DebugDrawerHelper::Colors::Yellow = DrawColor{1, 1, 0, 1};
+ const DrawColor DebugDrawerHelper::Colors::Orange = DrawColor{1, 0.5f, 0, 1};
- DrawColor DebugDrawerHelper::Colors::Lerp(const DrawColor& a, const DrawColor& b, float f)
+ DrawColor
+ DebugDrawerHelper::Colors::Lerp(const DrawColor& a, const DrawColor& b, float f)
{
- return DrawColor
- {
- ::math::Helpers::Lerp(a.r, b.r, f),
- ::math::Helpers::Lerp(a.g, b.g, f),
- ::math::Helpers::Lerp(a.b, b.b, f),
- ::math::Helpers::Lerp(a.a, b.a, f)
- };
+ return DrawColor{::math::Helpers::Lerp(a.r, b.r, f),
+ ::math::Helpers::Lerp(a.g, b.g, f),
+ ::math::Helpers::Lerp(a.b, b.b, f),
+ ::math::Helpers::Lerp(a.a, b.a, f)};
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
index 9da37492f73fd7b08310591a3ed294f6767acce7..9515b19666d328ef9202af7ead85a99f406e9f4f 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -23,9 +23,9 @@
#pragma once
-#include <VirtualRobot/VirtualRobot.h>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <SimoxUtility/shapes/XYConstrainedOrientedBox.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -34,7 +34,7 @@ namespace armarx
{
class DebugDrawerHelper;
using DebugDrawerHelperPtr = std::shared_ptr<DebugDrawerHelper>;
-}
+} // namespace armarx
namespace armarx::detail::DebugDrawerHelper
{
@@ -47,7 +47,8 @@ namespace armarx::detail::DebugDrawerHelper
class FrameView
{
public:
- FrameView(class DebugDrawerHelper& helper, const Eigen::Matrix4f frame = Eigen::Matrix4f::Identity());
+ FrameView(class DebugDrawerHelper& helper,
+ const Eigen::Matrix4f frame = Eigen::Matrix4f::Identity());
FrameView(class DebugDrawerHelper& helper, const VirtualRobot::RobotNodePtr& rn);
//make global
public:
@@ -63,19 +64,45 @@ namespace armarx::detail::DebugDrawerHelper
void drawPose(const std::string& name, const Eigen::Matrix4f& pose);
void drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale);
- void drawBox(const std::string& name, const Eigen::Matrix4f& pose, const Eigen::Vector3f& size, const DrawColor& color);
+ void drawBox(const std::string& name,
+ const Eigen::Matrix4f& pose,
+ const Eigen::Vector3f& size,
+ const DrawColor& color);
- void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float width, const DrawColor& color);
+ void drawLine(const std::string& name,
+ const Eigen::Vector3f& p1,
+ const Eigen::Vector3f& p2,
+ float width,
+ const DrawColor& color);
- void drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size);
+ void drawText(const std::string& name,
+ const Eigen::Vector3f& p1,
+ const std::string& text,
+ const DrawColor& color,
+ int size);
- void drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width);
+ void drawArrow(const std::string& name,
+ const Eigen::Vector3f& pos,
+ const Eigen::Vector3f& direction,
+ const DrawColor& color,
+ float length,
+ float width);
- void drawSphere(const std::string& name, const Eigen::Vector3f& position, float size, const DrawColor& color);
+ void drawSphere(const std::string& name,
+ const Eigen::Vector3f& position,
+ float size,
+ const DrawColor& color);
- void drawPointCloud(const std::string& name, const std::vector<Eigen::Vector3f>& points, float pointSize, const DrawColor& color);
+ void drawPointCloud(const std::string& name,
+ const std::vector<Eigen::Vector3f>& points,
+ float pointSize,
+ const DrawColor& color);
- void drawRobot(const std::string& name, const std::string& robotFile, const std::string& armarxProject, const Eigen::Matrix4f& pose, const DrawColor& color);
+ void drawRobot(const std::string& name,
+ const std::string& robotFile,
+ const std::string& armarxProject,
+ const Eigen::Matrix4f& pose,
+ const DrawColor& color);
void setRobotConfig(const std::string& name, const std::map<std::string, float>& config);
void setRobotColor(const std::string& name, const DrawColor& color);
void setRobotPose(const std::string& name, const Eigen::Matrix4f& pose);
@@ -83,34 +110,63 @@ namespace armarx::detail::DebugDrawerHelper
//2nd order draw functions (call 1st order)
public:
void drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses);
- void drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses, float scale);
+ void drawPoses(const std::string& prefix,
+ const std::vector<Eigen::Matrix4f>& poses,
+ float scale);
+
+ void drawBox(const std::string& name,
+ const Eigen::Vector3f& position,
+ float size,
+ const DrawColor& color);
+ void drawBox(const std::string& name,
+ const Eigen::Matrix4f& pose,
+ float size,
+ const DrawColor& color);
- void drawBox(const std::string& name, const Eigen::Vector3f& position, float size, const DrawColor& color);
- void drawBox(const std::string& name, const Eigen::Matrix4f& pose, float size, const DrawColor& color);
- template<class FloatT>
- void drawBox(const std::string& name, const simox::OrientedBox<FloatT>& box, const DrawColor& color)
+ template <class FloatT>
+ void
+ drawBox(const std::string& name,
+ const simox::OrientedBox<FloatT>& box,
+ const DrawColor& color)
{
- drawBox(name, box.template transformation_centered<float>(), box.template dimensions<float>(), color);
+ drawBox(name,
+ box.template transformation_centered<float>(),
+ box.template dimensions<float>(),
+ color);
}
- template<class FloatT>
- void drawBox(const std::string& name, const simox::XYConstrainedOrientedBox<FloatT>& box, const DrawColor& color)
+
+ template <class FloatT>
+ void
+ drawBox(const std::string& name,
+ const simox::XYConstrainedOrientedBox<FloatT>& box,
+ const DrawColor& color)
{
- drawBox(name, box.template transformation_centered<float>(), box.template dimensions<float>(), color);
+ drawBox(name,
+ box.template transformation_centered<float>(),
+ box.template dimensions<float>(),
+ color);
}
+ void
+ drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2);
- void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2);
-
- void drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps, float width, const DrawColor& color);
+ void drawLines(const std::string& prefix,
+ const std::vector<Eigen::Vector3f>& ps,
+ float width,
+ const DrawColor& color);
void drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps);
- void drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps, float width, const DrawColor& color);
+ void drawLines(const std::string& prefix,
+ const std::vector<Eigen::Matrix4f>& ps,
+ float width,
+ const DrawColor& color);
void drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps);
+
protected:
class DebugDrawerHelper* _helper;
Eigen::Matrix4f _fallbackFrame = Eigen::Matrix4f::Identity();
VirtualRobot::RobotNodePtr _rn;
};
-}
+} // namespace armarx::detail::DebugDrawerHelper
namespace armarx
{
@@ -125,26 +181,37 @@ namespace armarx
{
using FrameView = armarx::detail::DebugDrawerHelper::FrameView;
friend class armarx::detail::DebugDrawerHelper::FrameView;
+
public:
struct Defaults
{
float lineWidth = 2;
- DrawColor lineColor = DrawColor {0, 1, 0, 1};
+ DrawColor lineColor = DrawColor{0, 1, 0, 1};
};
enum class DrawElementType
{
- Pose, Box, Line, Text, Arrow, ColoredPointCloud, Robot
+ Pose,
+ Box,
+ Line,
+ Text,
+ Arrow,
+ ColoredPointCloud,
+ Robot
};
+
struct DrawElement
{
std::string name;
DrawElementType type;
- bool operator<(const DrawElement& rhs) const
+
+ bool
+ operator<(const DrawElement& rhs) const
{
int cmp = name.compare(rhs.name);
return cmp < 0 || (cmp == 0 && type < rhs.type);
}
};
+
class Colors
{
public:
@@ -156,7 +223,9 @@ namespace armarx
static DrawColor Lerp(const DrawColor& a, const DrawColor& b, float f);
};
- DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx, const std::string& layerName, const VirtualRobot::RobotPtr& robot);
+ DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx,
+ const std::string& layerName,
+ const VirtualRobot::RobotPtr& robot);
DebugDrawerHelper(const std::string& layerName);
//utility
@@ -194,4 +263,4 @@ namespace armarx
std::string layerName;
VirtualRobot::RobotPtr robot;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
index 7d20ed5fc22362c478b143fe79984b1c90e7d253..c9b9be11ae607731ddfecaa0382789b2716bd036 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
@@ -23,11 +23,12 @@
*/
#pragma once
+#include <Eigen/Core>
+
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include <Eigen/Core>
namespace armarx
{
class DebugDrawerUtils
@@ -35,4 +36,4 @@ namespace armarx
public:
static DebugDrawerTriMesh convertTriMesh(const VirtualRobot::TriMeshModel& trimesh);
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.cpp b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.cpp
index 68c8929d5154bf46f117f2e01ea0c779d87d499b..00d4cfff79bf1b4a77ef3a8d83d0bb7cc1d9797a 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.cpp
+++ b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.cpp
@@ -1,6 +1,5 @@
#include "BlackWhitelist.h"
-
namespace armarx
{
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.h b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.h
index 9b581a520d3c54ff6fee8d9427f6dfd321fd1725..753342c6cb26abb3888d11d7906f8d6957231cd3 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.h
+++ b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelist.h
@@ -1,9 +1,8 @@
#pragma once
+#include <ostream>
#include <set>
#include <string>
-#include <ostream>
-
namespace armarx
{
@@ -19,17 +18,16 @@ namespace armarx
class BlackWhitelist
{
public:
-
/// Construct an empty blacklist and whitelist.
BlackWhitelist() = default;
-
/**
* An element is included if
* (1) it is not in the blacklist, and
* (2) the whitelist is empty or it contains the element.
*/
- bool isIncluded(const Key& element) const
+ bool
+ isIncluded(const Key& element) const
{
return !isExcluded(element);
}
@@ -39,24 +37,24 @@ namespace armarx
* (1) it is in the blacklist, or
* (2) it is not in the non-empty whitelist
*/
- bool isExcluded(const Key& element) const
+ bool
+ isExcluded(const Key& element) const
{
return black.count(element) > 0 || (!white.empty() && white.count(element) == 0);
}
-
/// Elements in this list are always excluded.
std::set<Key> black;
/// If not empty, only these elements are included.
std::set<Key> white;
template <class K>
- friend std::ostream& operator<< (std::ostream& os, const BlackWhitelist<K>& bw);
+ friend std::ostream& operator<<(std::ostream& os, const BlackWhitelist<K>& bw);
};
-
template <class K>
- std::ostream& operator<< (std::ostream& os, const BlackWhitelist<K>& bw)
+ std::ostream&
+ operator<<(std::ostream& os, const BlackWhitelist<K>& bw)
{
os << "Blacklist (" << bw.black.size() << "): ";
for (const auto& e : bw.black)
@@ -73,8 +71,6 @@ namespace armarx
return os;
}
-
using StringBlackWhitelist = BlackWhitelist<std::string>;
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.cpp b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.cpp
index 8d2039c24127ac25aa4b1a20d2d0c92552f145ca..ef4f535f04891fa058e908f1140c75b0bf57599f 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.cpp
+++ b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.cpp
@@ -1,7 +1,7 @@
#include "BlackWhitelistUpdate.h"
-
-void armarx::updateStringList(std::set<std::string>& list, const StringListUpdate& update)
+void
+armarx::updateStringList(std::set<std::string>& list, const StringListUpdate& update)
{
if (update.clear)
{
@@ -13,6 +13,6 @@ void armarx::updateStringList(std::set<std::string>& list, const StringListUpdat
}
else if (!update.set.empty())
{
- list = { update.set.begin(), update.set.end() };
+ list = {update.set.begin(), update.set.end()};
}
}
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.h b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.h
index 5f11bf0bbe82fbac3424fbf051e35790991b964b..3bcbeababcb0b7b73cac1f97566a4cba44e14eee 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.h
+++ b/source/RobotAPI/components/DebugDrawerToArViz/BlackWhitelistUpdate.h
@@ -1,22 +1,19 @@
#pragma once
-#include "BlackWhitelist.h"
-
#include <RobotAPI/interface/core/BlackWhitelist.h>
+#include "BlackWhitelist.h"
namespace armarx
{
void updateStringList(std::set<std::string>& list, const StringListUpdate& update);
-
- inline
- void updateBlackWhitelist(StringBlackWhitelist& bw, const armarx::BlackWhitelistUpdate& update)
+ inline void
+ updateBlackWhitelist(StringBlackWhitelist& bw, const armarx::BlackWhitelistUpdate& update)
{
updateStringList(bw.black, update.blacklist);
updateStringList(bw.white, update.whitelist);
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
index 08b6827e5c9ce6d334d7cadeb6ad9a074a15a02b..229feff22825b1fc985a41c334b5726c306e95ce 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.cpp
@@ -21,70 +21,75 @@
*/
#include "DebugDrawerToArViz.h"
-#include "BlackWhitelistUpdate.h"
+
+#include <SimoxUtility/color/interpolation.h>
+#include <SimoxUtility/math/pose/pose.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <SimoxUtility/color/interpolation.h>
-#include <SimoxUtility/math/pose/pose.h>
+#include "BlackWhitelistUpdate.h"
-#define FUNCTION_NOT_IMPLEMENTED_MESSAGE \
+#define FUNCTION_NOT_IMPLEMENTED_MESSAGE \
"Function DebugDrawerToArViz::" << __FUNCTION__ << "(): Not implemented."
-#define LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE() \
- ARMARX_VERBOSE << FUNCTION_NOT_IMPLEMENTED_MESSAGE
-
+#define LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE() ARMARX_VERBOSE << FUNCTION_NOT_IMPLEMENTED_MESSAGE
namespace armarx
{
namespace
{
- Eigen::Vector3f toEigen(Vector3BasePtr v)
+ Eigen::Vector3f
+ toEigen(Vector3BasePtr v)
{
ARMARX_CHECK_NOT_NULL(v);
- return { v->x, v->y, v->z };
+ return {v->x, v->y, v->z};
}
- Eigen::Vector3f toEigen(DebugDrawerPointCloudElement e)
+ Eigen::Vector3f
+ toEigen(DebugDrawerPointCloudElement e)
{
- return { e.x, e.y, e.z };
+ return {e.x, e.y, e.z};
}
- Eigen::Quaternionf toEigen(QuaternionBasePtr q)
+ Eigen::Quaternionf
+ toEigen(QuaternionBasePtr q)
{
ARMARX_CHECK_NOT_NULL(q);
return Eigen::Quaternionf(q->qw, q->qx, q->qy, q->qz);
}
- Eigen::Matrix4f toEigen(PoseBasePtr pose)
+ Eigen::Matrix4f
+ toEigen(PoseBasePtr pose)
{
ARMARX_CHECK_NOT_NULL(pose);
return simox::math::pose(toEigen(pose->position), toEigen(pose->orientation));
}
-
- simox::Color toSimox(DrawColor c)
+ simox::Color
+ toSimox(DrawColor c)
{
return simox::Color(c.r, c.g, c.b, c.a);
}
- viz::Color toViz(DrawColor c)
+ viz::Color
+ toViz(DrawColor c)
{
return viz::Color(toSimox(c));
}
- }
+ } // namespace
-
- void DebugDrawerToArViz::setArViz(viz::Client arviz)
+ void
+ DebugDrawerToArViz::setArViz(viz::Client arviz)
{
this->arviz = arviz;
}
-
- void DebugDrawerToArViz::updateBlackWhitelist(const BlackWhitelistUpdate& update, const Ice::Current&)
+ void
+ DebugDrawerToArViz::updateBlackWhitelist(const BlackWhitelistUpdate& update,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
@@ -106,18 +111,23 @@ namespace armarx
}
}
-
- void DebugDrawerToArViz::exportScene(const std::string&, const Ice::Current&)
+ void
+ DebugDrawerToArViz::exportScene(const std::string&, const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- void DebugDrawerToArViz::exportLayer(const std::string&, const std::string&, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::exportLayer(const std::string&, const std::string&, const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
-
- void DebugDrawerToArViz::setPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setPoseVisu(const std::string& layer,
+ const std::string& name,
+ const PoseBasePtr& globalPose,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -127,7 +137,12 @@ namespace armarx
setAndCommit(layer, viz::Pose(name).pose(toEigen(globalPose)));
}
- void DebugDrawerToArViz::setScaledPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setScaledPoseVisu(const std::string& layer,
+ const std::string& name,
+ const PoseBasePtr& globalPose,
+ Ice::Float scale,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -137,18 +152,34 @@ namespace armarx
setAndCommit(layer, viz::Pose(name).pose(toEigen(globalPose)).scale(scale));
}
- void DebugDrawerToArViz::setLineVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setLineVisu(const std::string& layer,
+ const std::string& name,
+ const Vector3BasePtr& globalPosition1,
+ const Vector3BasePtr& globalPosition2,
+ Ice::Float lineWidth,
+ const DrawColor& color,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
- setAndCommit(layer, viz::Path(name).addPoint(toEigen(globalPosition1)).addPoint(toEigen(globalPosition2))
- .color(toViz(color)).width(lineWidth).color(simox::Color::black(0)));
+ setAndCommit(layer,
+ viz::Path(name)
+ .addPoint(toEigen(globalPosition1))
+ .addPoint(toEigen(globalPosition2))
+ .color(toViz(color))
+ .width(lineWidth)
+ .color(simox::Color::black(0)));
}
- void DebugDrawerToArViz::setLineSetVisu(const std::string& layerName, const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setLineSetVisu(const std::string& layerName,
+ const std::string& name,
+ const DebugDrawerLineSet& lineSet,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layerName))
@@ -162,7 +193,8 @@ namespace armarx
if (lineSet.useHeatMap)
{
- ARMARX_VERBOSE << "DebugDrawerToArViz::" << __FUNCTION__ << "(): " << "'useHeatMap' not supported.";
+ ARMARX_VERBOSE << "DebugDrawerToArViz::" << __FUNCTION__ << "(): "
+ << "'useHeatMap' not supported.";
}
simox::Color color0 = toSimox(lineSet.colorNoIntensity);
@@ -177,44 +209,80 @@ namespace armarx
std::stringstream ss;
ss << name << "/" << i << "_" << i + 1;
- setLayerElement(layer, viz::Polygon(ss.str()).addPoint(toEigen(p1)).addPoint(toEigen(p2))
- .lineColor(color).lineWidth(lineSet.lineWidth).color(simox::Color::black(0)));
+ setLayerElement(layer,
+ viz::Polygon(ss.str())
+ .addPoint(toEigen(p1))
+ .addPoint(toEigen(p2))
+ .lineColor(color)
+ .lineWidth(lineSet.lineWidth)
+ .color(simox::Color::black(0)));
}
arviz.commit({layer});
}
- void DebugDrawerToArViz::setBoxVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setBoxVisu(const std::string& layer,
+ const std::string& name,
+ const PoseBasePtr& globalPose,
+ const Vector3BasePtr& dimensions,
+ const DrawColor& color,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
- setAndCommit(layer, viz::Box(name).pose(toEigen(globalPose)).size(toEigen(dimensions)).color(toViz(color)));
+ setAndCommit(
+ layer,
+ viz::Box(name).pose(toEigen(globalPose)).size(toEigen(dimensions)).color(toViz(color)));
}
- void DebugDrawerToArViz::setTextVisu(const std::string& layer, const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setTextVisu(const std::string& layer,
+ const std::string& name,
+ const std::string& text,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ Ice::Int size,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
- setAndCommit(layer, viz::Text(name).text(text).position(toEigen(globalPosition))
- .scale(size).color(toViz(color)));
- }
-
- void DebugDrawerToArViz::setSphereVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current&)
+ setAndCommit(layer,
+ viz::Text(name)
+ .text(text)
+ .position(toEigen(globalPosition))
+ .scale(size)
+ .color(toViz(color)));
+ }
+
+ void
+ DebugDrawerToArViz::setSphereVisu(const std::string& layer,
+ const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ Ice::Float radius,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
- setAndCommit(layer, viz::Sphere(name).position(toEigen(globalPosition)).radius(radius).color(toViz(color)));
+ setAndCommit(
+ layer,
+ viz::Sphere(name).position(toEigen(globalPosition)).radius(radius).color(toViz(color)));
}
- void DebugDrawerToArViz::setPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setPointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const DebugDrawerPointCloud& pointCloud,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -230,7 +298,11 @@ namespace armarx
setAndCommit(layer, cloud.pointSizeInPixels(pointCloud.pointSize));
}
- void DebugDrawerToArViz::setColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setColoredPointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const DebugDrawerColoredPointCloud& pointCloud,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -251,7 +323,12 @@ namespace armarx
setAndCommit(layer, cloud.pointSizeInPixels(pointCloud.pointSize));
}
- void DebugDrawerToArViz::set24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current&)
+ void
+ DebugDrawerToArViz::set24BitColoredPointCloudVisu(
+ const std::string& layer,
+ const std::string& name,
+ const DebugDrawer24BitColoredPointCloud& pointCloud,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -272,7 +349,14 @@ namespace armarx
setAndCommit(layer, cloud.pointSizeInPixels(pointCloud.pointSize));
}
- void DebugDrawerToArViz::setPolygonVisu(const std::string& layer, const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setPolygonVisu(const std::string& layer,
+ const std::string& name,
+ const PolygonPointList& polygonPoints,
+ const DrawColor& colorInner,
+ const DrawColor& colorBorder,
+ Ice::Float lineWidth,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -285,10 +369,16 @@ namespace armarx
{
poly.addPoint(toEigen(p));
}
- setAndCommit(layer, poly.color(toViz(colorInner)).lineColor(toViz(colorBorder)).lineWidth(lineWidth));
+ setAndCommit(
+ layer,
+ poly.color(toViz(colorInner)).lineColor(toViz(colorBorder)).lineWidth(lineWidth));
}
- void DebugDrawerToArViz::setTriMeshVisu(const std::string& layer, const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setTriMeshVisu(const std::string& layer,
+ const std::string& name,
+ const DebugDrawerTriMesh& triMesh,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -320,42 +410,86 @@ namespace armarx
setAndCommit(layer, viz::Mesh(name).vertices(vertices).colors(colors).faces(faces));
}
- void DebugDrawerToArViz::setArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current&)
+ void
+ DebugDrawerToArViz::setArrowVisu(const std::string& layer,
+ const std::string& name,
+ const Vector3BasePtr& position,
+ const Vector3BasePtr& direction,
+ const DrawColor& color,
+ Ice::Float length,
+ Ice::Float width,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
- setAndCommit(layer, viz::Arrow(name).position(toEigen(position)).direction(toEigen(direction))
- .color(toViz(color)).width(width).length(length));
- }
-
- void DebugDrawerToArViz::setCylinderVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current&)
+ setAndCommit(layer,
+ viz::Arrow(name)
+ .position(toEigen(position))
+ .direction(toEigen(direction))
+ .color(toViz(color))
+ .width(width)
+ .length(length));
+ }
+
+ void
+ DebugDrawerToArViz::setCylinderVisu(const std::string& layer,
+ const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& direction,
+ Ice::Float length,
+ Ice::Float radius,
+ const DrawColor& color,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
- setAndCommit(layer, viz::Cylinder(name)
- .fromTo(toEigen(globalPosition), toEigen(globalPosition) + length * toEigen(direction))
- .color(toViz(color)).radius(radius));
- }
-
- void DebugDrawerToArViz::setCircleArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current&)
+ setAndCommit(layer,
+ viz::Cylinder(name)
+ .fromTo(toEigen(globalPosition),
+ toEigen(globalPosition) + length * toEigen(direction))
+ .color(toViz(color))
+ .radius(radius));
+ }
+
+ void
+ DebugDrawerToArViz::setCircleArrowVisu(const std::string& layer,
+ const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& directionVec,
+ Ice::Float radius,
+ Ice::Float circleCompletion,
+ Ice::Float width,
+ const DrawColor& color,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
{
return;
}
- setAndCommit(layer, viz::ArrowCircle(name).position(toEigen(globalPosition)).normal(toEigen(directionVec))
- .radius(radius).completion(circleCompletion).color(toViz(color)).width(width));
- }
-
-
- void DebugDrawerToArViz::setRobotVisu(const std::string& layer, const std::string& name, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyleType, const Ice::Current&)
+ setAndCommit(layer,
+ viz::ArrowCircle(name)
+ .position(toEigen(globalPosition))
+ .normal(toEigen(directionVec))
+ .radius(radius)
+ .completion(circleCompletion)
+ .color(toViz(color))
+ .width(width));
+ }
+
+ void
+ DebugDrawerToArViz::setRobotVisu(const std::string& layer,
+ const std::string& name,
+ const std::string& robotFile,
+ const std::string& armarxProject,
+ DrawStyle drawStyleType,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -378,7 +512,11 @@ namespace armarx
setAndCommit(layer, robot);
}
- void DebugDrawerToArViz::updateRobotPose(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current&)
+ void
+ DebugDrawerToArViz::updateRobotPose(const std::string& layer,
+ const std::string& name,
+ const PoseBasePtr& globalPose,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -393,7 +531,11 @@ namespace armarx
arviz.commit({getLayer(layer)});
}
- void DebugDrawerToArViz::updateRobotConfig(const std::string& layer, const std::string& name, const NameValueMap& configuration, const Ice::Current&)
+ void
+ DebugDrawerToArViz::updateRobotConfig(const std::string& layer,
+ const std::string& name,
+ const NameValueMap& configuration,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -408,7 +550,11 @@ namespace armarx
arviz.commit({getLayer(layer)});
}
- void DebugDrawerToArViz::updateRobotColor(const std::string& layer, const std::string& name, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerToArViz::updateRobotColor(const std::string& layer,
+ const std::string& name,
+ const DrawColor& color,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -423,13 +569,21 @@ namespace armarx
arviz.commit({getLayer(layer)});
}
- void DebugDrawerToArViz::updateRobotNodeColor(const std::string& layer, const std::string& name, const std::string& robotNodeName, const DrawColor& color, const Ice::Current&)
+ void
+ DebugDrawerToArViz::updateRobotNodeColor(const std::string& layer,
+ const std::string& name,
+ const std::string& robotNodeName,
+ const DrawColor& color,
+ const Ice::Current&)
{
- (void) layer, (void) name, (void) robotNodeName, (void) color;
+ (void)layer, (void)name, (void)robotNodeName, (void)color;
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- void DebugDrawerToArViz::removeRobotVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ void
+ DebugDrawerToArViz::removeRobotVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isExcluded(layer))
@@ -440,67 +594,154 @@ namespace armarx
removeAndCommit(layer, name);
}
-
- void DebugDrawerToArViz::setPoseDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& c)
+ void
+ DebugDrawerToArViz::setPoseDebugLayerVisu(const std::string& name,
+ const PoseBasePtr& globalPose,
+ const Ice::Current& c)
{
setPoseVisu(DEBUG_LAYER_NAME, name, globalPose, c);
}
- void DebugDrawerToArViz::setScaledPoseDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setScaledPoseDebugLayerVisu(const std::string& name,
+ const PoseBasePtr& globalPose,
+ Ice::Float scale,
+ const Ice::Current& c)
{
setScaledPoseVisu(DEBUG_LAYER_NAME, name, globalPose, scale, c);
}
- void DebugDrawerToArViz::setLineDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setLineDebugLayerVisu(const std::string& name,
+ const Vector3BasePtr& globalPosition1,
+ const Vector3BasePtr& globalPosition2,
+ Ice::Float lineWidth,
+ const DrawColor& color,
+ const Ice::Current& c)
{
setLineVisu(DEBUG_LAYER_NAME, name, globalPosition1, globalPosition2, lineWidth, color, c);
}
- void DebugDrawerToArViz::setLineSetDebugLayerVisu(const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setLineSetDebugLayerVisu(const std::string& name,
+ const DebugDrawerLineSet& lineSet,
+ const Ice::Current& c)
{
setLineSetVisu(DEBUG_LAYER_NAME, name, lineSet, c);
}
- void DebugDrawerToArViz::setBoxDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setBoxDebugLayerVisu(const std::string& name,
+ const PoseBasePtr& globalPose,
+ const Vector3BasePtr& dimensions,
+ const DrawColor& color,
+ const Ice::Current& c)
{
setBoxVisu(DEBUG_LAYER_NAME, name, globalPose, dimensions, color, c);
}
- void DebugDrawerToArViz::setTextDebugLayerVisu(const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setTextDebugLayerVisu(const std::string& name,
+ const std::string& text,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ Ice::Int size,
+ const Ice::Current& c)
{
setTextVisu(DEBUG_LAYER_NAME, name, text, globalPosition, color, size, c);
}
- void DebugDrawerToArViz::setSphereDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setSphereDebugLayerVisu(const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ Ice::Float radius,
+ const Ice::Current& c)
{
setSphereVisu(DEBUG_LAYER_NAME, name, globalPosition, color, radius, c);
}
- void DebugDrawerToArViz::setPointCloudDebugLayerVisu(const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setPointCloudDebugLayerVisu(const std::string& name,
+ const DebugDrawerPointCloud& pointCloud,
+ const Ice::Current& c)
{
setPointCloudVisu(DEBUG_LAYER_NAME, name, pointCloud, c);
}
- void DebugDrawerToArViz::set24BitColoredPointCloudDebugLayerVisu(const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::set24BitColoredPointCloudDebugLayerVisu(
+ const std::string& name,
+ const DebugDrawer24BitColoredPointCloud& pointCloud,
+ const Ice::Current& c)
{
set24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, name, pointCloud, c);
}
- void DebugDrawerToArViz::setPolygonDebugLayerVisu(const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setPolygonDebugLayerVisu(const std::string& name,
+ const PolygonPointList& polygonPoints,
+ const DrawColor& colorInner,
+ const DrawColor& colorBorder,
+ Ice::Float lineWidth,
+ const Ice::Current& c)
{
- setPolygonVisu(DEBUG_LAYER_NAME, name, polygonPoints, colorInner, colorBorder, lineWidth, c);
+ setPolygonVisu(
+ DEBUG_LAYER_NAME, name, polygonPoints, colorInner, colorBorder, lineWidth, c);
}
- void DebugDrawerToArViz::setTriMeshDebugLayerVisu(const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setTriMeshDebugLayerVisu(const std::string& name,
+ const DebugDrawerTriMesh& triMesh,
+ const Ice::Current& c)
{
setTriMeshVisu(DEBUG_LAYER_NAME, name, triMesh, c);
}
- void DebugDrawerToArViz::setArrowDebugLayerVisu(const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setArrowDebugLayerVisu(const std::string& name,
+ const Vector3BasePtr& position,
+ const Vector3BasePtr& direction,
+ const DrawColor& color,
+ Ice::Float length,
+ Ice::Float width,
+ const Ice::Current& c)
{
setArrowVisu(DEBUG_LAYER_NAME, name, position, direction, color, length, width, c);
}
- void DebugDrawerToArViz::setCylinderDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current& c)
+
+ void
+ DebugDrawerToArViz::setCylinderDebugLayerVisu(const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& direction,
+ Ice::Float length,
+ Ice::Float radius,
+ const DrawColor& color,
+ const Ice::Current& c)
{
- setCylinderVisu(DEBUG_LAYER_NAME, name, globalPosition, direction, length, radius, color, c);
+ setCylinderVisu(
+ DEBUG_LAYER_NAME, name, globalPosition, direction, length, radius, color, c);
}
- void DebugDrawerToArViz::setCircleDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::setCircleDebugLayerVisu(const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& directionVec,
+ Ice::Float radius,
+ Ice::Float circleCompletion,
+ Ice::Float width,
+ const DrawColor& color,
+ const Ice::Current&)
{
- (void) name, (void) globalPosition, (void) directionVec, (void) radius, (void) circleCompletion, (void) width, (void) color;
+ (void)name, (void)globalPosition, (void)directionVec, (void)radius, (void)circleCompletion,
+ (void)width, (void)color;
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
-
- void DebugDrawerToArViz::removePoseVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+ void
+ DebugDrawerToArViz::removePoseVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -508,7 +749,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removeLineVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeLineVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -516,7 +761,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removeLineSetVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeLineSetVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -524,7 +773,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removeBoxVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeBoxVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -532,7 +785,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removeTextVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeTextVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -540,7 +797,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removeSphereVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeSphereVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -548,7 +809,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removePointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removePointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -556,7 +821,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removeColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeColoredPointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -564,7 +833,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::remove24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::remove24BitColoredPointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -572,7 +845,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removePolygonVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removePolygonVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -580,7 +857,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removeTriMeshVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeTriMeshVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -588,7 +869,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removeArrowVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeArrowVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -596,7 +881,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removeCylinderVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeCylinderVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -604,7 +893,11 @@ namespace armarx
removeAndCommit(layer, name);
}
}
- void DebugDrawerToArViz::removeCircleVisu(const std::string& layer, const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeCircleVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current&)
{
std::scoped_lock lock(mutex);
if (layerBlackWhitelist.isIncluded(layer))
@@ -613,65 +906,94 @@ namespace armarx
}
}
-
- void DebugDrawerToArViz::removePoseDebugLayerVisu(const std::string& name, const Ice::Current&)
+ void
+ DebugDrawerToArViz::removePoseDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removePointCloudVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removeLineDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeLineDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removeLineVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removeLineSetDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeLineSetDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removeLineSetVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removeBoxDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeBoxDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removeBoxVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removeTextDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeTextDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removeTextVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removeSphereDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeSphereDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removeSphereVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removePointCloudDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removePointCloudDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removePointCloudVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removeColoredPointCloudDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeColoredPointCloudDebugLayerVisu(const std::string& name,
+ const Ice::Current&)
{
removeColoredPointCloudVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::remove24BitColoredPointCloudDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::remove24BitColoredPointCloudDebugLayerVisu(const std::string& name,
+ const Ice::Current&)
{
remove24BitColoredPointCloudVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removePolygonDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removePolygonDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removePolygonVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removeTriMeshDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeTriMeshDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removeTriMeshVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removeArrowDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeArrowDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removeArrowVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removeCylinderDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeCylinderDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removeCylinderVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::removeCircleDebugLayerVisu(const std::string& name, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeCircleDebugLayerVisu(const std::string& name, const Ice::Current&)
{
removeCircleVisu(DEBUG_LAYER_NAME, name);
}
- void DebugDrawerToArViz::clearAll(const Ice::Current&)
+ void
+ DebugDrawerToArViz::clearAll(const Ice::Current&)
{
std::scoped_lock lock(mutex);
@@ -684,7 +1006,9 @@ namespace armarx
}
arviz.commit(commit);
}
- void DebugDrawerToArViz::clearLayer(const std::string& layerName, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::clearLayer(const std::string& layerName, const Ice::Current&)
{
std::scoped_lock lock(mutex);
@@ -692,83 +1016,110 @@ namespace armarx
layer.clear();
arviz.commit({layer});
}
- void DebugDrawerToArViz::clearDebugLayer(const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::clearDebugLayer(const Ice::Current&)
{
clearLayer(DEBUG_LAYER_NAME);
}
- void DebugDrawerToArViz::enableLayerVisu(const std::string& layer, bool visible, const Ice::Current&)
+ void
+ DebugDrawerToArViz::enableLayerVisu(const std::string& layer, bool visible, const Ice::Current&)
{
- (void) layer, (void) visible;
+ (void)layer, (void)visible;
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- void DebugDrawerToArViz::enableDebugLayerVisu(bool visible, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::enableDebugLayerVisu(bool visible, const Ice::Current&)
{
enableLayerVisu(DEBUG_LAYER_NAME, visible);
}
- Ice::StringSeq DebugDrawerToArViz::layerNames(const Ice::Current&)
+ Ice::StringSeq
+ DebugDrawerToArViz::layerNames(const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
return {};
}
- LayerInformationSequence DebugDrawerToArViz::layerInformation(const Ice::Current&)
+
+ LayerInformationSequence
+ DebugDrawerToArViz::layerInformation(const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
return {};
}
- bool DebugDrawerToArViz::hasLayer(const std::string&, const Ice::Current&)
+ bool
+ DebugDrawerToArViz::hasLayer(const std::string&, const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
return false;
}
- void DebugDrawerToArViz::removeLayer(const std::string&, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::removeLayer(const std::string&, const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- void DebugDrawerToArViz::disableAllLayers(const Ice::Current&)
+ void
+ DebugDrawerToArViz::disableAllLayers(const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- void DebugDrawerToArViz::enableAllLayers(const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::enableAllLayers(const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- void DebugDrawerToArViz::enableSelections(const std::string&, const Ice::Current&)
+ void
+ DebugDrawerToArViz::enableSelections(const std::string&, const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- void DebugDrawerToArViz::disableSelections(const std::string&, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::disableSelections(const std::string&, const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- void DebugDrawerToArViz::clearSelections(const std::string&, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::clearSelections(const std::string&, const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- void DebugDrawerToArViz::select(const std::string& layer, const std::string& elementName, const Ice::Current&)
+ void
+ DebugDrawerToArViz::select(const std::string& layer,
+ const std::string& elementName,
+ const Ice::Current&)
{
- (void) layer, (void) elementName;
+ (void)layer, (void)elementName;
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- void DebugDrawerToArViz::deselect(const std::string& layer, const std::string& elementName, const Ice::Current&)
+
+ void
+ DebugDrawerToArViz::deselect(const std::string& layer,
+ const std::string& elementName,
+ const Ice::Current&)
{
- (void) layer, (void) elementName;
+ (void)layer, (void)elementName;
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
}
- DebugDrawerSelectionList DebugDrawerToArViz::getSelections(const Ice::Current&)
+ DebugDrawerSelectionList
+ DebugDrawerToArViz::getSelections(const Ice::Current&)
{
LOG_FUNCTION_NOT_IMPLEMENTED_MESSAGE();
return {};
}
-
- viz::Layer& DebugDrawerToArViz::getLayer(const std::string& layerName)
+ viz::Layer&
+ DebugDrawerToArViz::getLayer(const std::string& layerName)
{
if (auto it = layers.find(layerName); it != layers.end())
{
@@ -780,16 +1131,17 @@ namespace armarx
}
}
- viz::data::ElementSeq::iterator DebugDrawerToArViz::findLayerElement(viz::Layer& layer, const std::string& elementName)
+ viz::data::ElementSeq::iterator
+ DebugDrawerToArViz::findLayerElement(viz::Layer& layer, const std::string& elementName)
{
- return std::find_if(layer.data_.elements.begin(), layer.data_.elements.end(),
- [&elementName](const viz::data::ElementPtr & e)
- {
- return e->id == elementName;
- });
+ return std::find_if(layer.data_.elements.begin(),
+ layer.data_.elements.end(),
+ [&elementName](const viz::data::ElementPtr& e)
+ { return e->id == elementName; });
}
- void DebugDrawerToArViz::removeLayerElement(viz::Layer& layer, const std::string& name)
+ void
+ DebugDrawerToArViz::removeLayerElement(viz::Layer& layer, const std::string& name)
{
auto it = findLayerElement(layer, name);
if (it != layer.data_.elements.end())
@@ -798,11 +1150,12 @@ namespace armarx
}
}
- void DebugDrawerToArViz::removeAndCommit(const std::string& layerName, const std::string& name)
+ void
+ DebugDrawerToArViz::removeAndCommit(const std::string& layerName, const std::string& name)
{
viz::Layer& layer = getLayer(layerName);
removeLayerElement(layer, name);
arviz.commit({layer});
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h
index 38e66882d00f87ee17e55e76699b943be51d66c6..bcce5d788bc96bd0a9a872a097eb07dbd2942b63 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArViz.h
@@ -25,12 +25,11 @@
#include <map>
#include <mutex>
-#include <RobotAPI/interface/visualization/DebugDrawerToArViz.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/interface/visualization/DebugDrawerToArViz.h>
#include "BlackWhitelist.h"
-
namespace armarx
{
@@ -40,94 +39,290 @@ namespace armarx
class DebugDrawerToArViz : virtual public armarx::DebugDrawerToArvizInterface
{
public:
-
void setArViz(viz::Client arviz);
// BlackWhitelistTopic interface
public:
- void updateBlackWhitelist(const BlackWhitelistUpdate& update, const Ice::Current& = Ice::emptyCurrent) override;
+ void updateBlackWhitelist(const BlackWhitelistUpdate& update,
+ const Ice::Current& = Ice::emptyCurrent) override;
// DebugDrawerInterface interface
public:
-
- void exportScene(const std::string& filename, const Ice::Current& = Ice::emptyCurrent) override;
- void exportLayer(const std::string& filename, const std::string& layerName, const Ice::Current& = Ice::emptyCurrent) override;
-
- void setPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& = Ice::emptyCurrent) override;
- void setScaledPoseVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current& = Ice::emptyCurrent) override;
- void setLineVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
- void setLineSetVisu(const std::string& layer, const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current& = Ice::emptyCurrent) override;
- void setBoxVisu(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
- void setTextVisu(const std::string& layer, const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current& = Ice::emptyCurrent) override;
- void setSphereVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current& = Ice::emptyCurrent) override;
- void setPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
- void setColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawerColoredPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
- void set24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
- void setPolygonVisu(const std::string& layer, const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current& = Ice::emptyCurrent) override;
- void setTriMeshVisu(const std::string& layer, const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current& = Ice::emptyCurrent) override;
- void setArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current& = Ice::emptyCurrent) override;
- void setCylinderVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
- void setCircleArrowVisu(const std::string& layer, const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
-
- void setRobotVisu(const std::string& layer, const std::string& name, const std::string& robotFile, const std::string& armarxProject, DrawStyle drawStyleType, const Ice::Current& = Ice::emptyCurrent) override;
- void updateRobotPose(const std::string& layer, const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& = Ice::emptyCurrent) override;
- void updateRobotConfig(const std::string& layer, const std::string& name, const NameValueMap& configuration, const Ice::Current& = Ice::emptyCurrent) override;
- void updateRobotColor(const std::string& layer, const std::string& name, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
- void updateRobotNodeColor(const std::string& layer, const std::string& name, const std::string& robotNodeName, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
- void removeRobotVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
-
- void setPoseDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, const Ice::Current& = Ice::emptyCurrent) override;
- void setScaledPoseDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, Ice::Float scale, const Ice::Current& = Ice::emptyCurrent) override;
- void setLineDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition1, const Vector3BasePtr& globalPosition2, Ice::Float lineWidth, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
- void setLineSetDebugLayerVisu(const std::string& name, const DebugDrawerLineSet& lineSet, const Ice::Current& = Ice::emptyCurrent) override;
- void setBoxDebugLayerVisu(const std::string& name, const PoseBasePtr& globalPose, const Vector3BasePtr& dimensions, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
- void setTextDebugLayerVisu(const std::string& name, const std::string& text, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Int size, const Ice::Current& = Ice::emptyCurrent) override;
- void setSphereDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const DrawColor& color, Ice::Float radius, const Ice::Current& = Ice::emptyCurrent) override;
- void setPointCloudDebugLayerVisu(const std::string& name, const DebugDrawerPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
- void set24BitColoredPointCloudDebugLayerVisu(const std::string& name, const DebugDrawer24BitColoredPointCloud& pointCloud, const Ice::Current& = Ice::emptyCurrent) override;
- void setPolygonDebugLayerVisu(const std::string& name, const PolygonPointList& polygonPoints, const DrawColor& colorInner, const DrawColor& colorBorder, Ice::Float lineWidth, const Ice::Current& = Ice::emptyCurrent) override;
- void setTriMeshDebugLayerVisu(const std::string& name, const DebugDrawerTriMesh& triMesh, const Ice::Current& = Ice::emptyCurrent) override;
- void setArrowDebugLayerVisu(const std::string& name, const Vector3BasePtr& position, const Vector3BasePtr& direction, const DrawColor& color, Ice::Float length, Ice::Float width, const Ice::Current& = Ice::emptyCurrent) override;
- void setCylinderDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& direction, Ice::Float length, Ice::Float radius, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
- void setCircleDebugLayerVisu(const std::string& name, const Vector3BasePtr& globalPosition, const Vector3BasePtr& directionVec, Ice::Float radius, Ice::Float circleCompletion, Ice::Float width, const DrawColor& color, const Ice::Current& = Ice::emptyCurrent) override;
-
- void removePoseVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeLineVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeLineSetVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeBoxVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeTextVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeSphereVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removePointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void remove24BitColoredPointCloudVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removePolygonVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeTriMeshVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeArrowVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeCylinderVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeCircleVisu(const std::string& layer, const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
-
- void removePoseDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeLineDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeLineSetDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeBoxDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeTextDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeSphereDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removePointCloudDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeColoredPointCloudDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void remove24BitColoredPointCloudDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removePolygonDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeTriMeshDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeArrowDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeCylinderDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
- void removeCircleDebugLayerVisu(const std::string& name, const Ice::Current& = Ice::emptyCurrent) override;
+ void exportScene(const std::string& filename,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void exportLayer(const std::string& filename,
+ const std::string& layerName,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void setPoseVisu(const std::string& layer,
+ const std::string& name,
+ const PoseBasePtr& globalPose,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setScaledPoseVisu(const std::string& layer,
+ const std::string& name,
+ const PoseBasePtr& globalPose,
+ Ice::Float scale,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setLineVisu(const std::string& layer,
+ const std::string& name,
+ const Vector3BasePtr& globalPosition1,
+ const Vector3BasePtr& globalPosition2,
+ Ice::Float lineWidth,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setLineSetVisu(const std::string& layer,
+ const std::string& name,
+ const DebugDrawerLineSet& lineSet,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setBoxVisu(const std::string& layer,
+ const std::string& name,
+ const PoseBasePtr& globalPose,
+ const Vector3BasePtr& dimensions,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setTextVisu(const std::string& layer,
+ const std::string& name,
+ const std::string& text,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ Ice::Int size,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setSphereVisu(const std::string& layer,
+ const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ Ice::Float radius,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setPointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const DebugDrawerPointCloud& pointCloud,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setColoredPointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const DebugDrawerColoredPointCloud& pointCloud,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void set24BitColoredPointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const DebugDrawer24BitColoredPointCloud& pointCloud,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setPolygonVisu(const std::string& layer,
+ const std::string& name,
+ const PolygonPointList& polygonPoints,
+ const DrawColor& colorInner,
+ const DrawColor& colorBorder,
+ Ice::Float lineWidth,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setTriMeshVisu(const std::string& layer,
+ const std::string& name,
+ const DebugDrawerTriMesh& triMesh,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setArrowVisu(const std::string& layer,
+ const std::string& name,
+ const Vector3BasePtr& position,
+ const Vector3BasePtr& direction,
+ const DrawColor& color,
+ Ice::Float length,
+ Ice::Float width,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setCylinderVisu(const std::string& layer,
+ const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& direction,
+ Ice::Float length,
+ Ice::Float radius,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setCircleArrowVisu(const std::string& layer,
+ const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& directionVec,
+ Ice::Float radius,
+ Ice::Float circleCompletion,
+ Ice::Float width,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void setRobotVisu(const std::string& layer,
+ const std::string& name,
+ const std::string& robotFile,
+ const std::string& armarxProject,
+ DrawStyle drawStyleType,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotPose(const std::string& layer,
+ const std::string& name,
+ const PoseBasePtr& globalPose,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotConfig(const std::string& layer,
+ const std::string& name,
+ const NameValueMap& configuration,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotColor(const std::string& layer,
+ const std::string& name,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void updateRobotNodeColor(const std::string& layer,
+ const std::string& name,
+ const std::string& robotNodeName,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeRobotVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void setPoseDebugLayerVisu(const std::string& name,
+ const PoseBasePtr& globalPose,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setScaledPoseDebugLayerVisu(const std::string& name,
+ const PoseBasePtr& globalPose,
+ Ice::Float scale,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setLineDebugLayerVisu(const std::string& name,
+ const Vector3BasePtr& globalPosition1,
+ const Vector3BasePtr& globalPosition2,
+ Ice::Float lineWidth,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setLineSetDebugLayerVisu(const std::string& name,
+ const DebugDrawerLineSet& lineSet,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setBoxDebugLayerVisu(const std::string& name,
+ const PoseBasePtr& globalPose,
+ const Vector3BasePtr& dimensions,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setTextDebugLayerVisu(const std::string& name,
+ const std::string& text,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ Ice::Int size,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setSphereDebugLayerVisu(const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const DrawColor& color,
+ Ice::Float radius,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setPointCloudDebugLayerVisu(const std::string& name,
+ const DebugDrawerPointCloud& pointCloud,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void
+ set24BitColoredPointCloudDebugLayerVisu(const std::string& name,
+ const DebugDrawer24BitColoredPointCloud& pointCloud,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setPolygonDebugLayerVisu(const std::string& name,
+ const PolygonPointList& polygonPoints,
+ const DrawColor& colorInner,
+ const DrawColor& colorBorder,
+ Ice::Float lineWidth,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setTriMeshDebugLayerVisu(const std::string& name,
+ const DebugDrawerTriMesh& triMesh,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setArrowDebugLayerVisu(const std::string& name,
+ const Vector3BasePtr& position,
+ const Vector3BasePtr& direction,
+ const DrawColor& color,
+ Ice::Float length,
+ Ice::Float width,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setCylinderDebugLayerVisu(const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& direction,
+ Ice::Float length,
+ Ice::Float radius,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setCircleDebugLayerVisu(const std::string& name,
+ const Vector3BasePtr& globalPosition,
+ const Vector3BasePtr& directionVec,
+ Ice::Float radius,
+ Ice::Float circleCompletion,
+ Ice::Float width,
+ const DrawColor& color,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void removePoseVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineSetVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeBoxVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeTextVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeSphereVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removePointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeColoredPointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void remove24BitColoredPointCloudVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removePolygonVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeTriMeshVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeArrowVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCylinderVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCircleVisu(const std::string& layer,
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void removePoseDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeLineSetDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeBoxDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeTextDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeSphereDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removePointCloudDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void
+ removeColoredPointCloudDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void remove24BitColoredPointCloudDebugLayerVisu(
+ const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removePolygonDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeTriMeshDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeArrowDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCylinderDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void removeCircleDebugLayerVisu(const std::string& name,
+ const Ice::Current& = Ice::emptyCurrent) override;
void clearAll(const Ice::Current& = Ice::emptyCurrent) override;
void clearLayer(const std::string& layer, const Ice::Current& = Ice::emptyCurrent) override;
void clearDebugLayer(const Ice::Current& = Ice::emptyCurrent) override;
- void enableLayerVisu(const std::string& layer, bool visible, const Ice::Current& = Ice::emptyCurrent) override;
+ void enableLayerVisu(const std::string& layer,
+ bool visible,
+ const Ice::Current& = Ice::emptyCurrent) override;
void enableDebugLayerVisu(bool visible, const Ice::Current& = Ice::emptyCurrent) override;
Ice::StringSeq layerNames(const Ice::Current& = Ice::emptyCurrent) override;
@@ -140,24 +335,28 @@ namespace armarx
void enableAllLayers(const Ice::Current& = Ice::emptyCurrent) override;
void enableSelections(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
- void disableSelections(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
+ void disableSelections(const std::string&,
+ const Ice::Current& = Ice::emptyCurrent) override;
void clearSelections(const std::string&, const Ice::Current& = Ice::emptyCurrent) override;
- void select(const std::string& layer, const std::string& elementName, const Ice::Current& = Ice::emptyCurrent) override;
- void deselect(const std::string& layer, const std::string& elementName, const Ice::Current& = Ice::emptyCurrent) override;
+ void select(const std::string& layer,
+ const std::string& elementName,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void deselect(const std::string& layer,
+ const std::string& elementName,
+ const Ice::Current& = Ice::emptyCurrent) override;
DebugDrawerSelectionList getSelections(const Ice::Current& = Ice::emptyCurrent) override;
private:
-
viz::Layer& getLayer(const std::string& layerName);
- viz::data::ElementSeq::iterator
- findLayerElement(viz::Layer& layer, const std::string& elementName);
-
+ viz::data::ElementSeq::iterator findLayerElement(viz::Layer& layer,
+ const std::string& elementName);
template <class Element>
- void setLayerElement(viz::Layer& layer, Element element)
+ void
+ setLayerElement(viz::Layer& layer, Element element)
{
bool found = false;
for (size_t i = 0; i < layer.data_.elements.size(); ++i)
@@ -176,14 +375,17 @@ namespace armarx
layer.add(element);
}
}
+
template <class Element>
- void setLayerElement(const std::string& layerName, Element element)
+ void
+ setLayerElement(const std::string& layerName, Element element)
{
setLayerElement(getLayer(layerName), element);
}
template <class Element>
- void setAndCommit(const std::string& layerName, Element element)
+ void
+ setAndCommit(const std::string& layerName, Element element)
{
viz::Layer& layer = getLayer(layerName);
setLayerElement(layer, element);
@@ -195,12 +397,10 @@ namespace armarx
public:
-
armarx::StringBlackWhitelist layerBlackWhitelist;
private:
-
const std::string DEBUG_LAYER_NAME = "debug";
std::mutex mutex;
@@ -210,6 +410,5 @@ namespace armarx
std::map<std::string, viz::Layer> layers;
std::map<std::pair<std::string, std::string>, viz::Robot> robots;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.cpp b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.cpp
index 9db1d84ddf17abd2aaf65b17b61c317b4a71d749..0098315d29565fc77733188b8a435240141b42b2 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.cpp
@@ -24,67 +24,71 @@
#include "BlackWhitelistUpdate.h"
-
namespace armarx
{
- DebugDrawerToArVizPropertyDefinitions::DebugDrawerToArVizPropertyDefinitions(std::string prefix) :
+ DebugDrawerToArVizPropertyDefinitions::DebugDrawerToArVizPropertyDefinitions(
+ std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates",
+ defineOptionalProperty<std::string>("DebugDrawerTopicName",
+ "DebugDrawerUpdates",
"Name of the topic the DebugDrawer listens to.");
defineOptionalProperty<std::string>(
- "LayerBlackWhitelistTopic", "DebugDrawerToArVizLayerBlackWhitelistUpdates",
+ "LayerBlackWhitelistTopic",
+ "DebugDrawerToArVizLayerBlackWhitelistUpdates",
"The layer where updates to the layer black-whitelist are published.");
- defineOptionalProperty<std::vector<std::string>>("LayerWhitelist", {},
- "If not empty, layers are shown (comma separated list).")
- .map("[empty whitelist]", {});
- defineOptionalProperty<std::vector<std::string>>("LayerBlacklist", {},
- "These layers will never be shown (comma separated list).")
- .map("[empty blacklist]", {});
+ defineOptionalProperty<std::vector<std::string>>(
+ "LayerWhitelist", {}, "If not empty, layers are shown (comma separated list).")
+ .map("[empty whitelist]", {});
+ defineOptionalProperty<std::vector<std::string>>(
+ "LayerBlacklist", {}, "These layers will never be shown (comma separated list).")
+ .map("[empty blacklist]", {});
}
-
- std::string DebugDrawerToArVizComponent::getDefaultName() const
+ std::string
+ DebugDrawerToArVizComponent::getDefaultName() const
{
return "DebugDrawerToArViz";
}
-
- void DebugDrawerToArVizComponent::onInitComponent()
+ void
+ DebugDrawerToArVizComponent::onInitComponent()
{
{
BlackWhitelistUpdate update;
getProperty(update.whitelist.set, "LayerWhitelist");
getProperty(update.blacklist.set, "LayerBlacklist");
armarx::updateBlackWhitelist(DebugDrawerToArViz::layerBlackWhitelist, update);
- ARMARX_VERBOSE << "Layer black-white-list: \n" << DebugDrawerToArViz::layerBlackWhitelist;
+ ARMARX_VERBOSE << "Layer black-white-list: \n"
+ << DebugDrawerToArViz::layerBlackWhitelist;
}
usingTopicFromProperty("DebugDrawerTopicName");
usingTopicFromProperty("LayerBlackWhitelistTopic");
}
-
- void DebugDrawerToArVizComponent::onConnectComponent()
+ void
+ DebugDrawerToArVizComponent::onConnectComponent()
{
DebugDrawerToArViz::setArViz(ArVizComponentPluginUser::arviz);
}
-
- void DebugDrawerToArVizComponent::onDisconnectComponent()
+ void
+ DebugDrawerToArVizComponent::onDisconnectComponent()
{
}
-
- void DebugDrawerToArVizComponent::onExitComponent()
+ void
+ DebugDrawerToArVizComponent::onExitComponent()
{
}
-
- armarx::PropertyDefinitionsPtr DebugDrawerToArVizComponent::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ DebugDrawerToArVizComponent::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new DebugDrawerToArVizPropertyDefinitions(getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new DebugDrawerToArVizPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h
index 957237826c330671688be6c1c88427c04d0b9a1e..487bf3b488e2b5c980fff71e1a335b6e516a0f5b 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h
+++ b/source/RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h
@@ -26,26 +26,22 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/visualization/DebugDrawerToArViz.h>
-
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include "DebugDrawerToArViz.h"
-
namespace armarx
{
/**
* @class DebugDrawerToArVizPropertyDefinitions
* @brief Property definitions of `DebugDrawerToArViz`.
*/
- class DebugDrawerToArVizPropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
+ class DebugDrawerToArVizPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
DebugDrawerToArVizPropertyDefinitions(std::string prefix);
};
-
/**
* @defgroup Component-DebugDrawerToArViz DebugDrawerToArViz
* @ingroup RobotAPI-Components
@@ -59,17 +55,15 @@ namespace armarx
*/
class DebugDrawerToArVizComponent :
virtual public armarx::Component,
- virtual public armarx::DebugDrawerToArViz, // Implements armarx::DebugDrawerToArvizInterface
+ virtual public armarx::DebugDrawerToArViz, // Implements armarx::DebugDrawerToArvizInterface
virtual public armarx::ArVizComponentPluginUser
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
protected:
-
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -84,6 +78,5 @@ namespace armarx
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/main.cpp b/source/RobotAPI/components/DebugDrawerToArViz/main.cpp
index a498c8dc5741c89e7812cdefc0c77e8b5b03446d..a9c0856f93cf2ebeed980044696de7cd0430b4de 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/main.cpp
+++ b/source/RobotAPI/components/DebugDrawerToArViz/main.cpp
@@ -20,13 +20,15 @@
* GNU General Public License
*/
-#include <RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::DebugDrawerToArVizComponent > (argc, argv, "DebugDrawerToArViz");
+ return armarx::runSimpleComponentApp<armarx::DebugDrawerToArVizComponent>(
+ argc, argv, "DebugDrawerToArViz");
}
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/test/DebugDrawerToArVizTest.cpp b/source/RobotAPI/components/DebugDrawerToArViz/test/DebugDrawerToArVizTest.cpp
index 42047e6b01ac9b12ff0e96ecf5eef9f1be0cdef8..b74d8c4e522c90333500b8a9645512e3786b7173 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/test/DebugDrawerToArVizTest.cpp
+++ b/source/RobotAPI/components/DebugDrawerToArViz/test/DebugDrawerToArVizTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h>
#include <iostream>
+#include <RobotAPI/components/DebugDrawerToArViz/DebugDrawerToArVizComponent.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::DebugDrawerToArVizComponent instance;
diff --git a/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp b/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp
index 30cbeea21fdaa7c71bb349f2eb266bbf31dd6556..59e9bd3e6d031c9f6c0b59dd0b4e2d84ea4dfa6d 100644
--- a/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp
+++ b/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.cpp
@@ -26,40 +26,40 @@
namespace armarx
{
- void DummyTextToSpeech::onInitComponent()
+ void
+ DummyTextToSpeech::onInitComponent()
{
usingTopic(getProperty<std::string>("TextToSpeechTopicName").getValue());
offeringTopic(getProperty<std::string>("TextToSpeechStateTopicName").getValue());
}
-
- void DummyTextToSpeech::onConnectComponent()
+ void
+ DummyTextToSpeech::onConnectComponent()
{
- textToSpeechStateTopicPrx = getTopic<TextToSpeechStateInterfacePrx>(getProperty<std::string>("TextToSpeechStateTopicName").getValue());
+ textToSpeechStateTopicPrx = getTopic<TextToSpeechStateInterfacePrx>(
+ getProperty<std::string>("TextToSpeechStateTopicName").getValue());
textToSpeechStateTopicPrx->reportState(eIdle);
}
-
- void DummyTextToSpeech::onDisconnectComponent()
+ void
+ DummyTextToSpeech::onDisconnectComponent()
{
-
}
-
- void DummyTextToSpeech::onExitComponent()
+ void
+ DummyTextToSpeech::onExitComponent()
{
-
}
- armarx::PropertyDefinitionsPtr DummyTextToSpeech::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ DummyTextToSpeech::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new DummyTextToSpeechPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new DummyTextToSpeechPropertyDefinitions(getConfigIdentifier()));
}
-
-
- void armarx::DummyTextToSpeech::reportText(const std::string& text, const Ice::Current&)
+ void
+ armarx::DummyTextToSpeech::reportText(const std::string& text, const Ice::Current&)
{
//ARMARX_IMPORTANT << "reportState";
textToSpeechStateTopicPrx->reportState(eStartedSpeaking);
@@ -74,8 +74,11 @@ namespace armarx
textToSpeechStateTopicPrx->reportState(eFinishedSpeaking);
}
- void armarx::DummyTextToSpeech::reportTextWithParams(const std::string& text, const Ice::StringSeq& params, const Ice::Current&)
+ void
+ armarx::DummyTextToSpeech::reportTextWithParams(const std::string& text,
+ const Ice::StringSeq& params,
+ const Ice::Current&)
{
ARMARX_WARNING << "reportTextWithParams is not implemented";
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.h b/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.h
index 20c7f5e5e994e9c937dfb85103c8fcfec4dfdb62..04945f49c9a41957c57c6483ecaf5ee352ebce3f 100644
--- a/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.h
+++ b/source/RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.h
@@ -33,16 +33,18 @@ namespace armarx
* @class DummyTextToSpeechPropertyDefinitions
* @brief
*/
- class DummyTextToSpeechPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class DummyTextToSpeechPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- DummyTextToSpeechPropertyDefinitions(std::string prefix):
+ DummyTextToSpeechPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
- defineOptionalProperty<std::string>("TextToSpeechTopicName", "TextToSpeech", "Name of the TextToSpeechTopic");
- defineOptionalProperty<std::string>("TextToSpeechStateTopicName", "TextToSpeechState", "Name of the TextToSpeechStateTopic");
+ defineOptionalProperty<std::string>(
+ "TextToSpeechTopicName", "TextToSpeech", "Name of the TextToSpeechTopic");
+ defineOptionalProperty<std::string>("TextToSpeechStateTopicName",
+ "TextToSpeechState",
+ "Name of the TextToSpeechStateTopic");
}
};
@@ -57,15 +59,14 @@ namespace armarx
*
* Detailed description of class DummyTextToSpeech.
*/
- class DummyTextToSpeech :
- virtual public armarx::Component,
- virtual public TextListenerInterface
+ class DummyTextToSpeech : virtual public armarx::Component, virtual public TextListenerInterface
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "DummyTextToSpeech";
}
@@ -99,9 +100,11 @@ namespace armarx
// TextListenerInterface interface
public:
void reportText(const std::string& text, const Ice::Current&) override;
- void reportTextWithParams(const std::string& text, const Ice::StringSeq& params, const Ice::Current&) override;
+ void reportTextWithParams(const std::string& text,
+ const Ice::StringSeq& params,
+ const Ice::Current&) override;
private:
TextToSpeechStateInterfacePrx textToSpeechStateTopicPrx;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DummyTextToSpeech/test/DummyTextToSpeechTest.cpp b/source/RobotAPI/components/DummyTextToSpeech/test/DummyTextToSpeechTest.cpp
index e5f4d8f434570fcc61c9f7dc2e660418a460f9aa..fd8b38255f7c18ea553fc63557fca861ee18104f 100644
--- a/source/RobotAPI/components/DummyTextToSpeech/test/DummyTextToSpeechTest.cpp
+++ b/source/RobotAPI/components/DummyTextToSpeech/test/DummyTextToSpeechTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.h>
#include <iostream>
+#include <RobotAPI/components/DummyTextToSpeech/DummyTextToSpeech.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::DummyTextToSpeech instance;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index 5d077bca27a123793f5bd34023d9a6878dab9ae6..509b17d687d1abd3d5b3fd02d99266b00d39f2d8 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -25,10 +25,10 @@
#include <VirtualRobot/MathTools.h>
// STD/STL
-#include <string>
-#include <vector>
#include <cmath>
#include <limits>
+#include <string>
+#include <vector>
// Ice
#include <Ice/Current.h>
@@ -43,8 +43,9 @@ using namespace Eigen;
#include <VirtualRobot/XML/RobotIO.h>
// ArmarX
-#include <ArmarXCore/util/time.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/util/time.h>
+
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -52,8 +53,7 @@ using namespace Eigen;
using namespace armarx;
-const std::string
-armarx::DynamicObstacleManager::default_name = "DynamicObstacleManager";
+const std::string armarx::DynamicObstacleManager::default_name = "DynamicObstacleManager";
namespace armarx
{
@@ -76,49 +76,55 @@ namespace armarx
m_only_visualize(false),
m_allow_spwan_inside(false)
{
-
}
-
- std::string DynamicObstacleManager::getDefaultName() const
+ std::string
+ DynamicObstacleManager::getDefaultName() const
{
return DynamicObstacleManager::default_name;
}
-
- void DynamicObstacleManager::onInitComponent()
+ void
+ DynamicObstacleManager::onInitComponent()
{
-
}
-
- void DynamicObstacleManager::onConnectComponent()
+ void
+ DynamicObstacleManager::onConnectComponent()
{
m_decay_factor = m_periodic_task_interval;
m_access_bonus = m_periodic_task_interval;
- m_update_obstacles = new PeriodicTask<DynamicObstacleManager>(this,
- &DynamicObstacleManager::update_decayable_obstacles, m_periodic_task_interval);
+ m_update_obstacles = new PeriodicTask<DynamicObstacleManager>(
+ this, &DynamicObstacleManager::update_decayable_obstacles, m_periodic_task_interval);
m_update_obstacles->start();
}
- void DynamicObstacleManager::onDisconnectComponent()
+ void
+ DynamicObstacleManager::onDisconnectComponent()
{
m_update_obstacles->stop();
}
- void DynamicObstacleManager::onExitComponent()
+ void
+ DynamicObstacleManager::onExitComponent()
{
-
}
- void DynamicObstacleManager::add_decayable_obstacle(const Eigen::Vector2f& e_origin, float e_rx, float e_ry, float e_yaw, const Ice::Current&)
+ void
+ DynamicObstacleManager::add_decayable_obstacle(const Eigen::Vector2f& e_origin,
+ float e_rx,
+ float e_ry,
+ float e_yaw,
+ const Ice::Current&)
{
// TODO
}
-
- void DynamicObstacleManager::add_decayable_line_segment(const Eigen::Vector2f& line_start, const Eigen::Vector2f& line_end, const Ice::Current&)
+ void
+ DynamicObstacleManager::add_decayable_line_segment(const Eigen::Vector2f& line_start,
+ const Eigen::Vector2f& line_end,
+ const Ice::Current&)
{
TIMING_START(add_decayable_line_segment);
const Eigen::Vector2f difference_vec = line_end - line_start;
@@ -149,8 +155,12 @@ namespace armarx
std::lock_guard<std::shared_mutex> l(managed_obstacle->m_mutex);
float coverage = ManagedObstacle::line_segment_ellipsis_coverage(
- {managed_obstacle->m_obstacle.posX, managed_obstacle->m_obstacle.posY}, managed_obstacle->m_obstacle.axisLengthX, managed_obstacle->m_obstacle.axisLengthY, managed_obstacle->m_obstacle.yaw,
- line_start, line_end);
+ {managed_obstacle->m_obstacle.posX, managed_obstacle->m_obstacle.posY},
+ managed_obstacle->m_obstacle.axisLengthX,
+ managed_obstacle->m_obstacle.axisLengthY,
+ managed_obstacle->m_obstacle.yaw,
+ line_start,
+ line_end);
//ARMARX_DEBUG << "The coverage of the new line to obstacle " << managed_obstacle.obstacle.name << "(" << ManagedObstacle::calculateObstacleArea(managed_obstacle.obstacle) << ") is " << coverage;
if (coverage >= m_min_coverage_of_line_samples_in_obstacle)
{
@@ -181,13 +191,17 @@ namespace armarx
ARMARX_DEBUG << " No matching obstacle found. Create new obstacle and add to list";
ManagedObstaclePtr new_managed_obstacle(new ManagedObstacle());
- new_managed_obstacle->m_obstacle.name = "managed_obstacle_" + std::to_string(m_obstacle_index++);
+ new_managed_obstacle->m_obstacle.name =
+ "managed_obstacle_" + std::to_string(m_obstacle_index++);
new_managed_obstacle->m_obstacle.posX = center_vec(0);
new_managed_obstacle->m_obstacle.posY = center_vec(1);
new_managed_obstacle->m_obstacle.refPosX = center_vec(0);
new_managed_obstacle->m_obstacle.refPosY = center_vec(1);
new_managed_obstacle->m_obstacle.yaw = yaw;
- new_managed_obstacle->m_obstacle.axisLengthX = std::clamp(length * 0.5f, static_cast<float>(m_min_length_of_lines), static_cast<float>(m_max_length_of_lines));
+ new_managed_obstacle->m_obstacle.axisLengthX =
+ std::clamp(length * 0.5f,
+ static_cast<float>(m_min_length_of_lines),
+ static_cast<float>(m_max_length_of_lines));
new_managed_obstacle->m_obstacle.axisLengthY = m_thickness_of_lines;
new_managed_obstacle->m_obstacle.safetyMarginX = m_security_margin_for_lines;
new_managed_obstacle->m_obstacle.safetyMarginY = m_security_margin_for_lines;
@@ -206,7 +220,10 @@ namespace armarx
TIMING_END(add_decayable_line_segment);
}
- void DynamicObstacleManager::add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& c)
+ void
+ DynamicObstacleManager::add_decayable_line_segments(
+ const dynamicobstaclemanager::LineSegments& lines,
+ const Ice::Current& c)
{
for (const auto& line : lines)
{
@@ -214,8 +231,8 @@ namespace armarx
}
}
-
- void DynamicObstacleManager::remove_all_decayable_obstacles(const Ice::Current&)
+ void
+ DynamicObstacleManager::remove_all_decayable_obstacles(const Ice::Current&)
{
std::lock_guard l(m_managed_obstacles_mutex);
@@ -227,11 +244,13 @@ namespace armarx
}
}
- void DynamicObstacleManager::remove_obstacle(const std::string& name, const Ice::Current&)
+ void
+ DynamicObstacleManager::remove_obstacle(const std::string& name, const Ice::Current&)
{
std::lock_guard l(m_managed_obstacles_mutex);
- ARMARX_DEBUG << "Remove Obstacle " << name << " from obstacle map and from obstacledetection";
+ ARMARX_DEBUG << "Remove Obstacle " << name
+ << " from obstacle map and from obstacledetection";
for (ManagedObstaclePtr managed_obstacle : m_managed_obstacles)
{
if (managed_obstacle->m_obstacle.name == name)
@@ -246,21 +265,25 @@ namespace armarx
m_obstacle_detection->updateEnvironment();
}
- void DynamicObstacleManager::wait_unitl_obstacles_are_ready(const Ice::Current&)
+ void
+ DynamicObstacleManager::wait_unitl_obstacles_are_ready(const Ice::Current&)
{
ARMARX_INFO << "Waiting for obstacles to get ready";
usleep(2.0 * m_min_value_for_accepting);
ARMARX_INFO << "Finished waiting for obstacles";
}
-
float
- DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current&)
+ DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& agentPosition,
+ float safetyRadius,
+ const Eigen::Vector2f& goal,
+ const Ice::Current&)
{
std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
const Eigen::Vector2f diff = goal - agentPosition;
- const Eigen::Vector2f orthogonal_normalized = Eigen::Vector2f(Eigen::Rotation2Df(M_PI_2) * diff).normalized();
+ const Eigen::Vector2f orthogonal_normalized =
+ Eigen::Vector2f(Eigen::Rotation2Df(M_PI_2) * diff).normalized();
const float sample_step = 5; // in [mm], sample step size towards goal.
const float distance_to_goal = diff.norm() + safetyRadius;
@@ -271,24 +294,37 @@ namespace armarx
{
for (const auto& man_obstacle : m_managed_obstacles)
{
- Eigen::Vector2f sample = agentPosition + ((goal - agentPosition).normalized() * current_distance);
+ Eigen::Vector2f sample =
+ agentPosition + ((goal - agentPosition).normalized() * current_distance);
Eigen::Vector2f sample_left = sample + (orthogonal_normalized * 250);
Eigen::Vector2f sample_right = sample - (orthogonal_normalized * 250);
obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
- if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample))
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin,
+ obstacle.axisLengthX,
+ obstacle.axisLengthY,
+ obstacle.yaw,
+ sample))
{
return current_distance - safetyRadius;
}
- if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample_left))
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin,
+ obstacle.axisLengthX,
+ obstacle.axisLengthY,
+ obstacle.yaw,
+ sample_left))
{
return current_distance - safetyRadius;
}
- if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample_right))
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin,
+ obstacle.axisLengthX,
+ obstacle.axisLengthY,
+ obstacle.yaw,
+ sample_right))
{
return current_distance - safetyRadius;
}
@@ -300,8 +336,13 @@ namespace armarx
return std::numeric_limits<float>::infinity();
}
-
- void DynamicObstacleManager::directly_update_obstacle(const std::string& name, const Eigen::Vector2f& obs_pos, float e_rx, float e_ry, float e_yaw, const Ice::Current&)
+ void
+ DynamicObstacleManager::directly_update_obstacle(const std::string& name,
+ const Eigen::Vector2f& obs_pos,
+ float e_rx,
+ float e_ry,
+ float e_yaw,
+ const Ice::Current&)
{
obstacledetection::Obstacle new_unmanaged_obstacle;
new_unmanaged_obstacle.name = name;
@@ -310,16 +351,22 @@ namespace armarx
new_unmanaged_obstacle.refPosX = obs_pos(0);
new_unmanaged_obstacle.refPosY = obs_pos(1);
new_unmanaged_obstacle.yaw = e_yaw;
- new_unmanaged_obstacle.axisLengthX = std::clamp(e_rx, static_cast<float>(m_min_size_of_obstacles), static_cast<float>(m_max_size_of_obstacles));
- new_unmanaged_obstacle.axisLengthY = std::clamp(e_ry, static_cast<float>(m_min_size_of_obstacles), static_cast<float>(m_max_size_of_obstacles));
+ new_unmanaged_obstacle.axisLengthX =
+ std::clamp(e_rx,
+ static_cast<float>(m_min_size_of_obstacles),
+ static_cast<float>(m_max_size_of_obstacles));
+ new_unmanaged_obstacle.axisLengthY =
+ std::clamp(e_ry,
+ static_cast<float>(m_min_size_of_obstacles),
+ static_cast<float>(m_max_size_of_obstacles));
new_unmanaged_obstacle.safetyMarginX = m_security_margin_for_obstacles;
new_unmanaged_obstacle.safetyMarginY = m_security_margin_for_obstacles;
m_obstacle_detection->updateObstacle(new_unmanaged_obstacle);
m_obstacle_detection->updateEnvironment();
}
-
- void DynamicObstacleManager::update_decayable_obstacles()
+ void
+ DynamicObstacleManager::update_decayable_obstacles()
{
ARMARX_DEBUG << "update obstacles";
IceUtil::Time started = IceUtil::Time::now();
@@ -352,7 +399,9 @@ namespace armarx
{
ARMARX_DEBUG << "update obstacle values and publish";
std::lock_guard l(m_managed_obstacles_mutex);
- std::sort(m_managed_obstacles.begin(), m_managed_obstacles.end(), ManagedObstacle::ComparatorDESCPrt);
+ std::sort(m_managed_obstacles.begin(),
+ m_managed_obstacles.end(),
+ ManagedObstacle::ComparatorDESCPrt);
checked_obstacles = m_managed_obstacles.size();
@@ -361,11 +410,13 @@ namespace armarx
std::lock_guard<std::shared_mutex> l(managed_obstacle->m_mutex);
if (!managed_obstacle->m_updated)
{
- managed_obstacle->m_value = std::max(-1.0f, managed_obstacle->m_value - m_decay_factor);
+ managed_obstacle->m_value =
+ std::max(-1.0f, managed_obstacle->m_value - m_decay_factor);
}
else
{
- managed_obstacle->m_value = std::min(1.0f * m_decay_after_ms, managed_obstacle->m_value + m_access_bonus);
+ managed_obstacle->m_value = std::min(
+ 1.0f * m_decay_after_ms, managed_obstacle->m_value + m_access_bonus);
}
if (managed_obstacle->m_published)
@@ -413,17 +464,38 @@ namespace armarx
}
//visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{0, 1, 0, std::min(0.9f, managed_obstacle->m_value / m_decay_after_ms)}, 50, false);
}
- else if (managed_obstacle->m_value < 0 && m_remove_enabled) // Completely forget the obstacle
+ else if (managed_obstacle->m_value < 0 &&
+ m_remove_enabled) // Completely forget the obstacle
{
remove_obstacles.push_back(managed_obstacle->m_obstacle.name);
}
else if (managed_obstacle->m_updated)
{
- visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{1, 1, 1, std::min(0.9f, managed_obstacle->m_value / m_min_value_for_accepting)}, 40, true);
+ visualize_obstacle(
+ obstacleLayer,
+ managed_obstacle,
+ armarx::DrawColor{
+ 1,
+ 1,
+ 1,
+ std::min(0.9f,
+ managed_obstacle->m_value / m_min_value_for_accepting)},
+ 40,
+ true);
}
else
{
- visualize_obstacle(obstacleLayer, managed_obstacle, armarx::DrawColor{1, 1, 1, std::min(0.9f, managed_obstacle->m_value / m_min_value_for_accepting)}, 40, true);
+ visualize_obstacle(
+ obstacleLayer,
+ managed_obstacle,
+ armarx::DrawColor{
+ 1,
+ 1,
+ 1,
+ std::min(0.9f,
+ managed_obstacle->m_value / m_min_value_for_accepting)},
+ 40,
+ true);
}
}
managed_obstacle->m_updated = false;
@@ -443,24 +515,27 @@ namespace armarx
for (const auto& name : remove_obstacles)
{
// TODO schöner machen ohne loop. Iteratoren in main loop
- m_managed_obstacles.erase(
- std::remove_if(
- m_managed_obstacles.begin(),
- m_managed_obstacles.end(),
- [&](const ManagedObstaclePtr & oc)
- {
- return oc->m_obstacle.name == name;
- }
- ),
- m_managed_obstacles.end()
- );
+ m_managed_obstacles.erase(std::remove_if(m_managed_obstacles.begin(),
+ m_managed_obstacles.end(),
+ [&](const ManagedObstaclePtr& oc)
+ { return oc->m_obstacle.name == name; }),
+ m_managed_obstacles.end());
}
}
- ARMARX_DEBUG << "Finished updating the " << checked_obstacles << " managed obstacles (removed: " << remove_obstacles.size() << ", updated: " << updated_obstacles << "). " << published_obstacles << " of them should be published. The whole operation took " << (IceUtil::Time::now() - started).toMilliSeconds() << "ms";
+ ARMARX_DEBUG << "Finished updating the " << checked_obstacles
+ << " managed obstacles (removed: " << remove_obstacles.size()
+ << ", updated: " << updated_obstacles << "). " << published_obstacles
+ << " of them should be published. The whole operation took "
+ << (IceUtil::Time::now() - started).toMilliSeconds() << "ms";
}
- void DynamicObstacleManager::visualize_obstacle(viz::Layer& layer, const ManagedObstaclePtr& o, const armarx::DrawColor& color, double pos_z, bool text)
+ void
+ DynamicObstacleManager::visualize_obstacle(viz::Layer& layer,
+ const ManagedObstaclePtr& o,
+ const armarx::DrawColor& color,
+ double pos_z,
+ bool text)
{
// Visualize ellipses.m_obstacle_manager_layer_name
Eigen::Vector3f dim(o->m_obstacle.axisLengthX * 2,
@@ -469,35 +544,57 @@ namespace armarx
const std::string name = o->m_obstacle.name;
- layer.add(viz::Box(name).position(Eigen::Vector3f(o->m_obstacle.posX, o->m_obstacle.posY, pos_z))
- .orientation(Eigen::AngleAxisf(o->m_obstacle.yaw, Eigen::Vector3f::UnitZ()).toRotationMatrix())
- .size(dim).color(color.r, color.g, color.b, color.a));
-
+ layer.add(viz::Box(name)
+ .position(Eigen::Vector3f(o->m_obstacle.posX, o->m_obstacle.posY, pos_z))
+ .orientation(Eigen::AngleAxisf(o->m_obstacle.yaw, Eigen::Vector3f::UnitZ())
+ .toRotationMatrix())
+ .size(dim)
+ .color(color.r, color.g, color.b, color.a));
}
-
- armarx::PropertyDefinitionsPtr DynamicObstacleManager::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ DynamicObstacleManager::createPropertyDefinitions()
{
PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
- defs->component(m_obstacle_detection, "PlatformObstacleAvoidance", "ObstacleAvoidanceName", "The name of the used obstacle avoidance interface");
-
- defs->optional(m_min_coverage_of_obstacles, "MinSampleRatioPerEllipsis", "Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle");
- defs->optional(m_min_coverage_of_line_samples_in_obstacle, "MinSampleRatioPerLineSegment", "Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle");
- defs->optional(m_min_size_of_obstacles, "MinObstacleSize", "The minimal obstacle size in mm.");
- defs->optional(m_max_size_of_obstacles, "MaxObstacleSize", "The maximal obstacle size in mm.");
+ defs->component(m_obstacle_detection,
+ "PlatformObstacleAvoidance",
+ "ObstacleAvoidanceName",
+ "The name of the used obstacle avoidance interface");
+
+ defs->optional(m_min_coverage_of_obstacles,
+ "MinSampleRatioPerEllipsis",
+ "Minimum percentage of samples which have to be in an elllipsis to be "
+ "considered as known obsacle");
+ defs->optional(m_min_coverage_of_line_samples_in_obstacle,
+ "MinSampleRatioPerLineSegment",
+ "Minimum percentage of samples which have to be in an elllipsis to be "
+ "considered as known obsacle");
+ defs->optional(
+ m_min_size_of_obstacles, "MinObstacleSize", "The minimal obstacle size in mm.");
+ defs->optional(
+ m_max_size_of_obstacles, "MaxObstacleSize", "The maximal obstacle size in mm.");
defs->optional(m_min_length_of_lines, "MinLengthOfLines", "Minimum length of lines in mm");
defs->optional(m_max_length_of_lines, "MaxLengthOfLines", "Maximum length of lines in mm");
defs->optional(m_decay_after_ms, "MaxObstacleValue", "Maximum value for the obstacles");
- defs->optional(m_min_value_for_accepting, "MinObstacleValueForAccepting", "Minimum value for the obstacles to get accepted");
- defs->optional(m_periodic_task_interval, "UpdateInterval", "The interval to check the obstacles");
+ defs->optional(m_min_value_for_accepting,
+ "MinObstacleValueForAccepting",
+ "Minimum value for the obstacles to get accepted");
+ defs->optional(
+ m_periodic_task_interval, "UpdateInterval", "The interval to check the obstacles");
defs->optional(m_thickness_of_lines, "LineThickness", "The thickness of line obstacles");
- defs->optional(m_security_margin_for_obstacles, "ObstacleSecurityMargin", "Security margin of ellipsis obstacles");
- defs->optional(m_security_margin_for_lines, "LineSecurityMargin", "Security margin of line obstacles");
+ defs->optional(m_security_margin_for_obstacles,
+ "ObstacleSecurityMargin",
+ "Security margin of ellipsis obstacles");
+ defs->optional(
+ m_security_margin_for_lines, "LineSecurityMargin", "Security margin of line obstacles");
defs->optional(m_remove_enabled, "EnableRemove", "Delete Obstacles when value < 0");
- defs->optional(m_only_visualize, "OnlyVisualizeObstacles", "Connection to obstacle avoidance");
- defs->optional(m_allow_spwan_inside, "AllowInRobotSpawning", "Allow obstacles to spawn inside the robot");
+ defs->optional(
+ m_only_visualize, "OnlyVisualizeObstacles", "Connection to obstacle avoidance");
+ defs->optional(m_allow_spwan_inside,
+ "AllowInRobotSpawning",
+ "Allow obstacles to spawn inside the robot");
return defs;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
index cdabab3966b27c232b14c6e4f31a2070eba12e8f..bdf80bb1ea192762b432e77077bedc7182b58aad 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -26,10 +26,10 @@
// STD/STL
#include <array>
+#include <shared_mutex>
#include <string>
#include <tuple>
#include <vector>
-#include <shared_mutex>
// Eigen
#include <Eigen/Geometry>
@@ -38,8 +38,8 @@
#include <Ice/Current.h>
// ArmarX
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
// Managed Objects
@@ -61,19 +61,34 @@ namespace armarx
virtual public ArVizComponentPluginUser
{
public:
-
DynamicObstacleManager() noexcept;
std::string getDefaultName() const override;
- void add_decayable_obstacle(const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
- void add_decayable_line_segment(const Eigen::Vector2f&, const Eigen::Vector2f&, const Ice::Current& = Ice::Current()) override;
- void add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& = Ice::Current()) override;
+ void add_decayable_obstacle(const Eigen::Vector2f&,
+ float,
+ float,
+ float,
+ const Ice::Current& = Ice::Current()) override;
+ void add_decayable_line_segment(const Eigen::Vector2f&,
+ const Eigen::Vector2f&,
+ const Ice::Current& = Ice::Current()) override;
+ void add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines,
+ const Ice::Current& = Ice::Current()) override;
void remove_all_decayable_obstacles(const Ice::Current& = Ice::Current()) override;
- void directly_update_obstacle(const std::string& name, const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
- void remove_obstacle(const std::string& name, const Ice::Current& = Ice::Current()) override;
+ void directly_update_obstacle(const std::string& name,
+ const Eigen::Vector2f&,
+ float,
+ float,
+ float,
+ const Ice::Current& = Ice::Current()) override;
+ void remove_obstacle(const std::string& name,
+ const Ice::Current& = Ice::Current()) override;
void wait_unitl_obstacles_are_ready(const Ice::Current& = Ice::Current()) override;
- float distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current& = Ice::Current()) override;
+ float distanceToObstacle(const Eigen::Vector2f& agentPosition,
+ float safetyRadius,
+ const Eigen::Vector2f& goal,
+ const Ice::Current& = Ice::Current()) override;
protected:
void onInitComponent() override;
@@ -85,7 +100,11 @@ namespace armarx
private:
void update_decayable_obstacles();
- void visualize_obstacle(viz::Layer& l, const ManagedObstaclePtr& o, const armarx::DrawColor& color, double z_pos, bool);
+ void visualize_obstacle(viz::Layer& l,
+ const ManagedObstaclePtr& o,
+ const armarx::DrawColor& color,
+ double z_pos,
+ bool);
public:
static const std::string default_name;
@@ -120,6 +139,5 @@ namespace armarx
bool m_allow_spwan_inside;
ObstacleDetectionInterface::ProxyType m_obstacle_detection;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
index 91af23ed518e9348e3d37cd71ddcdcc2b8646ff8..9ca85da684c9315bbd8ff685ef7b6f57bb1b2fa8 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -37,12 +37,12 @@ using namespace Ice;
#include <Eigen/Core>
// ArmarX
-#include <RobotAPI/interface/core/PoseBase.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <opencv2/core/core_c.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <opencv2/core/core_c.h>
+#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/Pose.h>
using namespace Eigen;
using namespace armarx;
@@ -50,12 +50,18 @@ using namespace armarx;
namespace armarx
{
- double ManagedObstacle::calculateObstacleArea(const obstacledetection::Obstacle& o)
+ double
+ ManagedObstacle::calculateObstacleArea(const obstacledetection::Obstacle& o)
{
return M_PI * std::abs(o.axisLengthX) * std::abs(o.axisLengthY);
}
- bool ManagedObstacle::point_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f point)
+ bool
+ ManagedObstacle::point_ellipsis_coverage(const Eigen::Vector2f e_origin,
+ const float e_rx,
+ const float e_ry,
+ const float e_yaw,
+ const Eigen::Vector2f point)
{
// See https://stackoverflow.com/questions/7946187/point-and-ellipse-rotated-position-test-algorithm
const float sin = std::sin(e_yaw);
@@ -70,7 +76,15 @@ namespace armarx
return a * a * e_ry_sq + b * b * e_rx_sq <= e_rx_sq * e_ry_sq;
}
- float ManagedObstacle::ellipsis_ellipsis_coverage(const Eigen::Vector2f e1_origin, const float e1_rx, const float e1_ry, const float e1_yaw, const Eigen::Vector2f e2_origin, const float e2_rx, const float e2_ry, const float e2_yaw)
+ float
+ ManagedObstacle::ellipsis_ellipsis_coverage(const Eigen::Vector2f e1_origin,
+ const float e1_rx,
+ const float e1_ry,
+ const float e1_yaw,
+ const Eigen::Vector2f e2_origin,
+ const float e2_rx,
+ const float e2_ry,
+ const float e2_yaw)
{
// We use a very simple approach here: We sample points in one ellipsis and check whether it lies in the other (https://www.quora.com/Is-it-possible-to-generate-random-points-within-a-given-rotated-ellipse-without-using-rejection-sampling)
// For a real approach to intersect two ellipsis see https://arxiv.org/pdf/1106.3787.pdf
@@ -94,7 +108,13 @@ namespace armarx
return (1.0f * matches) / SAMPLES;
}
- float ManagedObstacle::line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f line_seg_start, const Eigen::Vector2f line_seg_end)
+ float
+ ManagedObstacle::line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin,
+ const float e_rx,
+ const float e_ry,
+ const float e_yaw,
+ const Eigen::Vector2f line_seg_start,
+ const Eigen::Vector2f line_seg_end)
{
// Again we discretize the line into n points and we check the coverage of those points
const Eigen::Vector2f difference_vec = line_seg_end - line_seg_start;
@@ -105,7 +125,8 @@ namespace armarx
const Vector2f difference_start_origin = e_origin - line_seg_start;
const Vector2f difference_end_origin = e_origin - line_seg_end;
- if (difference_start_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ if (difference_start_origin.norm() > std::max(e_rx, e_ry) &&
+ distance < std::max(e_rx, e_ry))
{
return 0.0;
}
@@ -128,10 +149,7 @@ namespace armarx
Eigen::Vector2f end_sample = line_seg_end - i * dir;
unsigned int samples_in_ellipsis_this_iteration = 0;
- for (const auto& point :
- {
- start_sample, end_sample
- })
+ for (const auto& point : {start_sample, end_sample})
{
if (ManagedObstacle::point_ellipsis_coverage(e_origin, e_rx, e_ry, e_yaw, point))
{
@@ -152,14 +170,16 @@ namespace armarx
return (1.0f * samples_in_ellipsis) / SAMPLES; // only if one point or no point in ellipsis
}
- void ManagedObstacle::update_position(const unsigned int thickness)
+ void
+ ManagedObstacle::update_position(const unsigned int thickness)
{
if (line_matches.size() < 2)
{
return;
}
- ARMARX_DEBUG << "Obstacle " << m_obstacle.name << " has " << line_matches.size() << " matching line segments since last update";
+ ARMARX_DEBUG << "Obstacle " << m_obstacle.name << " has " << line_matches.size()
+ << " matching line segments since last update";
std::vector<float> x_pos;
std::vector<float> y_pos;
@@ -198,8 +218,10 @@ namespace armarx
double pca2_eigenvalue = eigenvalues.at<float>(1, 0);
ARMARX_DEBUG << "PCA: Mean: " << pca_center;
- ARMARX_DEBUG << "PCA1: Eigenvector: " << pca1_direction << ", Eigenvalue: " << pca1_eigenvalue;
- ARMARX_DEBUG << "PCA2: Eigenvector: " << pca2_direction << ", Eigenvalue: " << pca2_eigenvalue;
+ ARMARX_DEBUG << "PCA1: Eigenvector: " << pca1_direction
+ << ", Eigenvalue: " << pca1_eigenvalue;
+ ARMARX_DEBUG << "PCA2: Eigenvector: " << pca2_direction
+ << ", Eigenvalue: " << pca2_eigenvalue;
// calculate yaw of best line (should be close to current yaw)
const Eigen::Vector2f difference_vec = pca1_direction;
@@ -216,7 +238,9 @@ namespace armarx
float yaw = yaw2;
- if ((diff_origin_yaw1 < diff_origin_yaw2 && diff_origin_yaw1 < diff_origin_yaw2_opposite) || (diff_origin_yaw1_opposite < diff_origin_yaw2 && diff_origin_yaw1_opposite < diff_origin_yaw2_opposite))
+ if ((diff_origin_yaw1 < diff_origin_yaw2 && diff_origin_yaw1 < diff_origin_yaw2_opposite) ||
+ (diff_origin_yaw1_opposite < diff_origin_yaw2 &&
+ diff_origin_yaw1_opposite < diff_origin_yaw2_opposite))
{
yaw = yaw1;
}
@@ -232,11 +256,19 @@ namespace armarx
//debug_drawer->setSphereVisu(layer_name, debug_line_name + "_ref2", end_vec_3d, armarx::DrawColor{0, 1, 0, 0.8}, 35);
// Udate position buffer with new best line
- position_buffer.at(position_buffer_index++) = std::make_tuple(center_vec(0), center_vec(1), yaw, sqrt(pca1_eigenvalue) * 1.3, std::max(1.0f * thickness, static_cast<float>(sqrt(pca2_eigenvalue)) * 1.3f)); // add 30% security margin to length of pca
+ position_buffer.at(position_buffer_index++) =
+ std::make_tuple(center_vec(0),
+ center_vec(1),
+ yaw,
+ sqrt(pca1_eigenvalue) * 1.3,
+ std::max(1.0f * thickness,
+ static_cast<float>(sqrt(pca2_eigenvalue)) *
+ 1.3f)); // add 30% security margin to length of pca
position_buffer_index %= position_buffer.size();
position_buffer_fillctr++;
- position_buffer_fillctr = std::min(position_buffer_fillctr, static_cast<unsigned int>(position_buffer.size()));
+ position_buffer_fillctr =
+ std::min(position_buffer_fillctr, static_cast<unsigned int>(position_buffer.size()));
// Calculate means from position buffer
double sum_x_pos = 0;
@@ -268,7 +300,10 @@ namespace armarx
double mean_x_pos = sum_x_pos / position_buffer_fillctr;
double mean_y_pos = sum_y_pos / position_buffer_fillctr;
- double mean_yaw = atan2((1.0 / position_buffer_fillctr) * sum_sin_yaw, (1.0 / position_buffer_fillctr) * sum_cos_yaw); //sum_yaw / position_buffer_fillctr; //std::fmod(sum_yaw, 2.0 * M_PI);
+ double mean_yaw = atan2(
+ (1.0 / position_buffer_fillctr) * sum_sin_yaw,
+ (1.0 / position_buffer_fillctr) *
+ sum_cos_yaw); //sum_yaw / position_buffer_fillctr; //std::fmod(sum_yaw, 2.0 * M_PI);
double mean_axisX = sum_axisX / position_buffer_fillctr;
double mean_axisY = sum_axisY / position_buffer_fillctr;
mean_yaw = normalizeYaw(mean_yaw);
@@ -292,7 +327,8 @@ namespace armarx
m_updated = true;
}
- float ManagedObstacle::normalizeYaw(float yaw) const
+ float
+ ManagedObstacle::normalizeYaw(float yaw) const
{
float pi2 = 2.0 * M_PI;
while (yaw < 0)
@@ -306,17 +342,21 @@ namespace armarx
return yaw;
}
- float ManagedObstacle::getAngleBetweenVectors(const Eigen::Vector2f& v1, const Eigen::Vector2f& v2) const
+ float
+ ManagedObstacle::getAngleBetweenVectors(const Eigen::Vector2f& v1,
+ const Eigen::Vector2f& v2) const
{
// Returns an angle between 0 and 180 degree (the minimum angle between the two vectors)
const Eigen::Vector2f v1_vec_normed = v1.normalized();
const Eigen::Vector2f v2_vec_normed = v2.normalized();
- const float dot_product_vec = v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
+ const float dot_product_vec =
+ v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
float yaw = acos(dot_product_vec);
return yaw;
}
- float ManagedObstacle::getXAxisAngle(const Eigen::Vector2f& v) const
+ float
+ ManagedObstacle::getXAxisAngle(const Eigen::Vector2f& v) const
{
// Returns an angle between 0 and 360 degree (counterclockwise from x axis)
const Eigen::Vector2f v_vec_normed = v.normalized();
@@ -329,4 +369,4 @@ namespace armarx
}
return yaw;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
index a730c63ab96362386781388d65f426217b328721..f4b6657664fe7b59932127d5922b4e8548b16128 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.h
@@ -26,19 +26,18 @@
// STD/STL
#include <array>
-#include <tuple>
+#include <memory>
#include <shared_mutex>
#include <string>
+#include <tuple>
#include <vector>
-#include <memory>
// Eigen
#include <Eigen/Geometry>
// ArmarX
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.h>
-
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
namespace armarx
{
@@ -49,21 +48,38 @@ namespace armarx
class ManagedObstacle
{
public:
-
- static bool ComparatorDESCPrt(ManagedObstaclePtr& i, ManagedObstaclePtr& j)
+ static bool
+ ComparatorDESCPrt(ManagedObstaclePtr& i, ManagedObstaclePtr& j)
{
- return (ManagedObstacle::calculateObstacleArea(i->m_obstacle) > ManagedObstacle::calculateObstacleArea(j->m_obstacle));
+ return (ManagedObstacle::calculateObstacleArea(i->m_obstacle) >
+ ManagedObstacle::calculateObstacleArea(j->m_obstacle));
}
void update_position(const unsigned int);
static double calculateObstacleArea(const obstacledetection::Obstacle& o);
- static bool point_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f point);
-
- static float ellipsis_ellipsis_coverage(const Eigen::Vector2f e1_origin, const float e1_rx, const float e1_ry, const float e1_yaw, const Eigen::Vector2f e2_origin, const float e2_rx, const float e2_ry, const float e2_yaw);
-
- static float line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f line_seg_start, const Eigen::Vector2f line_seg_end);
+ static bool point_ellipsis_coverage(const Eigen::Vector2f e_origin,
+ const float e_rx,
+ const float e_ry,
+ const float e_yaw,
+ const Eigen::Vector2f point);
+
+ static float ellipsis_ellipsis_coverage(const Eigen::Vector2f e1_origin,
+ const float e1_rx,
+ const float e1_ry,
+ const float e1_yaw,
+ const Eigen::Vector2f e2_origin,
+ const float e2_rx,
+ const float e2_ry,
+ const float e2_yaw);
+
+ static float line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin,
+ const float e_rx,
+ const float e_ry,
+ const float e_yaw,
+ const Eigen::Vector2f line_seg_start,
+ const Eigen::Vector2f line_seg_end);
obstacledetection::Obstacle m_obstacle;
@@ -75,17 +91,17 @@ namespace armarx
unsigned int position_buffer_fillctr = 0;
unsigned int position_buffer_index = 0;
- std::array<std::tuple<double, double, double, double, double>, 6> position_buffer; //x, y, yaw, axisX, axisY
+ std::array<std::tuple<double, double, double, double, double>, 6>
+ position_buffer; //x, y, yaw, axisX, axisY
std::vector<Eigen::Vector2f> line_matches; // points of line segments
unsigned long m_input_index = 0;
- protected:
+ protected:
private:
float normalizeYaw(float yaw) const;
float getAngleBetweenVectors(const Eigen::Vector2f&, const Eigen::Vector2f&) const;
float getXAxisAngle(const Eigen::Vector2f&) const;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp b/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
index 3184203cd117e51e8e9748ec823208089e40a488..771cc21179faf5ff444ee4b95a0dc80ae5d3af5a 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/test/DynamicObstacleManagerTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <armar6_skills/components/DynamicObstacleManager/DynamicObstacleManager.h>
#include <iostream>
+#include <armar6_skills/components/DynamicObstacleManager/DynamicObstacleManager.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::DynamicObstacleManager instance;
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp
index bf424e5b3f05a37b6d55f52d8e6ebb2fb7bbd2b0..bbb259c8897130aaf6832be780ff6eac14116804 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.cpp
@@ -12,13 +12,16 @@
vector<Vec3d> CGraphGenerator::m_Vertices;
vector<Face> CGraphGenerator::m_Faces;
-
-void CGraphGenerator::generateGraph(CSphericalGraph* pPlainGraph, EPolyhedronType nBaseType, int nLevels, float fRadius)
+void
+CGraphGenerator::generateGraph(CSphericalGraph* pPlainGraph,
+ EPolyhedronType nBaseType,
+ int nLevels,
+ float fRadius)
{
initBasePolyhedron(nBaseType);
projectToSphere(fRadius);
- for (int L = 0 ; L < nLevels ; L++)
+ for (int L = 0; L < nLevels; L++)
{
subDivide();
projectToSphere(fRadius);
@@ -27,7 +30,8 @@ void CGraphGenerator::generateGraph(CSphericalGraph* pPlainGraph, EPolyhedronTyp
buildNodesAndEdges(pPlainGraph);
}
-void CGraphGenerator::initBasePolyhedron(EPolyhedronType nBaseType)
+void
+CGraphGenerator::initBasePolyhedron(EPolyhedronType nBaseType)
{
m_Vertices.clear();
m_Faces.clear();
@@ -52,7 +56,8 @@ void CGraphGenerator::initBasePolyhedron(EPolyhedronType nBaseType)
}
}
-void CGraphGenerator::generateTetrahedron()
+void
+CGraphGenerator::generateTetrahedron()
{
// vertices of a tetrahedron
float fSqrt3 = sqrt(3.0);
@@ -63,7 +68,7 @@ void CGraphGenerator::generateTetrahedron()
Math3d::SetVec(v[2], -fSqrt3, fSqrt3, -fSqrt3);
Math3d::SetVec(v[3], fSqrt3, -fSqrt3, -fSqrt3);
- for (int i = 0 ; i < 4 ; i++)
+ for (int i = 0; i < 4; i++)
{
m_Vertices.push_back(v[i]);
}
@@ -75,7 +80,7 @@ void CGraphGenerator::generateTetrahedron()
f[2].set(3, 2, 4);
f[3].set(4, 1, 3);
- for (int i = 0 ; i < 4 ; i++)
+ for (int i = 0; i < 4; i++)
{
f[i].m_n1--;
f[i].m_n2--;
@@ -84,7 +89,8 @@ void CGraphGenerator::generateTetrahedron()
}
}
-void CGraphGenerator::generateOctahedron()
+void
+CGraphGenerator::generateOctahedron()
{
// Six equidistant points lying on the unit sphere
Vec3d v[6];
@@ -95,7 +101,7 @@ void CGraphGenerator::generateOctahedron()
Math3d::SetVec(v[4], 0, 0, 1);
Math3d::SetVec(v[5], 0, 0, -1);
- for (int i = 0 ; i < 6 ; i++)
+ for (int i = 0; i < 6; i++)
{
m_Vertices.push_back(v[i]);
}
@@ -111,7 +117,7 @@ void CGraphGenerator::generateOctahedron()
f[6].set(2, 4, 6);
f[7].set(4, 1, 6);
- for (int i = 0 ; i < 8 ; i++)
+ for (int i = 0; i < 8; i++)
{
f[i].m_n1--;
f[i].m_n2--;
@@ -120,7 +126,8 @@ void CGraphGenerator::generateOctahedron()
}
}
-void CGraphGenerator::generateIcosahedron()
+void
+CGraphGenerator::generateIcosahedron()
{
// Twelve vertices of icosahedron on unit sphere
float tau = 0.8506508084;
@@ -141,7 +148,7 @@ void CGraphGenerator::generateIcosahedron()
Math3d::SetVec(v[10], 0, -tau, -one);
Math3d::SetVec(v[11], 0, tau, -one);
- for (int i = 0 ; i < 12 ; i++)
+ for (int i = 0; i < 12; i++)
{
m_Vertices.push_back(v[i]);
}
@@ -149,28 +156,28 @@ void CGraphGenerator::generateIcosahedron()
// Structure for unit icosahedron
Face f[20];
- f[0].set(5, 8, 9);
- f[1].set(5, 10, 8);
- f[2].set(6, 12, 7);
- f[3].set(6, 7, 11);
- f[4].set(1, 4, 5);
- f[5].set(1, 6, 4);
- f[6].set(3, 2, 8);
- f[7].set(3, 7, 2);
- f[8].set(9, 12, 1);
- f[9].set(9, 2, 12);
- f[10].set(10, 4, 11);
+ f[0].set(5, 8, 9);
+ f[1].set(5, 10, 8);
+ f[2].set(6, 12, 7);
+ f[3].set(6, 7, 11);
+ f[4].set(1, 4, 5);
+ f[5].set(1, 6, 4);
+ f[6].set(3, 2, 8);
+ f[7].set(3, 7, 2);
+ f[8].set(9, 12, 1);
+ f[9].set(9, 2, 12);
+ f[10].set(10, 4, 11);
f[11].set(10, 11, 3);
- f[12].set(9, 1, 5);
- f[13].set(12, 6, 1);
- f[14].set(5, 4, 10);
- f[15].set(6, 11, 4);
- f[16].set(8, 2, 9);
- f[17].set(7, 12, 2);
+ f[12].set(9, 1, 5);
+ f[13].set(12, 6, 1);
+ f[14].set(5, 4, 10);
+ f[15].set(6, 11, 4);
+ f[16].set(8, 2, 9);
+ f[17].set(7, 12, 2);
f[18].set(8, 10, 3);
- f[19].set(7, 3, 11);
+ f[19].set(7, 3, 11);
- for (int i = 0 ; i < 20 ; i++)
+ for (int i = 0; i < 20; i++)
{
f[i].m_n1--;
f[i].m_n2--;
@@ -179,13 +186,14 @@ void CGraphGenerator::generateIcosahedron()
}
}
-void CGraphGenerator::subDivide()
+void
+CGraphGenerator::subDivide()
{
// buffer new faces
vector<Face> faces;
// gp through all faces and subdivide
- for (int f = 0 ; f < int(m_Faces.size()) ; f++)
+ for (int f = 0; f < int(m_Faces.size()); f++)
{
// get the triangle vertex indices
int nA = m_Faces.at(f).m_n1;
@@ -260,7 +268,7 @@ void CGraphGenerator::subDivide()
f[2].set(nC_m, nB_m, nC);
f[3].set(nA_m, nB_m, nC_m);
- for (int i = 0 ; i < 4 ; i++)
+ for (int i = 0; i < 4; i++)
{
faces.push_back(f[i]);
}
@@ -270,13 +278,14 @@ void CGraphGenerator::subDivide()
m_Faces = faces;
}
-void CGraphGenerator::projectToSphere(float fRadius)
+void
+CGraphGenerator::projectToSphere(float fRadius)
{
TSphereCoord coord;
Vec3d point;
coord.fDistance = 1.0f;
- for (int v = 0 ; v < int(m_Vertices.size()) ; v++)
+ for (int v = 0; v < int(m_Vertices.size()); v++)
{
float X = m_Vertices.at(v).x;
float Y = m_Vertices.at(v).y;
@@ -286,7 +295,7 @@ void CGraphGenerator::projectToSphere(float fRadius)
// Convert Cartesian X,Y,Z to spherical (radians)
float theta = atan2(Y, X);
- float phi = atan2(sqrt(X * X + Y * Y), Z);
+ float phi = atan2(sqrt(X * X + Y * Y), Z);
// Recalculate X,Y,Z for constant r, given theta & phi.
x = fRadius * sin(phi) * cos(theta);
@@ -299,12 +308,13 @@ void CGraphGenerator::projectToSphere(float fRadius)
}
}
-void CGraphGenerator::buildNodesAndEdges(CSphericalGraph* pGraph)
+void
+CGraphGenerator::buildNodesAndEdges(CSphericalGraph* pGraph)
{
// add nodes
TSphereCoord coord;
- for (int v = 0 ; v < int(m_Vertices.size()) ; v++)
+ for (int v = 0; v < int(m_Vertices.size()); v++)
{
// printf("Vertex: %f,%f,%f\n", m_Vertices.at(v).x, m_Vertices.at(v).y, m_Vertices.at(v).z);
MathTools::convert(m_Vertices.at(v), coord);
@@ -315,7 +325,7 @@ void CGraphGenerator::buildNodesAndEdges(CSphericalGraph* pGraph)
// add edges
// go through all faces (assumes check for duplicate edges in spherical graph!?)
- for (int f = 0 ; f < int(m_Faces.size()) ; f++)
+ for (int f = 0; f < int(m_Faces.size()); f++)
{
// printf(" %d,%d,%d\n", m_Faces.at(f).m_n1, m_Faces.at(f).m_n2, m_Faces.at(f).m_n3);
pGraph->addEdge(m_Faces.at(f).m_n1, m_Faces.at(f).m_n2);
@@ -324,13 +334,14 @@ void CGraphGenerator::buildNodesAndEdges(CSphericalGraph* pGraph)
}
}
-int CGraphGenerator::findVertex(Vec3d position, float fEpsilon)
+int
+CGraphGenerator::findVertex(Vec3d position, float fEpsilon)
{
float fDistance;
float fMinDistance = FLT_MAX;
int nIndex = -1;
- for (int v = 0 ; v < int(m_Vertices.size()) ; v++)
+ for (int v = 0; v < int(m_Vertices.size()); v++)
{
fDistance = Math3d::Angle(position, m_Vertices.at(v));
@@ -348,4 +359,3 @@ int CGraphGenerator::findVertex(Vec3d position, float fEpsilon)
return nIndex;
}
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h
index d4b66e3470ede2159e14d3872623dcbaf73910df..a38fa11a74eeee368d120b824ff866cb1bbd8b0a 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphGenerator.h
@@ -12,11 +12,13 @@
// *****************************************************************
// includes
// *****************************************************************
+#include <float.h>
+
+#include <vector>
+
#include "Base/DataStructures/Graph/SphericalGraph.h"
#include "Base/Math/MathTools.h"
#include "Math/Math3d.h"
-#include <vector>
-#include <float.h>
// *****************************************************************
// enums
@@ -34,17 +36,20 @@ enum EPolyhedronType
class Face
{
public:
- Face() {};
+ Face(){};
+
Face(int n1, int n2, int n3)
{
set(n1, n2, n3);
};
- ~Face() {};
- void set(int n1, int n2, int n3)
+ ~Face(){};
+
+ void
+ set(int n1, int n2, int n3)
{
m_n1 = n1;
- m_n2 = n2 ;
+ m_n2 = n2;
m_n3 = n3;
};
@@ -59,7 +64,10 @@ public:
class CGraphGenerator
{
public:
- static void generateGraph(CSphericalGraph* pPlainGraph, EPolyhedronType nBaseType, int nLevels, float fRadius);
+ static void generateGraph(CSphericalGraph* pPlainGraph,
+ EPolyhedronType nBaseType,
+ int nLevels,
+ float fRadius);
private:
static void initBasePolyhedron(EPolyhedronType nBaseType);
@@ -72,10 +80,9 @@ private:
static int findVertex(Vec3d position, float fEpsilon);
private:
- CGraphGenerator() {};
- ~CGraphGenerator() {};
+ CGraphGenerator(){};
+ ~CGraphGenerator(){};
- static vector<Vec3d> m_Vertices;
+ static vector<Vec3d> m_Vertices;
static vector<Face> m_Faces;
};
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp
index c74c87aefdb17f3d23610cd203c8e2926034b500..63434d815b1cc0d169c543b58127dd208608c1b7 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.cpp
@@ -11,9 +11,11 @@
// includes
// *****************************************************************
#include "GraphLookupTable.h"
-#include "MathTools.h"
+
#include <cfloat>
+#include "MathTools.h"
+
// *****************************************************************
// implementation of CGraphLookupTable
// *****************************************************************
@@ -31,14 +33,15 @@ CGraphLookupTable::~CGraphLookupTable()
cleanMemory();
}
-void CGraphLookupTable::buildLookupTable(CSphericalGraph* pGraph)
+void
+CGraphLookupTable::buildLookupTable(CSphericalGraph* pGraph)
{
reset();
TNodeList* pNodes = pGraph->getNodes();
int nSize = pNodes->size();
- for (int n = 0 ; n < nSize ; n++)
+ for (int n = 0; n < nSize; n++)
{
CSGNode* pNode = pNodes->at(n);
addEntry(pNode->getPosition(), n);
@@ -47,19 +50,22 @@ void CGraphLookupTable::buildLookupTable(CSphericalGraph* pGraph)
m_pGraph = pGraph;
}
-int CGraphLookupTable::getClosestNode(TSphereCoord position)
+int
+CGraphLookupTable::getClosestNode(TSphereCoord position)
{
bool bExact;
return getClosestNode(position, bExact);
}
-int CGraphLookupTable::getClosestNode(Eigen::Vector3d position)
+int
+CGraphLookupTable::getClosestNode(Eigen::Vector3d position)
{
bool bExact;
return getClosestNode(position, bExact);
}
-int CGraphLookupTable::getClosestNode(Eigen::Vector3d position, bool& bExact)
+int
+CGraphLookupTable::getClosestNode(Eigen::Vector3d position, bool& bExact)
{
TSphereCoord coords;
MathTools::convert(position, coords);
@@ -67,8 +73,8 @@ int CGraphLookupTable::getClosestNode(Eigen::Vector3d position, bool& bExact)
return getClosestNode(coords, bExact);
}
-
-int CGraphLookupTable::getClosestNode(TSphereCoord position, bool& bExact)
+int
+CGraphLookupTable::getClosestNode(TSphereCoord position, bool& bExact)
{
int nIndex1, nIndex2;
nIndex1 = getZenithBinIndex(position.fPhi);
@@ -123,15 +129,16 @@ int CGraphLookupTable::getClosestNode(TSphereCoord position, bool& bExact)
return nBestIndex;
}
-void CGraphLookupTable::reset()
+void
+CGraphLookupTable::reset()
{
float fZenith;
- for (int z = 0 ; z < getNumberZenithBins() ; z++)
+ for (int z = 0; z < getNumberZenithBins(); z++)
{
fZenith = 180.0f / m_nMaxZenithBins * z;
- for (int a = 0 ; a < getNumberAzimuthBins(fZenith) ; a++)
+ for (int a = 0; a < getNumberAzimuthBins(fZenith); a++)
{
m_ppTable[z][a].clear();
}
@@ -141,7 +148,8 @@ void CGraphLookupTable::reset()
// *****************************************************************
// private methods
// *****************************************************************
-void CGraphLookupTable::addEntry(TSphereCoord position, int nIndex)
+void
+CGraphLookupTable::addEntry(TSphereCoord position, int nIndex)
{
int nIndex1, nIndex2;
nIndex1 = getZenithBinIndex(position.fPhi);
@@ -150,35 +158,39 @@ void CGraphLookupTable::addEntry(TSphereCoord position, int nIndex)
m_ppTable[nIndex1][nIndex2].push_back(nIndex);
}
-void CGraphLookupTable::buildMemory()
+void
+CGraphLookupTable::buildMemory()
{
float fZenith;
- m_ppTable = new std::list<int>* [m_nMaxZenithBins];
+ m_ppTable = new std::list<int>*[m_nMaxZenithBins];
- for (int z = 0 ; z < m_nMaxZenithBins ; z++)
+ for (int z = 0; z < m_nMaxZenithBins; z++)
{
fZenith = 180.0f / m_nMaxZenithBins * z;
m_ppTable[z] = new std::list<int>[getNumberAzimuthBins(fZenith)];
}
}
-void CGraphLookupTable::cleanMemory()
+void
+CGraphLookupTable::cleanMemory()
{
- for (int z = 0 ; z < m_nMaxZenithBins ; z++)
+ for (int z = 0; z < m_nMaxZenithBins; z++)
{
- delete [] m_ppTable[z];
+ delete[] m_ppTable[z];
}
- delete [] m_ppTable;
+ delete[] m_ppTable;
}
-int CGraphLookupTable::getNumberZenithBins()
+int
+CGraphLookupTable::getNumberZenithBins()
{
return m_nMaxZenithBins;
}
-int CGraphLookupTable::getNumberAzimuthBins(float fZenith)
+int
+CGraphLookupTable::getNumberAzimuthBins(float fZenith)
{
int nZenithBin = getZenithBinIndex(fZenith);
@@ -189,7 +201,9 @@ int CGraphLookupTable::getNumberAzimuthBins(float fZenith)
if (getNumberZenithBins() > 1)
{
- fNumberBins = sin(float(nZenithBin) / (getNumberZenithBins() - 1) * M_PI) * (m_nMaxAzimuthBins - nMinBins) + nMinBins;
+ fNumberBins = sin(float(nZenithBin) / (getNumberZenithBins() - 1) * M_PI) *
+ (m_nMaxAzimuthBins - nMinBins) +
+ nMinBins;
}
else
{
@@ -199,19 +213,27 @@ int CGraphLookupTable::getNumberAzimuthBins(float fZenith)
return int(fNumberBins + 0.5f);
}
-int CGraphLookupTable::getAzimuthBinIndex(float fAzimuth, float fZenith)
+int
+CGraphLookupTable::getAzimuthBinIndex(float fAzimuth, float fZenith)
{
float fBinIndex = fAzimuth / 360.0f * (getNumberAzimuthBins(fZenith) - 1) + 0.5f;
return int(fBinIndex);
}
-int CGraphLookupTable::getZenithBinIndex(float fZenith)
+int
+CGraphLookupTable::getZenithBinIndex(float fZenith)
{
float fBinIndex = fZenith / 180.0f * (m_nMaxZenithBins - 1) + 0.5f;
return int(fBinIndex);
}
-void CGraphLookupTable::getBinMinMaxValues(int nZenithBinIndex, int nAzimuthBinIndex, float& fMinZenith, float& fMaxZenith, float& fMinAzimuth, float& fMaxAzimuth)
+void
+CGraphLookupTable::getBinMinMaxValues(int nZenithBinIndex,
+ int nAzimuthBinIndex,
+ float& fMinZenith,
+ float& fMaxZenith,
+ float& fMinAzimuth,
+ float& fMaxAzimuth)
{
if ((m_nMaxZenithBins - 1) == 0)
{
@@ -231,8 +253,9 @@ void CGraphLookupTable::getBinMinMaxValues(int nZenithBinIndex, int nAzimuthBinI
}
else
{
- fMinAzimuth = (nAzimuthBinIndex - 0.5f) * 360.0f / (getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1);
- fMaxAzimuth = (nAzimuthBinIndex + 0.5f) * 360.0f / (getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1);
+ fMinAzimuth = (nAzimuthBinIndex - 0.5f) * 360.0f /
+ (getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1);
+ fMaxAzimuth = (nAzimuthBinIndex + 0.5f) * 360.0f /
+ (getNumberAzimuthBins((fMinZenith + fMaxZenith) / 2.0f) - 1);
}
}
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h
index 237d764519b235abbc19627286380fa99dd626d3..4e65b7e9bea2fdf1dcf5be2c4894772e8a4dd5bd 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphLookupTable.h
@@ -12,9 +12,10 @@
// *****************************************************************
// includes
// *****************************************************************
-#include "SphericalGraph.h"
#include <list>
+#include "SphericalGraph.h"
+
// *****************************************************************
// decleration of CGraphLookupTable
// *****************************************************************
@@ -45,14 +46,17 @@ private:
int getAzimuthBinIndex(float fAzimuth, float fZenith);
int getZenithBinIndex(float fZenith);
- void getBinMinMaxValues(int nZenithBinIndex, int nAzimuthBinIndex, float& fMinZenith, float& fMaxZenith, float& fMinAzimuth, float& fMaxAzimuth);
+ void getBinMinMaxValues(int nZenithBinIndex,
+ int nAzimuthBinIndex,
+ float& fMinZenith,
+ float& fMaxZenith,
+ float& fMinAzimuth,
+ float& fMaxAzimuth);
- int m_nMaxZenithBins;
- int m_nMaxAzimuthBins;
+ int m_nMaxZenithBins;
+ int m_nMaxAzimuthBins;
- std::list<int>** m_ppTable;
+ std::list<int>** m_ppTable;
- CSphericalGraph* m_pGraph;
+ CSphericalGraph* m_pGraph;
};
-
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp
index deaeab33e6a845231419d0856a3c8c507ac3ffa4..0f15bbced40e2ccc47a94cdb3cf4f177fe1a26ae 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.cpp
@@ -11,9 +11,11 @@
// includes
// *****************************************************************
#include "GraphMap.h"
-#include "Base/Math/MathTools.h"
-#include "Base/DataStructures/Graph/GraphProcessor.h"
+
#include <list>
+
+#include "Base/DataStructures/Graph/GraphProcessor.h"
+#include "Base/Math/MathTools.h"
#include "Helpers/helpers.h"
using namespace std;
@@ -27,7 +29,7 @@ CGraphMap::CGraphMap(CIntensityGraph* pGraph, bool bUseLookup)
m_pGraph = pGraph;
m_nNumberNodes = pGraph->getNodes()->size();
- m_nWidth = (int) sqrt(float(m_nNumberNodes));
+ m_nWidth = (int)sqrt(float(m_nNumberNodes));
m_nHeight = m_nNumberNodes / m_nWidth + 1;
m_bUseLookup = bUseLookup;
@@ -58,7 +60,8 @@ CGraphMap::~CGraphMap()
}
}
-void CGraphMap::setMap(CFloatMatrix* pMap, bool bUpdateGraph)
+void
+CGraphMap::setMap(CFloatMatrix* pMap, bool bUpdateGraph)
{
if (m_bOwnMemory)
{
@@ -77,23 +80,27 @@ void CGraphMap::setMap(CFloatMatrix* pMap, bool bUpdateGraph)
}
}
-CFloatMatrix* CGraphMap::getMap()
+CFloatMatrix*
+CGraphMap::getMap()
{
return m_pMap;
}
-int CGraphMap::getWidth()
+int
+CGraphMap::getWidth()
{
return m_nWidth;
}
-int CGraphMap::getHeight()
+int
+CGraphMap::getHeight()
{
return m_nHeight;
}
// manipulation
-void CGraphMap::applyGaussian(EOperation nOperation, Vec3d position, float fAmplitude, float fVariance)
+void
+CGraphMap::applyGaussian(EOperation nOperation, Vec3d position, float fAmplitude, float fVariance)
{
TSphereCoord coord;
MathTools::convert(position, coord);
@@ -101,7 +108,11 @@ void CGraphMap::applyGaussian(EOperation nOperation, Vec3d position, float fAmpl
applyGaussian(nOperation, coord, fAmplitude, fVariance);
}
-void CGraphMap::applyGaussian(EOperation nOperation, TSphereCoord coord, float fAmplitude, float fVariance)
+void
+CGraphMap::applyGaussian(EOperation nOperation,
+ TSphereCoord coord,
+ float fAmplitude,
+ float fVariance)
{
int nNodeID = 0;
@@ -117,7 +128,8 @@ void CGraphMap::applyGaussian(EOperation nOperation, TSphereCoord coord, float f
applyGaussian(nOperation, nNodeID, fAmplitude, fVariance);
}
-void CGraphMap::applyGaussian(EOperation nOperation, int nNodeID, float fAmplitude, float fVariance)
+void
+CGraphMap::applyGaussian(EOperation nOperation, int nNodeID, float fAmplitude, float fVariance)
{
list<int> nodes;
vector<int> accomplished;
@@ -183,19 +195,19 @@ void CGraphMap::applyGaussian(EOperation nOperation, int nNodeID, float fAmplitu
}
// add neighbours to nodelist
- vector<int>* pAdjacency = m_pGraph->getNodeAdjacency(nCurrentNode);
+ vector<int>* pAdjacency = m_pGraph->getNodeAdjacency(nCurrentNode);
// check for double nodes
int nAdjNode;
- for (int a = 0 ; a < int(pAdjacency->size()) ; a++)
+ for (int a = 0; a < int(pAdjacency->size()); a++)
{
nAdjNode = pAdjacency->at(a);
// check if we already processed this node
bool bNewNode = true;
- for (int c = 0 ; c < int(accomplished.size()) ; c++)
+ for (int c = 0; c < int(accomplished.size()); c++)
if (accomplished.at(c) == nAdjNode)
{
bNewNode = false;
@@ -222,41 +234,44 @@ void CGraphMap::applyGaussian(EOperation nOperation, int nNodeID, float fAmplitu
}
}
-void CGraphMap::updateGraph()
+void
+CGraphMap::updateGraph()
{
toGraph();
}
-void CGraphMap::updateMap()
+void
+CGraphMap::updateMap()
{
toMap();
}
-float CGraphMap::evaluateGaussian(float fDist, float fAmplitude, float fVariance)
+float
+CGraphMap::evaluateGaussian(float fDist, float fAmplitude, float fVariance)
{
- return exp(- (fDist / fVariance * 2.0f)) * fAmplitude;
+ return exp(-(fDist / fVariance * 2.0f)) * fAmplitude;
}
-void CGraphMap::toGraph()
+void
+CGraphMap::toGraph()
{
float fIntensity;
- for (int n = 0 ; n < m_nNumberNodes ; n++)
+ for (int n = 0; n < m_nNumberNodes; n++)
{
fIntensity = m_pMap->data[n];
- ((CIntensityNode*) m_pGraph->getNodes()->at(n))->setIntensity(fIntensity);
+ ((CIntensityNode*)m_pGraph->getNodes()->at(n))->setIntensity(fIntensity);
}
}
-void CGraphMap::toMap()
+void
+CGraphMap::toMap()
{
float fIntensity;
- for (int n = 0 ; n < m_nNumberNodes ; n++)
+ for (int n = 0; n < m_nNumberNodes; n++)
{
- fIntensity = ((CIntensityNode*) m_pGraph->getNodes()->at(n))->getIntensity();
+ fIntensity = ((CIntensityNode*)m_pGraph->getNodes()->at(n))->getIntensity();
m_pMap->data[n] = fIntensity;
}
}
-
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphMap.h b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.h
index d6ba662e13cc553d21b2b4d849f8b96966368e91..b276f14a7ca396e6b65aa220edd5bf3ff6042c58 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphMap.h
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphMap.h
@@ -12,8 +12,8 @@
// *****************************************************************
// includes
// *****************************************************************
-#include "Base/DataStructures/Graph/IntensityGraph.h"
#include "Base/DataStructures/Graph/GraphPyramidLookupTable.h"
+#include "Base/DataStructures/Graph/IntensityGraph.h"
#include "Math/FloatMatrix.h"
// *****************************************************************
@@ -45,7 +45,8 @@ public:
// manipulation
void applyGaussian(EOperation nOperation, Vec3d position, float fAmplitude, float fVariance);
- void applyGaussian(EOperation nOperation, TSphereCoord coord, float fAmplitude, float fVariance);
+ void
+ applyGaussian(EOperation nOperation, TSphereCoord coord, float fAmplitude, float fVariance);
void applyGaussian(EOperation nOperation, int nNodeID, float fAmplitude, float fVariance);
// update
@@ -55,18 +56,16 @@ public:
private:
float evaluateGaussian(float fDist, float fAmplitude, float fVariance);
void toGraph(); // update intensitygraph
- void toMap(); // update intensitymap
+ void toMap(); // update intensitymap
- CSphericalGraph* m_pGraph;
- int m_nNumberNodes;
- CFloatMatrix* m_pMap;
- bool m_bOwnMemory;
+ CSphericalGraph* m_pGraph;
+ int m_nNumberNodes;
+ CFloatMatrix* m_pMap;
+ bool m_bOwnMemory;
- int m_nWidth;
- int m_nHeight;
+ int m_nWidth;
+ int m_nHeight;
- bool m_bUseLookup;
- CGraphPyramidLookupTable* m_pLookupTable;
+ bool m_bUseLookup;
+ CGraphPyramidLookupTable* m_pLookupTable;
};
-
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp
index c5faffd65d311340c76ee9ea38da2dcc266b86b3..ffbb5ddbd81d160975865a61fc15050a77ede7e5 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.cpp
@@ -12,21 +12,22 @@
// includes
// *****************************************************************
// system
-#include <math.h>
#include <float.h>
+#include <math.h>
// local includes
-#include "GraphProcessor.h"
#include "Base/Math/MathTools.h"
#include "Base/Tools/DebugMemory.h"
+#include "GraphProcessor.h"
-void GraphProcessor::transform(CSphericalGraph* pGraph, TTransform transform)
+void
+GraphProcessor::transform(CSphericalGraph* pGraph, TTransform transform)
{
TNodeList* pNodes = pGraph->getNodes();
TSphereCoord position, position_tr;
- for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ for (int n = 0; n < int(pNodes->size()); n++)
{
CSGNode* pNode = pNodes->at(n);
position = pNode->getPosition();
@@ -35,13 +36,14 @@ void GraphProcessor::transform(CSphericalGraph* pGraph, TTransform transform)
}
}
-void GraphProcessor::inverseTransform(CSphericalGraph* pGraph, TTransform transform)
+void
+GraphProcessor::inverseTransform(CSphericalGraph* pGraph, TTransform transform)
{
TNodeList* pNodes = pGraph->getNodes();
TSphereCoord position, position_tr;
- for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ for (int n = 0; n < int(pNodes->size()); n++)
{
CSGNode* pNode = pNodes->at(n);
position = pNode->getPosition();
@@ -50,13 +52,14 @@ void GraphProcessor::inverseTransform(CSphericalGraph* pGraph, TTransform transf
}
}
-void GraphProcessor::invertPhi(CSphericalGraph* pGraph)
+void
+GraphProcessor::invertPhi(CSphericalGraph* pGraph)
{
TNodeList* pNodes = pGraph->getNodes();
TSphereCoord position;
- for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ for (int n = 0; n < int(pNodes->size()); n++)
{
CSGNode* pNode = pNodes->at(n);
position = pNode->getPosition();
@@ -65,13 +68,14 @@ void GraphProcessor::invertPhi(CSphericalGraph* pGraph)
}
}
-void GraphProcessor::invertTheta(CSphericalGraph* pGraph)
+void
+GraphProcessor::invertTheta(CSphericalGraph* pGraph)
{
TNodeList* pNodes = pGraph->getNodes();
TSphereCoord position;
- for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ for (int n = 0; n < int(pNodes->size()); n++)
{
CSGNode* pNode = pNodes->at(n);
position = pNode->getPosition();
@@ -80,14 +84,15 @@ void GraphProcessor::invertTheta(CSphericalGraph* pGraph)
}
}
-void GraphProcessor::copyGraphNodesAndEdges(CSphericalGraph* pSrcGraph, CSphericalGraph* pDstGraph)
+void
+GraphProcessor::copyGraphNodesAndEdges(CSphericalGraph* pSrcGraph, CSphericalGraph* pDstGraph)
{
pDstGraph->clear();
TNodeList* pNodes = pSrcGraph->getNodes();
TEdgeList* pEdges = pSrcGraph->getEdges();
- for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ for (int n = 0; n < int(pNodes->size()); n++)
{
CSGNode* pNode = pNodes->at(n);
CSGNode* pNewNode = pDstGraph->getNewNode();
@@ -95,14 +100,15 @@ void GraphProcessor::copyGraphNodesAndEdges(CSphericalGraph* pSrcGraph, CSpheric
pDstGraph->addNode(pNewNode);
}
- for (int e = 0 ; e < int(pEdges->size()) ; e++)
+ for (int e = 0; e < int(pEdges->size()); e++)
{
CSGEdge* pEdge = pEdges->at(e);
pDstGraph->addEdge(pEdge->nIndex1, pEdge->nIndex2, pEdge->nLeftFace, pEdge->nRightFace);
}
}
-int GraphProcessor::findClosestNode(CSphericalGraph* pGraph, TSphereCoord coord)
+int
+GraphProcessor::findClosestNode(CSphericalGraph* pGraph, TSphereCoord coord)
{
TNodeList* pNodes = pGraph->getNodes();
@@ -110,7 +116,7 @@ int GraphProcessor::findClosestNode(CSphericalGraph* pGraph, TSphereCoord coord)
float fDistance = 0.0f;
int nIndex = -1;
- for (int n = 0 ; n < int(pNodes->size()) ; n++)
+ for (int n = 0; n < int(pNodes->size()); n++)
{
CSGNode* pNode = pNodes->at(n);
fDistance = MathTools::getDistanceOnArc(pNode->getPosition(), coord);
@@ -125,7 +131,8 @@ int GraphProcessor::findClosestNode(CSphericalGraph* pGraph, TSphereCoord coord)
return nIndex;
}
-void GraphProcessor::findClosestNeighbours(CSphericalGraph* pGraph, int& s, int& t)
+void
+GraphProcessor::findClosestNeighbours(CSphericalGraph* pGraph, int& s, int& t)
{
TNodeList* pNodes = pGraph->getNodes();
@@ -134,13 +141,14 @@ void GraphProcessor::findClosestNeighbours(CSphericalGraph* pGraph, int& s, int&
int nIndex2 = -1;
float fDistance;
- for (int i = 0 ; i < int(pNodes->size()); i++)
+ for (int i = 0; i < int(pNodes->size()); i++)
{
- for (int j = 0 ; j < int(pNodes->size()); j++)
+ for (int j = 0; j < int(pNodes->size()); j++)
{
if (i != j)
{
- fDistance = MathTools::getDistanceOnArc(pNodes->at(i)->getPosition(), pNodes->at(j)->getPosition());
+ fDistance = MathTools::getDistanceOnArc(pNodes->at(i)->getPosition(),
+ pNodes->at(j)->getPosition());
if (fDistance < fMinDistance)
{
@@ -156,11 +164,12 @@ void GraphProcessor::findClosestNeighbours(CSphericalGraph* pGraph, int& s, int&
t = nIndex2;
}
-int GraphProcessor::findEdge(CSphericalGraph* pGraph, int nIndex1, int nIndex2)
+int
+GraphProcessor::findEdge(CSphericalGraph* pGraph, int nIndex1, int nIndex2)
{
TEdgeList* pEdges = pGraph->getEdges();
- for (int e = 0 ; e < int(pEdges->size()) ; e++)
+ for (int e = 0; e < int(pEdges->size()); e++)
{
if ((pEdges->at(e)->nIndex1 == nIndex1) && (pEdges->at(e)->nIndex2 == nIndex2))
{
@@ -177,7 +186,13 @@ int GraphProcessor::findEdge(CSphericalGraph* pGraph, int nIndex1, int nIndex2)
return -1;
}
-bool GraphProcessor::insideCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, int nPointIndex, bool& bInside)
+bool
+GraphProcessor::insideCircumcircle(CSphericalGraph* pGraph,
+ int nIndex1,
+ int nIndex2,
+ int nIndex3,
+ int nPointIndex,
+ bool& bInside)
{
TNodeList* pNodes = pGraph->getNodes();
@@ -192,14 +207,21 @@ bool GraphProcessor::insideCircumcircle(CSphericalGraph* pGraph, int nIndex1, in
TSphereCoord point = pNodes->at(nPointIndex)->getPosition();
// check with give coordinates
- float fDistance = sqrt((center.fPhi - point.fPhi) * (center.fPhi - point.fPhi) + (center.fTheta - point.fTheta) * (center.fTheta - point.fTheta));
+ float fDistance = sqrt((center.fPhi - point.fPhi) * (center.fPhi - point.fPhi) +
+ (center.fTheta - point.fTheta) * (center.fTheta - point.fTheta));
bInside = (fDistance < (fRadius + 0.0001));
return true;
}
-bool GraphProcessor::getCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, TSphereCoord& center, float& fRadius)
+bool
+GraphProcessor::getCircumcircle(CSphericalGraph* pGraph,
+ int nIndex1,
+ int nIndex2,
+ int nIndex3,
+ TSphereCoord& center,
+ float& fRadius)
{
TNodeList* pNodes = pGraph->getNodes();
float cp;
@@ -218,10 +240,11 @@ bool GraphProcessor::getCircumcircle(CSphericalGraph* pGraph, int nIndex1, int n
float cx, cy;
p1Sq = s1.fPhi * s1.fPhi + s1.fTheta * s1.fTheta;
- p2Sq = s2.fPhi * s2.fPhi + s2.fTheta * s2.fTheta;
- p3Sq = s3.fPhi * s3.fPhi + s3.fTheta * s3.fTheta;
+ p2Sq = s2.fPhi * s2.fPhi + s2.fTheta * s2.fTheta;
+ p3Sq = s3.fPhi * s3.fPhi + s3.fTheta * s3.fTheta;
- num = p1Sq * (s2.fTheta - s3.fTheta) + p2Sq * (s3.fTheta - s1.fTheta) + p3Sq * (s1.fTheta - s2.fTheta);
+ num = p1Sq * (s2.fTheta - s3.fTheta) + p2Sq * (s3.fTheta - s1.fTheta) +
+ p3Sq * (s1.fTheta - s2.fTheta);
cx = num / (2.0f * cp);
num = p1Sq * (s3.fPhi - s2.fPhi) + p2Sq * (s1.fPhi - s3.fPhi) + p3Sq * (s2.fPhi - s1.fPhi);
cy = num / (2.0f * cp);
@@ -229,7 +252,8 @@ bool GraphProcessor::getCircumcircle(CSphericalGraph* pGraph, int nIndex1, int n
center.fPhi = cx;
center.fTheta = cy;
- fRadius = sqrt((center.fPhi - s1.fPhi) * (center.fPhi - s1.fPhi) + (center.fTheta - s1.fTheta) * (center.fTheta - s1.fTheta)) ;
+ fRadius = sqrt((center.fPhi - s1.fPhi) * (center.fPhi - s1.fPhi) +
+ (center.fTheta - s1.fTheta) * (center.fTheta - s1.fTheta));
return true;
}
else
@@ -239,7 +263,8 @@ bool GraphProcessor::getCircumcircle(CSphericalGraph* pGraph, int nIndex1, int n
}
}
-int GraphProcessor::getDelaunayNeighbour(CSphericalGraph* pGraph, CSGEdge* pEdge)
+int
+GraphProcessor::getDelaunayNeighbour(CSphericalGraph* pGraph, CSGEdge* pEdge)
{
TNodeList* pNodes = pGraph->getNodes();
float fMinRadius = FLT_MAX;
@@ -247,7 +272,7 @@ int GraphProcessor::getDelaunayNeighbour(CSphericalGraph* pGraph, CSGEdge* pEdge
int nIndex = -1;
// go through all points
- for (int v = 0 ; v < int(pNodes->size()) ; v++)
+ for (int v = 0; v < int(pNodes->size()); v++)
{
if ((v == pEdge->nIndex1) || (v == pEdge->nIndex2))
{
@@ -285,7 +310,8 @@ int GraphProcessor::getDelaunayNeighbour(CSphericalGraph* pGraph, CSGEdge* pEdge
return nIndex;
}
-bool GraphProcessor::isEdgePresent(CSphericalGraph* pGraph, int nIndex1, int nIndex2)
+bool
+GraphProcessor::isEdgePresent(CSphericalGraph* pGraph, int nIndex1, int nIndex2)
{
bool bPresent = false;
@@ -304,4 +330,3 @@ bool GraphProcessor::isEdgePresent(CSphericalGraph* pGraph, int nIndex1, int nIn
return bPresent;
}
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h
index f8ef20486d22e85ddd813cbfd6b2f4350744355b..49ed1a21ab5d04a4416db2a692ac182a6ac4a1d5 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphProcessor.h
@@ -42,7 +42,16 @@ namespace GraphProcessor
int getDelaunayNeighbour(CSphericalGraph* pGraph, CSGEdge* pEdge);
// circumcircle operations
- bool insideCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, int nPointIndex, bool& bInside);
- bool getCircumcircle(CSphericalGraph* pGraph, int nIndex1, int nIndex2, int nIndex3, TSphereCoord& center, float& fRadius);
-};
-
+ bool insideCircumcircle(CSphericalGraph* pGraph,
+ int nIndex1,
+ int nIndex2,
+ int nIndex3,
+ int nPointIndex,
+ bool& bInside);
+ bool getCircumcircle(CSphericalGraph* pGraph,
+ int nIndex1,
+ int nIndex2,
+ int nIndex3,
+ TSphereCoord& center,
+ float& fRadius);
+}; // namespace GraphProcessor
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp
index ee677ba31eba996d29fc7b9ba87f383efd2677a8..594d50219d14fa7d4eaa7a5dc3139af8d2d8459d 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.cpp
@@ -10,8 +10,10 @@
// *****************************************************************
// includes
// *****************************************************************
-#include <cstdlib>
#include "GraphPyramidLookupTable.h"
+
+#include <cstdlib>
+
#include "MathTools.h"
// *****************************************************************
@@ -40,7 +42,6 @@ CGraphPyramidLookupTable::CGraphPyramidLookupTable(int nMaxZenithBins, int nMaxA
nMaxZenithBins /= m_nSubDivision;
nMaxAzimuthBins /= m_nSubDivision;
}
-
}
CGraphPyramidLookupTable::~CGraphPyramidLookupTable()
@@ -55,7 +56,8 @@ CGraphPyramidLookupTable::~CGraphPyramidLookupTable()
}
}
-void CGraphPyramidLookupTable::buildLookupTable(CSphericalGraph* pGraph)
+void
+CGraphPyramidLookupTable::buildLookupTable(CSphericalGraph* pGraph)
{
std::list<CGraphLookupTable*>::iterator iter = m_Tables.begin();
@@ -67,7 +69,8 @@ void CGraphPyramidLookupTable::buildLookupTable(CSphericalGraph* pGraph)
}
}
-int CGraphPyramidLookupTable::getClosestNode(Eigen::Vector3d position)
+int
+CGraphPyramidLookupTable::getClosestNode(Eigen::Vector3d position)
{
TSphereCoord coords;
MathTools::convert(position, coords);
@@ -75,7 +78,8 @@ int CGraphPyramidLookupTable::getClosestNode(Eigen::Vector3d position)
return getClosestNode(coords);
}
-int CGraphPyramidLookupTable::getClosestNode(TSphereCoord position)
+int
+CGraphPyramidLookupTable::getClosestNode(TSphereCoord position)
{
bool bFinished = false;
std::list<CGraphLookupTable*>::iterator iter = m_Tables.begin();
@@ -105,4 +109,3 @@ int CGraphPyramidLookupTable::getClosestNode(TSphereCoord position)
return nIndex;
}
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h
index e2cee27e2933f5738b0936128131328f47d91988..25286ac22df27439f99938f3a630ae909287bf59 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h
@@ -12,10 +12,11 @@
// *****************************************************************
// includes
// *****************************************************************
-#include "SphericalGraph.h"
-#include "GraphLookupTable.h"
#include <list>
+#include "GraphLookupTable.h"
+#include "SphericalGraph.h"
+
// *****************************************************************
// decleration of CGraphPyramidLookupTable
// *****************************************************************
@@ -35,11 +36,9 @@ public:
private:
std::list<CGraphLookupTable*> m_Tables;
- int m_nSubDivision;
- int m_nLevels;
+ int m_nSubDivision;
+ int m_nLevels;
- int m_nMaxZenithBins;
- int m_nMaxAzimuthBins;
+ int m_nMaxZenithBins;
+ int m_nMaxAzimuthBins;
};
-
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp
index 7e5bdbd399862327915a79dd8f0800149fe951fd..9537996da8c0233e75e9989cefa9fabd8234f3a9 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.cpp
@@ -13,14 +13,16 @@
// *****************************************************************
// local includes
#include "GraphTriangulation.h"
-#include "GraphProcessor.h"
+
#include "Base/Math/MathTools.h"
#include "Base/Tools/DebugMemory.h"
+#include "GraphProcessor.h"
-void GraphTriangulation::delaunayTriangulationQuadratic(CSphericalGraph* pGraph)
+void
+GraphTriangulation::delaunayTriangulationQuadratic(CSphericalGraph* pGraph)
{
TC3DNodeList* pC3DNodeList = new TC3DNodeList();
- int nNodes = (int) pGraph->getNodes()->size();
+ int nNodes = (int)pGraph->getNodes()->size();
for (int i = 0; i < nNodes; i++)
{
@@ -41,7 +43,7 @@ void GraphTriangulation::delaunayTriangulationQuadratic(CSphericalGraph* pGraph)
// add edges from triangulated graph to pGraph
TEdgeList* pEdges = pCopy->getEdges();
- for (int e = 0 ; e < pEdges->size() ; e++)
+ for (int e = 0; e < pEdges->size(); e++)
{
pGraph->addEdge(pEdges->at(e)->nIndex1, pEdges->at(e)->nIndex2);
}
@@ -49,10 +51,10 @@ void GraphTriangulation::delaunayTriangulationQuadratic(CSphericalGraph* pGraph)
delete pCopy;
}
-
-void GraphTriangulation::triangulationQuadraticSpherical(CSphericalGraph* pGraph)
+void
+GraphTriangulation::triangulationQuadraticSpherical(CSphericalGraph* pGraph)
{
- int nNumberViews = (int) pGraph->getNodes()->size();
+ int nNumberViews = (int)pGraph->getNodes()->size();
// create graph with nodes and nodes + 360
CSphericalGraph* pDoubleGraph = doubleNodes(pGraph);
@@ -65,19 +67,21 @@ void GraphTriangulation::triangulationQuadraticSpherical(CSphericalGraph* pGraph
int nIndex1, nIndex2;
CSGEdge* pCurrentEdge;
- for (int e = 0 ; e < int(pDoubleGraph->getEdges()->size()) ; e++)
+ for (int e = 0; e < int(pDoubleGraph->getEdges()->size()); e++)
{
pCurrentEdge = pDoubleGraph->getEdges()->at(e);
nIndex1 = pCurrentEdge->nIndex1;
nIndex2 = pCurrentEdge->nIndex2;
// HACK: delete outer edges
- if ((pDoubleGraph->getNode(nIndex1)->getPosition().fTheta < 30.0) || (pDoubleGraph->getNode(nIndex1)->getPosition().fTheta > 690.0))
+ if ((pDoubleGraph->getNode(nIndex1)->getPosition().fTheta < 30.0) ||
+ (pDoubleGraph->getNode(nIndex1)->getPosition().fTheta > 690.0))
{
continue;
}
- if ((pDoubleGraph->getNode(nIndex2)->getPosition().fTheta < 30.0) || (pDoubleGraph->getNode(nIndex2)->getPosition().fTheta > 690.0))
+ if ((pDoubleGraph->getNode(nIndex2)->getPosition().fTheta < 30.0) ||
+ (pDoubleGraph->getNode(nIndex2)->getPosition().fTheta > 690.0))
{
continue;
}
@@ -113,19 +117,20 @@ void GraphTriangulation::triangulationQuadraticSpherical(CSphericalGraph* pGraph
delete pDoubleGraph;
}
-void GraphTriangulation::triangulationQuartic(CSphericalGraph* pGraph, float fThreshold)
+void
+GraphTriangulation::triangulationQuartic(CSphericalGraph* pGraph, float fThreshold)
{
TNodeList* pNodes = pGraph->getNodes();
- int nNumberPoints = int(pNodes->size());
- bool bPointFree;
- int i, j, k, l;
+ int nNumberPoints = int(pNodes->size());
+ bool bPointFree;
+ int i, j, k, l;
- for (i = 0 ; i < nNumberPoints - 2 ; i++)
+ for (i = 0; i < nNumberPoints - 2; i++)
{
printf("Round %d / %d\n", i, nNumberPoints - 2);
- for (j = i + 1 ; j < nNumberPoints - 1 ; j++)
+ for (j = i + 1; j < nNumberPoints - 1; j++)
{
if (j != i)
{
@@ -137,14 +142,15 @@ void GraphTriangulation::triangulationQuartic(CSphericalGraph* pGraph, float fTh
// check if this rectangle is point free
bPointFree = true;
- for (l = 0; l < nNumberPoints ; l++)
+ for (l = 0; l < nNumberPoints; l++)
{
// only if not one of the triangle points
if (l != i && l != j && l != k)
{
bool bInside;
- if (!GraphProcessor::insideCircumcircle(pGraph, i, j, k, l, bInside))
+ if (!GraphProcessor::insideCircumcircle(
+ pGraph, i, j, k, l, bInside))
{
bPointFree = false;
break;
@@ -181,7 +187,6 @@ void GraphTriangulation::triangulationQuartic(CSphericalGraph* pGraph, float fTh
pGraph->addEdge(k, j);
}
}
-
}
}
}
@@ -189,8 +194,8 @@ void GraphTriangulation::triangulationQuartic(CSphericalGraph* pGraph, float fTh
}
}
-
-void GraphTriangulation::triangulationQuadratic(CSphericalGraph* pGraph)
+void
+GraphTriangulation::triangulationQuadratic(CSphericalGraph* pGraph)
{
TEdgeList* pEdges = pGraph->getEdges();
@@ -222,7 +227,8 @@ void GraphTriangulation::triangulationQuadratic(CSphericalGraph* pGraph)
}
}
-void GraphTriangulation::completeFacet(CSphericalGraph* pGraph, int nEdgeIndex, int nFaces)
+void
+GraphTriangulation::completeFacet(CSphericalGraph* pGraph, int nEdgeIndex, int nFaces)
{
TNodeList* pNodes = pGraph->getNodes();
TEdgeList* pEdges = pGraph->getEdges();
@@ -255,7 +261,9 @@ void GraphTriangulation::completeFacet(CSphericalGraph* pGraph, int nEdgeIndex,
}
// check side
- float fAngle = MathTools::CalculateCrossProductFromDifference(pNodes->at(s)->getPosition(), pNodes->at(t)->getPosition(), pNodes->at(u)->getPosition());
+ float fAngle = MathTools::CalculateCrossProductFromDifference(pNodes->at(s)->getPosition(),
+ pNodes->at(t)->getPosition(),
+ pNodes->at(u)->getPosition());
if (fAngle > 0.0)
{
@@ -280,7 +288,9 @@ void GraphTriangulation::completeFacet(CSphericalGraph* pGraph, int nEdgeIndex,
continue;
}
- fCp = MathTools::CalculateCrossProductFromDifference(pNodes->at(s)->getPosition(), pNodes->at(t)->getPosition(), pNodes->at(u)->getPosition());
+ fCp = MathTools::CalculateCrossProductFromDifference(pNodes->at(s)->getPosition(),
+ pNodes->at(t)->getPosition(),
+ pNodes->at(u)->getPosition());
if (fCp > 0.0)
{
@@ -338,19 +348,26 @@ void GraphTriangulation::completeFacet(CSphericalGraph* pGraph, int nEdgeIndex,
}
}
-void GraphTriangulation::updateLeftFace(CSphericalGraph* pGraph, int nEdgeIndex, int nIndex1, int nIndex2, int nFace)
+void
+GraphTriangulation::updateLeftFace(CSphericalGraph* pGraph,
+ int nEdgeIndex,
+ int nIndex1,
+ int nIndex2,
+ int nFace)
{
// vector<TView*>* pViews = &pGraph->getAspectPool()->m_Views;
vector<CSGEdge*>* pEdges = pGraph->getEdges();
bool bValid = false;
- if ((pEdges->at(nEdgeIndex)->nIndex1 == nIndex1) && (pEdges->at(nEdgeIndex)->nIndex2 == nIndex2))
+ if ((pEdges->at(nEdgeIndex)->nIndex1 == nIndex1) &&
+ (pEdges->at(nEdgeIndex)->nIndex2 == nIndex2))
{
bValid = true;
}
- if ((pEdges->at(nEdgeIndex)->nIndex1 == nIndex2) && (pEdges->at(nEdgeIndex)->nIndex2 == nIndex1))
+ if ((pEdges->at(nEdgeIndex)->nIndex1 == nIndex2) &&
+ (pEdges->at(nEdgeIndex)->nIndex2 == nIndex1))
{
bValid = true;
}
@@ -364,31 +381,32 @@ void GraphTriangulation::updateLeftFace(CSphericalGraph* pGraph, int nEdgeIndex,
{
pEdges->at(nEdgeIndex)->nLeftFace = nFace;
}
- else if ((pEdges->at(nEdgeIndex)->nIndex2 == nIndex1) && (pEdges->at(nEdgeIndex)->nRightFace == -1))
+ else if ((pEdges->at(nEdgeIndex)->nIndex2 == nIndex1) &&
+ (pEdges->at(nEdgeIndex)->nRightFace == -1))
{
pEdges->at(nEdgeIndex)->nRightFace = nFace;
}
-
}
-CSphericalGraph* GraphTriangulation::doubleNodes(CSphericalGraph* pSourceGraph)
+CSphericalGraph*
+GraphTriangulation::doubleNodes(CSphericalGraph* pSourceGraph)
{
// list to source views
TNodeList* pNodes = pSourceGraph->getNodes();
- int nNumberSourceNodes = (int) pNodes->size();
+ int nNumberSourceNodes = (int)pNodes->size();
// add views to aspectpool
vector<TSphereCoord> coordinates;
- for (int i = 0 ; i < nNumberSourceNodes ; i++)
+ for (int i = 0; i < nNumberSourceNodes; i++)
{
coordinates.push_back(pNodes->at(i)->getPosition());
}
TSphereCoord shiftedCoord;
- for (int i = 0 ; i < nNumberSourceNodes ; i++)
+ for (int i = 0; i < nNumberSourceNodes; i++)
{
shiftedCoord = pNodes->at(i)->getPosition();
shiftedCoord.fTheta += 360.0f;
@@ -413,7 +431,7 @@ CSphericalGraph* GraphTriangulation::doubleNodes(CSphericalGraph* pSourceGraph)
*/
CSphericalGraph* pTmpGraph = new CSphericalGraph();
- for (int i = 0 ; i < 2 * nNumberSourceNodes ; i++)
+ for (int i = 0; i < 2 * nNumberSourceNodes; i++)
{
CSGNode* pNode = new CSGNode(coordinates.at(i));
pTmpGraph->addNode(pNode);
diff --git a/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h
index d7296293429d8fc37d9878f10cdc5cc4e612717a..db41ca33f4f01a964f3bc441501187b872d05716 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h
+++ b/source/RobotAPI/components/EarlyVisionGraph/GraphTriangulation.h
@@ -16,11 +16,12 @@
// *****************************************************************
// includes
// *****************************************************************
-#include "SphericalGraph.h"
+#include <float.h>
+
#include "Base/DataStructures/Graph/Convexhull/C3DNode.h"
-#include "Base/DataStructures/Graph/Convexhull/HalfSpace.h"
#include "Base/DataStructures/Graph/Convexhull/CHGiftWrap.h"
-#include <float.h>
+#include "Base/DataStructures/Graph/Convexhull/HalfSpace.h"
+#include "SphericalGraph.h"
// *****************************************************************
// namespaces
@@ -41,10 +42,9 @@ public:
static void triangulationQuadratic(CSphericalGraph* pGraph);
private:
- static void updateLeftFace(CSphericalGraph* pGraph, int nEdgeIndex, int nIndex1, int nIndex2, int nFace);
+ static void
+ updateLeftFace(CSphericalGraph* pGraph, int nEdgeIndex, int nIndex1, int nIndex2, int nFace);
static void completeFacet(CSphericalGraph* pGraph, int nEdgeIndex, int nFaces);
static CSphericalGraph* doubleNodes(CSphericalGraph* pSourceGraph);
-
};
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp
index 903af4a17f79ba3cd8fb660c67e4209581b9a70e..7eb76871a201c76a5dac7f4ecd9ebe3341e10b0f 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp
+++ b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.cpp
@@ -30,21 +30,21 @@ CIntensityGraph::CIntensityGraph(const CIntensityGraph& prototype)
= default;
-CIntensityNode::CIntensityNode(TSphereCoord coord)
- : CSGNode(coord)
+CIntensityNode::CIntensityNode(TSphereCoord coord) : CSGNode(coord)
{
setIntensity(0.0f);
}
-CIntensityNode::~CIntensityNode()
- = default;
+CIntensityNode::~CIntensityNode() = default;
-void CIntensityNode::setIntensity(float fIntensity)
+void
+CIntensityNode::setIntensity(float fIntensity)
{
m_fIntensity = fIntensity;
}
-float CIntensityNode::getIntensity()
+float
+CIntensityNode::getIntensity()
{
return m_fIntensity;
}
@@ -95,38 +95,40 @@ CIntensityGraph::CIntensityGraph(std::string sFilename)
//printf("Read number edges: %d\n", getEdges()->size());
}
-CIntensityGraph::~CIntensityGraph()
- = default;
+CIntensityGraph::~CIntensityGraph() = default;
-CIntensityGraph& CIntensityGraph::operator= (CIntensityGraph const& rhs)
+CIntensityGraph&
+CIntensityGraph::operator=(CIntensityGraph const& rhs)
{
- *((CSphericalGraph*) this) = (const CSphericalGraph) rhs;
+ *((CSphericalGraph*)this) = (const CSphericalGraph)rhs;
return *this;
}
// *****************************************************************
// setting
// *****************************************************************
-void CIntensityGraph::reset()
+void
+CIntensityGraph::reset()
{
set(0.0f);
}
-void CIntensityGraph::set(float fIntensity)
+void
+CIntensityGraph::set(float fIntensity)
{
TNodeList* pNodes = getNodes();
for (auto& pNode : *pNodes)
{
- ((CIntensityNode*) pNode)->setIntensity(fIntensity);
+ ((CIntensityNode*)pNode)->setIntensity(fIntensity);
}
}
-
// *****************************************************************
// file io
// *****************************************************************
-bool CIntensityGraph::read(std::istream& infile)
+bool
+CIntensityGraph::read(std::istream& infile)
{
try
{
@@ -136,7 +138,7 @@ bool CIntensityGraph::read(std::istream& infile)
infile >> nNumberNodes;
// read all nodes
- for (int n = 0 ; n < nNumberNodes ; n++)
+ for (int n = 0; n < nNumberNodes; n++)
{
readNode(infile);
}
@@ -145,11 +147,10 @@ bool CIntensityGraph::read(std::istream& infile)
int nNumberEdges;
infile >> nNumberEdges;
- for (int e = 0 ; e < nNumberEdges ; e++)
+ for (int e = 0; e < nNumberEdges; e++)
{
readEdge(infile);
}
-
}
catch (std::istream::failure&)
{
@@ -160,7 +161,8 @@ bool CIntensityGraph::read(std::istream& infile)
return true;
}
-bool CIntensityGraph::readNode(std::istream& infile)
+bool
+CIntensityGraph::readNode(std::istream& infile)
{
CIntensityNode* pNewNode = getNewNode();
@@ -187,7 +189,8 @@ bool CIntensityGraph::readNode(std::istream& infile)
return true;
}
-bool CIntensityGraph::readEdge(std::istream& infile)
+bool
+CIntensityGraph::readEdge(std::istream& infile)
{
int nIndex1, nIndex2, nLeftFace, nRightFace;
@@ -201,8 +204,8 @@ bool CIntensityGraph::readEdge(std::istream& infile)
return true;
}
-
-bool CIntensityGraph::write(std::ostream& outfile)
+bool
+CIntensityGraph::write(std::ostream& outfile)
{
try
{
@@ -211,7 +214,7 @@ bool CIntensityGraph::write(std::ostream& outfile)
outfile << nNumberNodes << std::endl;
// write all nodes
- for (int n = 0 ; n < nNumberNodes ; n++)
+ for (int n = 0; n < nNumberNodes; n++)
{
writeNode(outfile, n);
}
@@ -220,11 +223,10 @@ bool CIntensityGraph::write(std::ostream& outfile)
int nNumberEdges = int(m_Edges.size());
outfile << nNumberEdges << std::endl;
- for (int e = 0 ; e < nNumberEdges ; e++)
+ for (int e = 0; e < nNumberEdges; e++)
{
writeEdge(outfile, e);
}
-
}
catch (std::ostream::failure&)
{
@@ -235,9 +237,10 @@ bool CIntensityGraph::write(std::ostream& outfile)
return true;
}
-bool CIntensityGraph::writeNode(std::ostream& outfile, int n)
+bool
+CIntensityGraph::writeNode(std::ostream& outfile, int n)
{
- CIntensityNode* pCurrentNode = (CIntensityNode*) m_Nodes.at(n);
+ CIntensityNode* pCurrentNode = (CIntensityNode*)m_Nodes.at(n);
outfile << pCurrentNode->getIndex() << std::endl;
outfile << pCurrentNode->getPosition().fPhi << std::endl;
@@ -248,7 +251,8 @@ bool CIntensityGraph::writeNode(std::ostream& outfile, int n)
return true;
}
-bool CIntensityGraph::writeEdge(std::ostream& outfile, int e)
+bool
+CIntensityGraph::writeEdge(std::ostream& outfile, int e)
{
CSGEdge* pCurrentEdge = m_Edges.at(e);
outfile << pCurrentEdge->nIndex1 << " ";
@@ -261,13 +265,14 @@ bool CIntensityGraph::writeEdge(std::ostream& outfile, int e)
return true;
}
-void CIntensityGraph::graphToVec(std::vector<float>& vec)
+void
+CIntensityGraph::graphToVec(std::vector<float>& vec)
{
TNodeList* nodes = this->getNodes();
for (auto& i : *nodes)
{
- CIntensityNode* node = (CIntensityNode*) i;
+ CIntensityNode* node = (CIntensityNode*)i;
vec.push_back(node->getIntensity());
}
}
diff --git a/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h
index 841f66ac7d7d46e184af2dab4e6ef61fb2dfea04..a23597776ced2a6c5e517a876ab5f46901e475b9 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h
+++ b/source/RobotAPI/components/EarlyVisionGraph/IntensityGraph.h
@@ -12,9 +12,9 @@
// *****************************************************************
// includes
// *****************************************************************
-#include "SphericalGraph.h"
#include <string>
+#include "SphericalGraph.h"
// *****************************************************************
// structures
@@ -32,7 +32,8 @@ public:
void setIntensity(float fIntensity);
float getIntensity();
- CSGNode* clone() override
+ CSGNode*
+ clone() override
{
CIntensityNode* copy = new CIntensityNode(m_Position);
copy->setIndex(m_nIndex);
@@ -41,10 +42,9 @@ public:
}
private:
- float m_fIntensity;
+ float m_fIntensity;
};
-
// *****************************************************************
// definition of sensory egosphere
// *****************************************************************
@@ -74,9 +74,9 @@ public:
bool writeNode(std::ostream& outfile, int n) override;
bool writeEdge(std::ostream& outfile, int e) override;
-
// inherited
- CIntensityNode* getNewNode() override
+ CIntensityNode*
+ getNewNode() override
{
return new CIntensityNode();
}
@@ -84,7 +84,5 @@ public:
///////////////////////////////
// operators
///////////////////////////////
- CIntensityGraph& operator= (CIntensityGraph const& rhs);
-
+ CIntensityGraph& operator=(CIntensityGraph const& rhs);
};
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp b/source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp
index 2d09f1e004fe830124cf10f47f69df24504bb860..5fe7a7e91d8197d85967c9f4bf0498e375f0de72 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp
+++ b/source/RobotAPI/components/EarlyVisionGraph/MathTools.cpp
@@ -11,7 +11,9 @@
// includes
// *****************************************************************
#include "MathTools.h"
+
#include <cmath>
+
//#include "Base/Tools/DebugMemory.h"
@@ -19,7 +21,8 @@
// transformation on sphere coordinates and paths
// **********************************************************
// returns transformation from p1 -> p2
-TTransform MathTools::determinePathTransformation(TPath* p1, TPath* p2)
+TTransform
+MathTools::determinePathTransformation(TPath* p1, TPath* p2)
{
TSphereCoord start1, start2, end1, end2;
start1 = p1->at(0).Position;
@@ -30,7 +33,8 @@ TTransform MathTools::determinePathTransformation(TPath* p1, TPath* p2)
return determineTransformation(start1, start2, end1, end2);
}
-TPath* MathTools::transformPath(TTransform transform, TPath* pPath)
+TPath*
+MathTools::transformPath(TTransform transform, TPath* pPath)
{
TPath* pResulting = new TPath();
@@ -51,10 +55,10 @@ TPath* MathTools::transformPath(TTransform transform, TPath* pPath)
}
return pResulting;
-
}
-TPath* MathTools::inverseTransformPath(TTransform transform, TPath* pPath)
+TPath*
+MathTools::inverseTransformPath(TTransform transform, TPath* pPath)
{
TPath* pResulting = new TPath();
@@ -75,10 +79,13 @@ TPath* MathTools::inverseTransformPath(TTransform transform, TPath* pPath)
}
return pResulting;
-
}
-TTransform MathTools::determineTransformation(TSphereCoord start1, TSphereCoord start2, TSphereCoord end1, TSphereCoord end2)
+TTransform
+MathTools::determineTransformation(TSphereCoord start1,
+ TSphereCoord start2,
+ TSphereCoord end1,
+ TSphereCoord end2)
{
TTransform result;
@@ -116,10 +123,12 @@ TTransform MathTools::determineTransformation(TSphereCoord start1, TSphereCoord
return result;
}
-TSphereCoord MathTools::transform(TSphereCoord sc, TTransform transform)
+TSphereCoord
+MathTools::transform(TSphereCoord sc, TTransform transform)
{
// first apply alpha , beta
- TSphereCoord rotated_ab = transformAlphaBeta(sc, transform.fAlpha, transform.fBeta, transform.betaAxis);
+ TSphereCoord rotated_ab =
+ transformAlphaBeta(sc, transform.fAlpha, transform.fBeta, transform.betaAxis);
Eigen::Vector3d rotated_ab_v;
@@ -135,25 +144,32 @@ TSphereCoord MathTools::transform(TSphereCoord sc, TTransform transform)
return rotated;
}
-TSphereCoord MathTools::inverseTransform(TSphereCoord sc, TTransform transform)
+TSphereCoord
+MathTools::inverseTransform(TSphereCoord sc, TTransform transform)
{
Eigen::Vector3d sc_v;
convert(sc, sc_v);
// first rotate
- Eigen::AngleAxisd rot(transform.fPsi * M_PI / 180.0, transform.psiAxis);
+ Eigen::AngleAxisd rot(transform.fPsi * M_PI / 180.0, transform.psiAxis);
sc_v = rot.matrix() * sc_v;
TSphereCoord rotated;
convert(sc_v, rotated);
- return inverseTransformAlphaBeta(rotated, transform.fAlpha, transform.fBeta, transform.betaAxis);
+ return inverseTransformAlphaBeta(
+ rotated, transform.fAlpha, transform.fBeta, transform.betaAxis);
}
// determine alpha and beta of a transform
// fAlpha: rotation around the z axis in counterclockwise direction considering the positive z-axis
// fBeta: rotation around the rotated y axis in counterclockwise direction considering the positive y-axis
-void MathTools::determineTransformationAlphaBeta(TSphereCoord sc1, TSphereCoord sc2, float& fAlpha, float& fBeta, Eigen::Vector3d& betaAxis)
+void
+MathTools::determineTransformationAlphaBeta(TSphereCoord sc1,
+ TSphereCoord sc2,
+ float& fAlpha,
+ float& fBeta,
+ Eigen::Vector3d& betaAxis)
{
Eigen::Vector3d vector1, vector2;
convert(sc1, vector1);
@@ -188,7 +204,8 @@ void MathTools::determineTransformationAlphaBeta(TSphereCoord sc1, TSphereCoord
rotated1 = vector1;
// set rotation: alpha around z-axis to transform v1 in same plane as v2
- Eigen::AngleAxisd rotation(fAlpha, Eigen::Vector3d::UnitZ());;
+ Eigen::AngleAxisd rotation(fAlpha, Eigen::Vector3d::UnitZ());
+ ;
Eigen::AngleAxisd rotation_yaxis(sc2.fTheta, Eigen::Vector3d::UnitZ());
// we need normal for angle calculateion, so also rotate x-axis
@@ -199,7 +216,7 @@ void MathTools::determineTransformationAlphaBeta(TSphereCoord sc1, TSphereCoord
rotated1 = rotation.matrix() * rotated1;
- rotated_y = rotation_yaxis.matrix() * rotated_y;
+ rotated_y = rotation_yaxis.matrix() * rotated_y;
betaAxis = rotated_y;
@@ -210,14 +227,15 @@ void MathTools::determineTransformationAlphaBeta(TSphereCoord sc1, TSphereCoord
fBeta *= 180.0 / M_PI;
}
-TSphereCoord MathTools::transformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis)
+TSphereCoord
+MathTools::transformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis)
{
TSphereCoord result;
Eigen::Vector3d vector1, vector1_rotated;
convert(sc, vector1);
// rotate around positive z-axis
- Eigen::AngleAxisd rotation_alpha(fAlpha / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
+ Eigen::AngleAxisd rotation_alpha(fAlpha / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
// rotate vector aroun z axis
vector1_rotated = rotation_alpha.matrix() * vector1;
@@ -231,7 +249,11 @@ TSphereCoord MathTools::transformAlphaBeta(TSphereCoord sc, float fAlpha, float
return result;
}
-TSphereCoord MathTools::inverseTransformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis)
+TSphereCoord
+MathTools::inverseTransformAlphaBeta(TSphereCoord sc,
+ float fAlpha,
+ float fBeta,
+ Eigen::Vector3d betaAxis)
{
Eigen::Vector3d vector1;
convert(sc, vector1);
@@ -255,26 +277,29 @@ TSphereCoord MathTools::inverseTransformAlphaBeta(TSphereCoord sc, float fAlpha,
// **********************************************************
// calculations on sphere coordinates
// **********************************************************
-float MathTools::CalculateCrossProductFromDifference(TSphereCoord p1, TSphereCoord p2, TSphereCoord p3)
+float
+MathTools::CalculateCrossProductFromDifference(TSphereCoord p1, TSphereCoord p2, TSphereCoord p3)
{
float u1, v1, u2, v2;
- u1 = p2.fPhi - p1.fPhi;
- v1 = p2.fTheta - p1.fTheta;
- u2 = p3.fPhi - p1.fPhi;
- v2 = p3.fTheta - p1.fTheta;
+ u1 = p2.fPhi - p1.fPhi;
+ v1 = p2.fTheta - p1.fTheta;
+ u2 = p3.fPhi - p1.fPhi;
+ v2 = p3.fTheta - p1.fTheta;
return u1 * v2 - v1 * u2;
}
-double MathTools::Angle(Eigen::Vector3d vec1, Eigen::Vector3d vec2)
+double
+MathTools::Angle(Eigen::Vector3d vec1, Eigen::Vector3d vec2)
{
double r = vec1.dot(vec2) / (vec1.norm() * vec2.norm());
r = std::min(1.0, std::max(-1.0, r));
- return std::acos(r);
+ return std::acos(r);
}
-float MathTools::getDistanceOnArc(TSphereCoord p1, TSphereCoord p2)
+float
+MathTools::getDistanceOnArc(TSphereCoord p1, TSphereCoord p2)
{
Eigen::Vector3d vec1, vec2;
p1.fDistance = 1.0f;
@@ -283,7 +308,7 @@ float MathTools::getDistanceOnArc(TSphereCoord p1, TSphereCoord p2)
convert(p1, vec1);
convert(p2, vec2);
- float fAngle = (float) MathTools::Angle(vec1, vec2);
+ float fAngle = (float)MathTools::Angle(vec1, vec2);
// length of circular arc
// not necessary because Math3d::Angle returns a value in radians
@@ -292,7 +317,8 @@ float MathTools::getDistanceOnArc(TSphereCoord p1, TSphereCoord p2)
return fAngle;
}
-float MathTools::getDistance(TSphereCoord p1, TSphereCoord p2, float fRadius)
+float
+MathTools::getDistance(TSphereCoord p1, TSphereCoord p2, float fRadius)
{
// never use this!!!!
TSphereCoord diff;
@@ -307,7 +333,12 @@ float MathTools::getDistance(TSphereCoord p1, TSphereCoord p2, float fRadius)
// **********************************************************
// extended 2d Math
// **********************************************************
-bool MathTools::IntersectLines(Eigen::Vector2d p1, Eigen::Vector2d m1, Eigen::Vector2d p2, Eigen::Vector2d m2, Eigen::Vector2d& res)
+bool
+MathTools::IntersectLines(Eigen::Vector2d p1,
+ Eigen::Vector2d m1,
+ Eigen::Vector2d p2,
+ Eigen::Vector2d m2,
+ Eigen::Vector2d& res)
{
// calculate perpendicular bisector of sides
float dx = float(p1(0) - p2(0));
@@ -350,7 +381,10 @@ bool MathTools::IntersectLines(Eigen::Vector2d p1, Eigen::Vector2d m1, Eigen::Ve
// extended 3d Math
// **********************************************************
// checked and working
-float MathTools::AngleAndDirection(Eigen::Vector3d vector1, Eigen::Vector3d vector2, Eigen::Vector3d normal)
+float
+MathTools::AngleAndDirection(Eigen::Vector3d vector1,
+ Eigen::Vector3d vector2,
+ Eigen::Vector3d normal)
{
double fAngle = MathTools::Angle(vector1, vector2);
@@ -359,7 +393,7 @@ float MathTools::AngleAndDirection(Eigen::Vector3d vector1, Eigen::Vector3d vect
// compare axis and normal
normal.normalize();
- double c = normal.dot(vector1);
+ double c = normal.dot(vector1);
axis += vector1;
double d = normal.dot(axis) - c;
@@ -370,19 +404,24 @@ float MathTools::AngleAndDirection(Eigen::Vector3d vector1, Eigen::Vector3d vect
return 2 * M_PI - fAngle;
}
- return (float) fAngle;
+ return (float)fAngle;
}
// checked and working
-Eigen::Vector3d MathTools::dropAPerpendicular(Eigen::Vector3d point, Eigen::Vector3d linepoint, Eigen::Vector3d linevector)
+Eigen::Vector3d
+MathTools::dropAPerpendicular(Eigen::Vector3d point,
+ Eigen::Vector3d linepoint,
+ Eigen::Vector3d linevector)
{
// helping plane normal (ax + by + cz + d = 0)
Eigen::Vector3d plane_normal = linevector; // (a,b,c)
- float d = (float) - plane_normal.dot(point);
+ float d = (float)-plane_normal.dot(point);
// calculate k from plane and line intersection
- float z = float(- plane_normal(0) * linepoint(0) - plane_normal(1) * linepoint(1) - plane_normal(2) * linepoint(2) - d);
- float n = float(plane_normal(0) * linevector(0) + plane_normal(1) * linevector(1) + plane_normal(2) * linevector(2));
+ float z = float(-plane_normal(0) * linepoint(0) - plane_normal(1) * linepoint(1) -
+ plane_normal(2) * linepoint(2) - d);
+ float n = float(plane_normal(0) * linevector(0) + plane_normal(1) * linevector(1) +
+ plane_normal(2) * linevector(2));
float k = z / n;
@@ -391,14 +430,14 @@ Eigen::Vector3d MathTools::dropAPerpendicular(Eigen::Vector3d point, Eigen::Vect
linevector += linepoint;
return linevector;
-
}
// **********************************************************
// conversions
// **********************************************************
// checked and working
-void MathTools::convert(TSphereCoord in, Eigen::Vector3d& out)
+void
+MathTools::convert(TSphereCoord in, Eigen::Vector3d& out)
{
// alpha is azimuth, beta is polar angle
in.fPhi *= M_PI / 180.0f;
@@ -423,23 +462,24 @@ void MathTools::convert(TSphereCoord in, Eigen::Vector3d& out)
out = vector;
}
-
-void MathTools::convertWithDistance(TSphereCoord in, Eigen::Vector3d& out)
+void
+MathTools::convertWithDistance(TSphereCoord in, Eigen::Vector3d& out)
{
convert(in, out);
out *= in.fDistance;
}
-void MathTools::convert(TSphereCoord in, Eigen::Vector3f& out)
+void
+MathTools::convert(TSphereCoord in, Eigen::Vector3f& out)
{
Eigen::Vector3d vec;
convert(in, vec);
out = vec.cast<float>();
}
-
// checked and working
-void MathTools::convert(Eigen::Vector3d in, TSphereCoord& out)
+void
+MathTools::convert(Eigen::Vector3d in, TSphereCoord& out)
{
// alpha is azimuth, beta is polar angle
out.fDistance = in.norm();
@@ -456,9 +496,9 @@ void MathTools::convert(Eigen::Vector3d in, TSphereCoord& out)
fAngle = 1.0f;
}
- out.fPhi = (float) acos(fAngle);
+ out.fPhi = (float)acos(fAngle);
- out.fTheta = (float) atan2(in(1), in(0));
+ out.fTheta = (float)atan2(in(1), in(0));
// convert to angles and correct interval
out.fPhi /= M_PI / 180.0f;
@@ -477,10 +517,9 @@ void MathTools::convert(Eigen::Vector3d in, TSphereCoord& out)
}
}
-
-void MathTools::convert(Eigen::Vector3f in, TSphereCoord& out)
+void
+MathTools::convert(Eigen::Vector3f in, TSphereCoord& out)
{
Eigen::Vector3d vec(in(0), in(1), in(2));
convert(vec, out);
}
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/MathTools.h b/source/RobotAPI/components/EarlyVisionGraph/MathTools.h
index a42bd392f8e7a871167bdc38296100a4805116ac..839d3c6752fae8449285807ea968c2f4e44c4abd 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/MathTools.h
+++ b/source/RobotAPI/components/EarlyVisionGraph/MathTools.h
@@ -20,11 +20,11 @@
// *****************************************************************
// includes
// *****************************************************************
-#include "Structs.h"
-
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include "Structs.h"
+
// *****************************************************************
// class MathTools
// *****************************************************************
@@ -36,25 +36,42 @@ public:
static TPath* transformPath(TTransform transform, TPath* pPath);
static TPath* inverseTransformPath(TTransform transform, TPath* pPath);
- static TTransform determineTransformation(TSphereCoord start1, TSphereCoord start2, TSphereCoord end1, TSphereCoord end2);
+ static TTransform determineTransformation(TSphereCoord start1,
+ TSphereCoord start2,
+ TSphereCoord end1,
+ TSphereCoord end2);
static TSphereCoord transform(TSphereCoord sc, TTransform transform);
static TSphereCoord inverseTransform(TSphereCoord sc, TTransform transform);
- static void determineTransformationAlphaBeta(TSphereCoord sc1, TSphereCoord sc2, float& fAlpha, float& fBeta, Eigen::Vector3d& betaAxis);
- static TSphereCoord transformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis);
- static TSphereCoord inverseTransformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis);
+ static void determineTransformationAlphaBeta(TSphereCoord sc1,
+ TSphereCoord sc2,
+ float& fAlpha,
+ float& fBeta,
+ Eigen::Vector3d& betaAxis);
+ static TSphereCoord
+ transformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis);
+ static TSphereCoord
+ inverseTransformAlphaBeta(TSphereCoord sc, float fAlpha, float fBeta, Eigen::Vector3d betaAxis);
// calculations on sphere coordimates
- static float CalculateCrossProductFromDifference(TSphereCoord p1, TSphereCoord p2, TSphereCoord p3);
+ static float
+ CalculateCrossProductFromDifference(TSphereCoord p1, TSphereCoord p2, TSphereCoord p3);
static float getDistanceOnArc(TSphereCoord p1, TSphereCoord p2);
static float getDistance(TSphereCoord p1, TSphereCoord p2, float fRadius);
// extended 2d math
- static bool IntersectLines(Eigen::Vector2d p1, Eigen::Vector2d m1, Eigen::Vector2d p2, Eigen::Vector2d m2, Eigen::Vector2d& res);
+ static bool IntersectLines(Eigen::Vector2d p1,
+ Eigen::Vector2d m1,
+ Eigen::Vector2d p2,
+ Eigen::Vector2d m2,
+ Eigen::Vector2d& res);
// extended 3d math
- static Eigen::Vector3d dropAPerpendicular(Eigen::Vector3d point, Eigen::Vector3d linepoint, Eigen::Vector3d linevector);
- static float AngleAndDirection(Eigen::Vector3d vector1, Eigen::Vector3d vector2, Eigen::Vector3d normal);
+ static Eigen::Vector3d dropAPerpendicular(Eigen::Vector3d point,
+ Eigen::Vector3d linepoint,
+ Eigen::Vector3d linevector);
+ static float
+ AngleAndDirection(Eigen::Vector3d vector1, Eigen::Vector3d vector2, Eigen::Vector3d normal);
// coorrdinate conversion
static void convert(TSphereCoord in, Eigen::Vector3d& out);
@@ -64,7 +81,4 @@ public:
static void convertWithDistance(TSphereCoord in, Eigen::Vector3d& out);
static double Angle(Eigen::Vector3d vec1, Eigen::Vector3d vec2);
-
-
};
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp
index ec3c4e3fcd85437ec7d99f391197a245e4e19d55..fdc47d30679f635a82fb657e2fa80b58fe67a25e 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp
+++ b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.cpp
@@ -12,6 +12,7 @@
// includes
// *****************************************************************
#include "SphericalGraph.h"
+
//#include "Base/Tools/DebugMemory.h"
// *****************************************************************
@@ -32,7 +33,8 @@ CSphericalGraph::~CSphericalGraph()
clear();
}
-CSphericalGraph& CSphericalGraph::operator= (CSphericalGraph const& rhs)
+CSphericalGraph&
+CSphericalGraph::operator=(CSphericalGraph const& rhs)
{
clear();
@@ -41,7 +43,7 @@ CSphericalGraph& CSphericalGraph::operator= (CSphericalGraph const& rhs)
int nNumberNodes = int(nodes->size());
- for (int n = 0 ; n < nNumberNodes ; n++)
+ for (int n = 0; n < nNumberNodes; n++)
{
addNode(nodes->at(n)->clone());
}
@@ -51,7 +53,7 @@ CSphericalGraph& CSphericalGraph::operator= (CSphericalGraph const& rhs)
int nNumberEdges = int(edges->size());
- for (int e = 0 ; e < nNumberEdges ; e++)
+ for (int e = 0; e < nNumberEdges; e++)
{
CSGEdge* edge = edges->at(e);
addEdge(edge->nIndex1, edge->nIndex2, edge->nLeftFace, edge->nRightFace);
@@ -60,11 +62,11 @@ CSphericalGraph& CSphericalGraph::operator= (CSphericalGraph const& rhs)
return *this;
}
-
// *****************************************************************
// control
// *****************************************************************
-void CSphericalGraph::clear()
+void
+CSphericalGraph::clear()
{
// clear edges
std::vector<CSGEdge*>::iterator iter = m_Edges.begin();
@@ -93,7 +95,8 @@ void CSphericalGraph::clear()
// graph building
// *****************************************************************
// nodes
-int CSphericalGraph::addNode(CSGNode* pNode)
+int
+CSphericalGraph::addNode(CSGNode* pNode)
{
m_Nodes.push_back(pNode);
pNode->setIndex(int(m_Nodes.size() - 1));
@@ -102,12 +105,14 @@ int CSphericalGraph::addNode(CSGNode* pNode)
}
// edges
-int CSphericalGraph::addEdge(int nIndex1, int nIndex2)
+int
+CSphericalGraph::addEdge(int nIndex1, int nIndex2)
{
return addEdge(nIndex1, nIndex2, -1, -1);
}
-int CSphericalGraph::addEdge(int nIndex1, int nIndex2, int nLeftFace, int nRightFace)
+int
+CSphericalGraph::addEdge(int nIndex1, int nIndex2, int nLeftFace, int nRightFace)
{
CSGEdge* pEdge = new CSGEdge;
pEdge->nIndex1 = nIndex1;
@@ -127,7 +132,8 @@ int CSphericalGraph::addEdge(int nIndex1, int nIndex2, int nLeftFace, int nRight
return int(m_Edges.size()) - 1;
}
-void CSphericalGraph::addNodeAdjacency(int nNode, int nAdjacency)
+void
+CSphericalGraph::addNodeAdjacency(int nNode, int nAdjacency)
{
if (nNode == nAdjacency)
{
@@ -154,27 +160,32 @@ void CSphericalGraph::addNodeAdjacency(int nNode, int nAdjacency)
// *****************************************************************
// member access
// *****************************************************************
-TEdgeList* CSphericalGraph::getEdges()
+TEdgeList*
+CSphericalGraph::getEdges()
{
return &m_Edges;
}
-TNodeList* CSphericalGraph::getNodes()
+TNodeList*
+CSphericalGraph::getNodes()
{
return &m_Nodes;
}
-CSGEdge* CSphericalGraph::getEdge(int nEdgeIndex)
+CSGEdge*
+CSphericalGraph::getEdge(int nEdgeIndex)
{
return m_Edges.at(nEdgeIndex);
}
-CSGNode* CSphericalGraph::getNode(int nIndex)
+CSGNode*
+CSphericalGraph::getNode(int nIndex)
{
return m_Nodes.at(nIndex);
}
-std::vector<int>* CSphericalGraph::getNodeAdjacency(int nIndex)
+std::vector<int>*
+CSphericalGraph::getNodeAdjacency(int nIndex)
{
return &m_NodeAdjacency[nIndex];
}
@@ -182,7 +193,8 @@ std::vector<int>* CSphericalGraph::getNodeAdjacency(int nIndex)
// *****************************************************************
// file io
// *****************************************************************
-bool CSphericalGraph::read(std::istream& infile)
+bool
+CSphericalGraph::read(std::istream& infile)
{
try
{
@@ -192,7 +204,7 @@ bool CSphericalGraph::read(std::istream& infile)
infile >> nNumberNodes;
// read all nodes
- for (int n = 0 ; n < nNumberNodes ; n++)
+ for (int n = 0; n < nNumberNodes; n++)
{
readNode(infile);
}
@@ -201,11 +213,10 @@ bool CSphericalGraph::read(std::istream& infile)
int nNumberEdges;
infile >> nNumberEdges;
- for (int e = 0 ; e < nNumberEdges ; e++)
+ for (int e = 0; e < nNumberEdges; e++)
{
readEdge(infile);
}
-
}
catch (std::istream::failure&)
{
@@ -216,9 +227,10 @@ bool CSphericalGraph::read(std::istream& infile)
return true;
}
-bool CSphericalGraph::readNode(std::istream& infile)
+bool
+CSphericalGraph::readNode(std::istream& infile)
{
- CSGNode* pNewNode = (CSGNode*) getNewNode();
+ CSGNode* pNewNode = (CSGNode*)getNewNode();
int nIndex;
TSphereCoord pos;
@@ -238,9 +250,8 @@ bool CSphericalGraph::readNode(std::istream& infile)
return true;
}
-
-
-bool CSphericalGraph::readEdge(std::istream& infile)
+bool
+CSphericalGraph::readEdge(std::istream& infile)
{
int nIndex1, nIndex2, nLeftFace, nRightFace;
@@ -254,8 +265,8 @@ bool CSphericalGraph::readEdge(std::istream& infile)
return true;
}
-
-bool CSphericalGraph::write(std::ostream& outfile)
+bool
+CSphericalGraph::write(std::ostream& outfile)
{
try
{
@@ -264,7 +275,7 @@ bool CSphericalGraph::write(std::ostream& outfile)
outfile << nNumberNodes << std::endl;
// write all nodes
- for (int n = 0 ; n < nNumberNodes ; n++)
+ for (int n = 0; n < nNumberNodes; n++)
{
writeNode(outfile, n);
}
@@ -273,11 +284,10 @@ bool CSphericalGraph::write(std::ostream& outfile)
int nNumberEdges = int(m_Edges.size());
outfile << nNumberEdges << std::endl;
- for (int e = 0 ; e < nNumberEdges ; e++)
+ for (int e = 0; e < nNumberEdges; e++)
{
writeEdge(outfile, e);
}
-
}
catch (std::ostream::failure&)
{
@@ -288,7 +298,8 @@ bool CSphericalGraph::write(std::ostream& outfile)
return true;
}
-bool CSphericalGraph::writeNode(std::ostream& outfile, int n)
+bool
+CSphericalGraph::writeNode(std::ostream& outfile, int n)
{
CSGNode* pCurrentNode = m_Nodes.at(n);
@@ -300,7 +311,8 @@ bool CSphericalGraph::writeNode(std::ostream& outfile, int n)
return true;
}
-bool CSphericalGraph::writeEdge(std::ostream& outfile, int e)
+bool
+CSphericalGraph::writeEdge(std::ostream& outfile, int e)
{
CSGEdge* pCurrentEdge = m_Edges.at(e);
outfile << pCurrentEdge->nIndex1 << " ";
diff --git a/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h
index f72f3bc3414e1bb39a735a9b916dc63f4d33079b..45dc09a8a16d7cd460c5b0b4644a1abdd818e137 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h
+++ b/source/RobotAPI/components/EarlyVisionGraph/SphericalGraph.h
@@ -13,9 +13,9 @@
// includes
// *****************************************************************
// stl includes
-#include <vector>
#include <istream>
#include <ostream>
+#include <vector>
// local includes
#include "Structs.h"
@@ -32,43 +32,56 @@ class CSGEdge
{
public:
// indices of nodes
- int nIndex1;
- int nIndex2;
+ int nIndex1;
+ int nIndex2;
// indices of faces (needed in triangulation
- int nLeftFace;
- int nRightFace;
+ int nLeftFace;
+ int nRightFace;
};
class CSGNode
{
public:
- CSGNode() {}
+ CSGNode()
+ {
+ }
+
CSGNode(TSphereCoord position)
{
m_Position = position;
}
- virtual ~CSGNode() {}
- TSphereCoord getPosition()
+ virtual ~CSGNode()
+ {
+ }
+
+ TSphereCoord
+ getPosition()
{
return m_Position;
}
- void setPosition(TSphereCoord position)
+
+ void
+ setPosition(TSphereCoord position)
{
m_Position = position;
}
- int getIndex()
+ int
+ getIndex()
{
return m_nIndex;
}
- void setIndex(int nIndex)
+
+ void
+ setIndex(int nIndex)
{
m_nIndex = nIndex;
}
- virtual CSGNode* clone()
+ virtual CSGNode*
+ clone()
{
CSGNode* copy = new CSGNode(m_Position);
copy->setIndex(m_nIndex);
@@ -76,9 +89,8 @@ public:
}
protected:
- TSphereCoord m_Position;
- int m_nIndex;
-
+ TSphereCoord m_Position;
+ int m_nIndex;
};
// *****************************************************************
@@ -100,7 +112,9 @@ public:
CSphericalGraph(const CSphericalGraph& prototype);
virtual ~CSphericalGraph();
- virtual CSGNode* getNewNode()
+
+ virtual CSGNode*
+ getNewNode()
{
return new CSGNode();
}
@@ -124,12 +138,12 @@ public:
///////////////////////////////
// member access
///////////////////////////////
- TEdgeList* getEdges();
- TNodeList* getNodes();
+ TEdgeList* getEdges();
+ TNodeList* getNodes();
- CSGEdge* getEdge(int nEdgeIndex);
- CSGNode* getNode(int nIndex);
- std::vector<int>* getNodeAdjacency(int nIndex);
+ CSGEdge* getEdge(int nEdgeIndex);
+ CSGNode* getNode(int nIndex);
+ std::vector<int>* getNodeAdjacency(int nIndex);
///////////////////////////////
// file io
@@ -143,16 +157,13 @@ public:
virtual bool writeEdge(std::ostream& outfile, int e);
-
///////////////////////////////
// operators
///////////////////////////////
- CSphericalGraph& operator= (CSphericalGraph const& rhs);
+ CSphericalGraph& operator=(CSphericalGraph const& rhs);
protected:
- TEdgeList m_Edges;
- TNodeList m_Nodes;
- std::vector<int> m_NodeAdjacency[MAX_NODES];
-
+ TEdgeList m_Edges;
+ TNodeList m_Nodes;
+ std::vector<int> m_NodeAdjacency[MAX_NODES];
};
-
diff --git a/source/RobotAPI/components/EarlyVisionGraph/Structs.h b/source/RobotAPI/components/EarlyVisionGraph/Structs.h
index cf8308fccc46311320251542a0971918addb43e4..30164292bac4abfe21f24158651f57f83806bd61 100644
--- a/source/RobotAPI/components/EarlyVisionGraph/Structs.h
+++ b/source/RobotAPI/components/EarlyVisionGraph/Structs.h
@@ -10,6 +10,7 @@
#pragma once
#include <vector>
+
#include <Eigen/Core>
// *****************************************************************
@@ -22,8 +23,8 @@
struct TSphereCoord
{
float fDistance;
- float fPhi; // zenith
- float fTheta; // azimuth
+ float fPhi; // zenith
+ float fTheta; // azimuth
};
// transformation from one sphere to another
@@ -31,8 +32,8 @@ struct TSphereCoord
// psi: orientation of the view = rotation around the rotated (by spherical coords) x-axis
struct TSphereTransform
{
- TSphereCoord fPosition;
- float fPsi; // orientation
+ TSphereCoord fPosition;
+ float fPsi; // orientation
};
// transformation from one sphere to another
@@ -53,29 +54,26 @@ struct TTransform
// *****************************************************************
struct TPathElement
{
- TSphereCoord Position;
- int nNodeIndex;
- int nHits;
- float fBestSimilarity;
+ TSphereCoord Position;
+ int nNodeIndex;
+ int nHits;
+ float fBestSimilarity;
};
using TPath = std::vector<TPathElement>;
struct THypothesis
{
- int nID;
- TPath Path;
+ int nID;
+ TPath Path;
// path look-a-like rating
- float fRating;
+ float fRating;
// visual similarity of last comparison
- float fSimilarity;
+ float fSimilarity;
// overall visual similarity
- float fAccSimilarity;
+ float fAccSimilarity;
// overall rating
- float fOverallRating;
- TTransform Transform;
- bool bValidTransform;
+ float fOverallRating;
+ TTransform Transform;
+ bool bValidTransform;
};
-
-
-
diff --git a/source/RobotAPI/components/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.cpp b/source/RobotAPI/components/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.cpp
index e2f09c5f519d5959954e0ccd2d21e2f9aad0fdae..5a95318cebbcc28309be1d92ba06c9bdbe7dcc4c 100644
--- a/source/RobotAPI/components/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.cpp
+++ b/source/RobotAPI/components/FamiliarObjectDetectionExample/FamiliarObjectDetectionExample.cpp
@@ -86,7 +86,6 @@ namespace armarx::familiar_objects
void
FamiliarObjectDetectionExample::onInitComponent()
{
-
}
void
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
index f8ad1a0439f937ec5ebe15f5afae05dd2ab93a6e..b7d2aad5982b95fad8d452446fb325cba8bee788 100644
--- a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
@@ -22,6 +22,8 @@
#include "FrameTracking.h"
+#include <time.h>
+
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
@@ -30,12 +32,10 @@
#include <RobotAPI/components/units/KinematicUnitObserver.h>
-#include <time.h>
-
-
namespace armarx
{
- void FrameTracking::onInitComponent()
+ void
+ FrameTracking::onInitComponent()
{
usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
@@ -55,33 +55,40 @@ namespace armarx
pitchAcceleration = getProperty<float>("PitchAcceleration").getValue();
}
-
- void FrameTracking::onConnectComponent()
+ void
+ FrameTracking::onConnectComponent()
{
robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
- getProperty<std::string>("RobotStateComponentName").getValue());
+ getProperty<std::string>("RobotStateComponentName").getValue());
kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(
- getProperty<std::string>("KinematicUnitName").getValue());
+ getProperty<std::string>("KinematicUnitName").getValue());
kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(
- getProperty<std::string>("KinematicUnitObserverName").getValue());
+ getProperty<std::string>("KinematicUnitObserverName").getValue());
localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
- headYawJoint = localRobot->getRobotNode(getProperty<std::string>("HeadYawJoint").getValue());
- if (!headYawJoint || !(headYawJoint->isRotationalJoint() || headYawJoint->isTranslationalJoint()))
+ headYawJoint =
+ localRobot->getRobotNode(getProperty<std::string>("HeadYawJoint").getValue());
+ if (!headYawJoint ||
+ !(headYawJoint->isRotationalJoint() || headYawJoint->isTranslationalJoint()))
{
- ARMARX_ERROR << getProperty<std::string>("HeadYawJoint").getValue() << " is not a valid joint.";
+ ARMARX_ERROR << getProperty<std::string>("HeadYawJoint").getValue()
+ << " is not a valid joint.";
getArmarXManager()->asyncShutdown();
}
- headPitchJoint = localRobot->getRobotNode(getProperty<std::string>("HeadPitchJoint").getValue());
- if (!headPitchJoint || !(headPitchJoint->isRotationalJoint() || headPitchJoint->isTranslationalJoint()))
+ headPitchJoint =
+ localRobot->getRobotNode(getProperty<std::string>("HeadPitchJoint").getValue());
+ if (!headPitchJoint ||
+ !(headPitchJoint->isRotationalJoint() || headPitchJoint->isTranslationalJoint()))
{
- ARMARX_ERROR << getProperty<std::string>("HeadPitchJoint").getValue() << " is not a valid joint.";
+ ARMARX_ERROR << getProperty<std::string>("HeadPitchJoint").getValue()
+ << " is not a valid joint.";
getArmarXManager()->asyncShutdown();
}
cameraNode = localRobot->getRobotNode(getProperty<std::string>("CameraNode").getValue());
if (!cameraNode)
{
- ARMARX_ERROR << getProperty<std::string>("CameraNode").getValue() << " is not a valid node.";
+ ARMARX_ERROR << getProperty<std::string>("CameraNode").getValue()
+ << " is not a valid node.";
getArmarXManager()->asyncShutdown();
}
@@ -90,171 +97,224 @@ namespace armarx
if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
{
- _remoteGui = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
+ _remoteGui = getProxy<RemoteGuiInterfacePrx>(
+ getProperty<std::string>("RemoteGuiName").getValue());
RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Tracking: ")).addChild(
- RemoteGui::makeTextLabel("Enabled")).addChild(
- RemoteGui::makeCheckBox("enabled").value(enabled)).addChild(
- RemoteGui::makeTextLabel("Frame")).addChild(
- RemoteGui::makeComboBox("tracking_frame").options(
- localRobot->getRobotNodeNames()).addOptions({""}).value(frameName)));
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Tracking: "))
+ .addChild(RemoteGui::makeTextLabel("Enabled"))
+ .addChild(RemoteGui::makeCheckBox("enabled").value(enabled))
+ .addChild(RemoteGui::makeTextLabel("Frame"))
+ .addChild(RemoteGui::makeComboBox("tracking_frame")
+ .options(localRobot->getRobotNodeNames())
+ .addOptions({""})
+ .value(frameName)));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Look at frame: ")).addChild(
- RemoteGui::makeComboBox("frame_look").options(localRobot->getRobotNodeNames()).value(
- frameName)).addChild(
- RemoteGui::makeButton("button_look_at_frame").label("look at")));
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Look at frame: "))
+ .addChild(RemoteGui::makeComboBox("frame_look")
+ .options(localRobot->getRobotNodeNames())
+ .value(frameName))
+ .addChild(RemoteGui::makeButton("button_look_at_frame").label("look at")));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Look at global point: ")).addChild(
- RemoteGui::makeFloatSpinBox("global_point_x").min(-1000000000).max(1000000000).steps(
- 2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeFloatSpinBox("global_point_y").min(-1000000000).max(1000000000).steps(
- 2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeFloatSpinBox("global_point_z").min(-1000000000).max(1000000000).steps(
- 2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeButton("button_look_at_global_point").label("look at")));
-
- rootLayoutBuilder.addChild(RemoteGui::makeHBoxLayout().addChild(
- RemoteGui::makeTextLabel("Look at point in robot frame: ")).addChild(
- RemoteGui::makeFloatSpinBox("robot_point_x").min(-1000000000).max(1000000000).steps(
- 2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeFloatSpinBox("robot_point_y").min(-1000000000).max(1000000000).steps(
- 2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeFloatSpinBox("robot_point_z").min(-1000000000).max(1000000000).steps(
- 2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeButton("button_look_at_robot_point").label("look at")));
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Look at global point: "))
+ .addChild(RemoteGui::makeFloatSpinBox("global_point_x")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("global_point_y")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("global_point_z")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(
+ RemoteGui::makeButton("button_look_at_global_point").label("look at")));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Scan: ")).addChild(
- RemoteGui::makeTextLabel("yaw from ")).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from").min(
- headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(
- static_cast<int>(
- (headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) /
- 0.1))).addChild(RemoteGui::makeTextLabel("yaw to ")).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to").min(
- headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(
- static_cast<int>(
- (headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) /
- 0.1))).addChild(RemoteGui::makeTextLabel("pitch ")).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch").min(
- headPitchJoint->getJointLimitLo()).max(headPitchJoint->getJointLimitHi()).steps(
- static_cast<int>(
- (headPitchJoint->getJointLimitHi() - headPitchJoint->getJointLimitLo()) /
- 0.1))).addChild(RemoteGui::makeTextLabel("velocity ")).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity").min(0).max(6).steps(
- static_cast<int>(6 / 0.1)).value(0.8f)).addChild(
- RemoteGui::makeButton("button_scan_in_configuration_space").label("scan")));
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Look at point in robot frame: "))
+ .addChild(RemoteGui::makeFloatSpinBox("robot_point_x")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("robot_point_y")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("robot_point_z")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(
+ RemoteGui::makeButton("button_look_at_robot_point").label("look at")));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Scan: ")).addChild(
- RemoteGui::makeTextLabel("from ")).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x").min(-1000000000).max(
- 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y").min(-1000000000).max(
- 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z").min(-1000000000).max(
- 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeTextLabel("to ")).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x").min(-1000000000).max(
- 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y").min(-1000000000).max(
- 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z").min(-1000000000).max(
- 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
- RemoteGui::makeTextLabel("velocity ")).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity").min(0).max(6).steps(
- static_cast<int>(6 / 0.1)).value(0.8f)).addChild(
- RemoteGui::makeTextLabel("acceleration ")).addChild(
- RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration").min(0).max(8).steps(
- static_cast<int>(8 / 0.1)).value(4.0f)).addChild(
- RemoteGui::makeButton("button_scan_in_workspace").label("scan")));
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Scan: "))
+ .addChild(RemoteGui::makeTextLabel("yaw from "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from")
+ .min(headYawJoint->getJointLimitLo())
+ .max(headYawJoint->getJointLimitHi())
+ .steps(static_cast<int>((headYawJoint->getJointLimitHi() -
+ headYawJoint->getJointLimitLo()) /
+ 0.1)))
+ .addChild(RemoteGui::makeTextLabel("yaw to "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to")
+ .min(headYawJoint->getJointLimitLo())
+ .max(headYawJoint->getJointLimitHi())
+ .steps(static_cast<int>((headYawJoint->getJointLimitHi() -
+ headYawJoint->getJointLimitLo()) /
+ 0.1)))
+ .addChild(RemoteGui::makeTextLabel("pitch "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch")
+ .min(headPitchJoint->getJointLimitLo())
+ .max(headPitchJoint->getJointLimitHi())
+ .steps(static_cast<int>((headPitchJoint->getJointLimitHi() -
+ headPitchJoint->getJointLimitLo()) /
+ 0.1)))
+ .addChild(RemoteGui::makeTextLabel("velocity "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity")
+ .min(0)
+ .max(6)
+ .steps(static_cast<int>(6 / 0.1))
+ .value(0.8f))
+ .addChild(
+ RemoteGui::makeButton("button_scan_in_configuration_space").label("scan")));
+
+ rootLayoutBuilder.addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(RemoteGui::makeTextLabel("Scan: "))
+ .addChild(RemoteGui::makeTextLabel("from "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(RemoteGui::makeTextLabel("to "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z")
+ .min(-1000000000)
+ .max(1000000000)
+ .steps(2 * 1000000000 / 10)
+ .value(0.f))
+ .addChild(RemoteGui::makeTextLabel("velocity "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity")
+ .min(0)
+ .max(6)
+ .steps(static_cast<int>(6 / 0.1))
+ .value(0.8f))
+ .addChild(RemoteGui::makeTextLabel("acceleration "))
+ .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration")
+ .min(0)
+ .max(8)
+ .steps(static_cast<int>(8 / 0.1))
+ .value(4.0f))
+ .addChild(RemoteGui::makeButton("button_scan_in_workspace").label("scan")));
rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
- _guiTask = new SimplePeriodicTask<>([this]()
- {
- bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
- std::string oldFrameGui = _guiTab.getValue<std::string>(
- "tracking_frame").get();
-
- _guiTab.receiveUpdates();
-
- if (oldEnabledGui == enabled)
- {
- // only apply changes of gui if not already changed by ice
- _enableTracking(_guiTab.getValue<bool>("enabled").get());
- }
- _guiTab.getValue<bool>("enabled").set(enabled);
-
- if (oldFrameGui == frameName && oldFrameGui !=
- _guiTab.getValue<std::string>(
- "tracking_frame").get())
- {
- // only apply changes of gui if not already changed by ice
- setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
- }
- _guiTab.getValue<std::string>("tracking_frame").set(frameName);
-
- _guiTab.sendUpdates();
-
- if (_guiTab.getButton("button_look_at_frame").clicked())
- {
- lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
- }
-
- if (_guiTab.getButton("button_look_at_global_point").clicked())
- {
- lookAtPointInGlobalFrame(Vector3f{
- _guiTab.getValue<float>("global_point_x").get(),
- _guiTab.getValue<float>("global_point_y").get(),
- _guiTab.getValue<float>("global_point_z").get()});
- }
-
- if (_guiTab.getButton("button_look_at_robot_point").clicked())
- {
- lookAtPointInRobotFrame(
- Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
- _guiTab.getValue<float>("robot_point_y").get(),
- _guiTab.getValue<float>(
- "robot_point_z").get()});
- }
-
- if (_guiTab.getButton(
- "button_scan_in_configuration_space").clicked())
- {
- scanInConfigurationSpace(_guiTab.getValue<float>(
- "scan_in_configuration_space_yaw_from").get(),
- _guiTab.getValue<float>(
- "scan_in_configuration_space_yaw_to").get(),
- {_guiTab.getValue<float>(
- "scan_in_configuration_space_pitch").get()},
- _guiTab.getValue<float>(
- "scan_in_configuration_space_velocity").get());
- }
-
- if (_guiTab.getButton("button_scan_in_workspace").clicked())
- {
- scanInWorkspace({{_guiTab.getValue<float>(
- "scan_in_workspace_from_x").get(), _guiTab.getValue<
- float>(
- "scan_in_workspace_from_y").get(), _guiTab.getValue<
- float>("scan_in_workspace_from_z").get()},
- {_guiTab.getValue<float>(
- "scan_in_workspace_to_x").get(), _guiTab.getValue<
- float>(
- "scan_in_workspace_to_y").get(), _guiTab.getValue<
- float>(
- "scan_in_workspace_to_z").get()}},
- _guiTab.getValue<float>(
- "scan_in_workspace_velocity").get(),
- _guiTab.getValue<float>(
- "scan_in_workspace_acceleration").get());
- }
- }, 10);
+ _guiTask = new SimplePeriodicTask<>(
+ [this]()
+ {
+ bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
+ std::string oldFrameGui = _guiTab.getValue<std::string>("tracking_frame").get();
+
+ _guiTab.receiveUpdates();
+
+ if (oldEnabledGui == enabled)
+ {
+ // only apply changes of gui if not already changed by ice
+ _enableTracking(_guiTab.getValue<bool>("enabled").get());
+ }
+ _guiTab.getValue<bool>("enabled").set(enabled);
+
+ if (oldFrameGui == frameName &&
+ oldFrameGui != _guiTab.getValue<std::string>("tracking_frame").get())
+ {
+ // only apply changes of gui if not already changed by ice
+ setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
+ }
+ _guiTab.getValue<std::string>("tracking_frame").set(frameName);
+
+ _guiTab.sendUpdates();
+
+ if (_guiTab.getButton("button_look_at_frame").clicked())
+ {
+ lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
+ }
+
+ if (_guiTab.getButton("button_look_at_global_point").clicked())
+ {
+ lookAtPointInGlobalFrame(
+ Vector3f{_guiTab.getValue<float>("global_point_x").get(),
+ _guiTab.getValue<float>("global_point_y").get(),
+ _guiTab.getValue<float>("global_point_z").get()});
+ }
+
+ if (_guiTab.getButton("button_look_at_robot_point").clicked())
+ {
+ lookAtPointInRobotFrame(
+ Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
+ _guiTab.getValue<float>("robot_point_y").get(),
+ _guiTab.getValue<float>("robot_point_z").get()});
+ }
+
+ if (_guiTab.getButton("button_scan_in_configuration_space").clicked())
+ {
+ scanInConfigurationSpace(
+ _guiTab.getValue<float>("scan_in_configuration_space_yaw_from").get(),
+ _guiTab.getValue<float>("scan_in_configuration_space_yaw_to").get(),
+ {_guiTab.getValue<float>("scan_in_configuration_space_pitch").get()},
+ _guiTab.getValue<float>("scan_in_configuration_space_velocity").get());
+ }
+
+ if (_guiTab.getButton("button_scan_in_workspace").clicked())
+ {
+ scanInWorkspace(
+ {{_guiTab.getValue<float>("scan_in_workspace_from_x").get(),
+ _guiTab.getValue<float>("scan_in_workspace_from_y").get(),
+ _guiTab.getValue<float>("scan_in_workspace_from_z").get()},
+ {_guiTab.getValue<float>("scan_in_workspace_to_x").get(),
+ _guiTab.getValue<float>("scan_in_workspace_to_y").get(),
+ _guiTab.getValue<float>("scan_in_workspace_to_z").get()}},
+ _guiTab.getValue<float>("scan_in_workspace_velocity").get(),
+ _guiTab.getValue<float>("scan_in_workspace_acceleration").get());
+ }
+ },
+ 10);
RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
@@ -265,8 +325,8 @@ namespace armarx
}
}
-
- void FrameTracking::onDisconnectComponent()
+ void
+ FrameTracking::onDisconnectComponent()
{
_enableTracking(false);
if (_guiTask)
@@ -276,23 +336,26 @@ namespace armarx
}
}
-
- void FrameTracking::onExitComponent()
+ void
+ FrameTracking::onExitComponent()
{
-
}
- armarx::PropertyDefinitionsPtr FrameTracking::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ FrameTracking::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new FrameTrackingPropertyDefinitions(getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new FrameTrackingPropertyDefinitions(getConfigIdentifier()));
}
- void FrameTracking::enableTracking(bool enable, const Ice::Current&)
+ void
+ FrameTracking::enableTracking(bool enable, const Ice::Current&)
{
_enableTracking(enable);
}
- void FrameTracking::setFrame(const std::string& frameName, const Ice::Current&)
+ void
+ FrameTracking::setFrame(const std::string& frameName, const Ice::Current&)
{
if (enabled)
{
@@ -302,12 +365,14 @@ namespace armarx
this->frameName = frameName;
}
- std::string FrameTracking::getFrame(const Ice::Current&) const
+ std::string
+ FrameTracking::getFrame(const Ice::Current&) const
{
return frameName;
}
- void FrameTracking::lookAtFrame(const std::string& frameName, const Ice::Current&)
+ void
+ FrameTracking::lookAtFrame(const std::string& frameName, const Ice::Current&)
{
if (enabled)
{
@@ -323,7 +388,8 @@ namespace armarx
_lookAtFrame(frameName);
}
- void FrameTracking::lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current&)
+ void
+ FrameTracking::lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current&)
{
if (enabled)
{
@@ -334,7 +400,10 @@ namespace armarx
_lookAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)));
}
- bool FrameTracking::isLookingAtPointInGlobalFrame(const Vector3f& point, float max_diff, const Ice::Current&)
+ bool
+ FrameTracking::isLookingAtPointInGlobalFrame(const Vector3f& point,
+ float max_diff,
+ const Ice::Current&)
{
if (enabled)
{
@@ -345,7 +414,8 @@ namespace armarx
return _looksAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)), max_diff);
}
- void FrameTracking::lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current&)
+ void
+ FrameTracking::lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current&)
{
if (enabled)
{
@@ -356,17 +426,20 @@ namespace armarx
_lookAtPoint(ToEigen(point));
}
- void FrameTracking::moveJointsTo(float yaw, float pitch, const Ice::Current&)
+ void
+ FrameTracking::moveJointsTo(float yaw, float pitch, const Ice::Current&)
{
const float currentYaw = headYawJoint->getJointValue();
const float currentPitch = headPitchJoint->getJointValue();
- const float currentYawVel = DatafieldRefPtr::dynamicCast(
- kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
- headYawJoint->getName()))->getFloat();
- const float currentPitchVel = DatafieldRefPtr::dynamicCast(
- kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
- headPitchJoint->getName()))->getFloat();
+ const float currentYawVel =
+ DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
+ "jointvelocities", headYawJoint->getName()))
+ ->getFloat();
+ const float currentPitchVel =
+ DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
+ "jointvelocities", headPitchJoint->getName()))
+ ->getFloat();
FrameTracking::HeadState headState;
headState.currentYawPos = currentYaw;
@@ -382,19 +455,24 @@ namespace armarx
while (std::abs(headYawJoint->getJointValue() - yaw) > static_cast<float>(M_PI / 180.) ||
std::abs(headPitchJoint->getJointValue() - pitch) > static_cast<float>(M_PI / 180.))
{
- ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw << " pitch: "
- << headPitchJoint->getJointValue() << " -> " << pitch;
+ ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw
+ << " pitch: " << headPitchJoint->getJointValue() << " -> " << pitch;
syncronizeLocalClone();
// sleep for 30 milliseconds
nanosleep(&req, nullptr);
}
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
- {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->switchControlMode(
+ {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- void FrameTracking::scanInConfigurationSpace(float yawFrom, float yawTo, const Ice::FloatSeq& pitchValues,
- float velocity, const Ice::Current&)
+ void
+ FrameTracking::scanInConfigurationSpace(float yawFrom,
+ float yawTo,
+ const Ice::FloatSeq& pitchValues,
+ float velocity,
+ const Ice::Current&)
{
if (enabled)
{
@@ -405,15 +483,16 @@ namespace armarx
syncronizeLocalClone();
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl},
- {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ kinematicUnitInterfacePrx->switchControlMode(
+ {{headYawJoint->getName(), ControlMode::eVelocityControl},
+ {headPitchJoint->getName(), ControlMode::eVelocityControl}});
// to initial yaw
{
bool wasGreater = headYawJoint->getJointValue() > yawFrom;
float yawVelocityToInit = wasGreater ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), yawVelocityToInit},
- {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->setJointVelocities(
+ {{headYawJoint->getName(), yawVelocityToInit}, {headPitchJoint->getName(), 0.f}});
// if the joint angle was greater before we want to run as long as it is greater
// otherwise we want to run as long as it is smaler
while ((wasGreater && headYawJoint->getJointValue() > yawFrom) ||
@@ -423,13 +502,13 @@ namespace armarx
}
}
- for (const auto& p: pitchValues)
+ for (const auto& p : pitchValues)
{
// to pitch value
bool wasGreaterP = headPitchJoint->getJointValue() > p;
float velocityPitch = wasGreaterP ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
- {headPitchJoint->getName(), velocityPitch}});
+ kinematicUnitInterfacePrx->setJointVelocities(
+ {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), velocityPitch}});
while ((wasGreaterP && headPitchJoint->getJointValue() > p) ||
(!wasGreaterP && headPitchJoint->getJointValue() < p))
{
@@ -439,8 +518,8 @@ namespace armarx
// to yaw value
bool wasGreaterY = yawFrom > yawTo; // yawFrom == headYawJoint->getJointValue()
float velocityYaw = wasGreaterY ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), velocityYaw},
- {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->setJointVelocities(
+ {{headYawJoint->getName(), velocityYaw}, {headPitchJoint->getName(), 0.f}});
while ((wasGreaterY && headYawJoint->getJointValue() > yawTo) ||
(!wasGreaterY && headYawJoint->getJointValue() < yawTo))
{
@@ -449,14 +528,18 @@ namespace armarx
std::swap(yawFrom, yawTo);
}
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
- {headPitchJoint->getName(), 0.f}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
- {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->setJointVelocities(
+ {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->switchControlMode(
+ {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- void FrameTracking::scanInWorkspace(const Vector3fSeq& points, float maxVelocity, float acceleration,
- const Ice::Current&)
+ void
+ FrameTracking::scanInWorkspace(const Vector3fSeq& points,
+ float maxVelocity,
+ float acceleration,
+ const Ice::Current&)
{
if (enabled)
{
@@ -465,18 +548,22 @@ namespace armarx
}
syncronizeLocalClone();
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl},
- {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ kinematicUnitInterfacePrx->switchControlMode(
+ {{headYawJoint->getName(), ControlMode::eVelocityControl},
+ {headPitchJoint->getName(), ControlMode::eVelocityControl}});
struct timespec req = {0, 30 * 1000000L};
- for (const auto& p: points)
+ for (const auto& p : points)
{
auto pEigen = localRobot->toLocalCoordinateSystemVec(ToEigen(p));
auto target = _calculateJointAngles(pEigen);
- while (std::abs(target.currentYawPos - target.desiredYawPos) > static_cast<float>(M_PI / 180.) ||
- std::abs(target.currentPitchPos - target.desiredPitchPos) > static_cast<float>(M_PI / 180.))
+ while (std::abs(target.currentYawPos - target.desiredYawPos) >
+ static_cast<float>(M_PI / 180.) ||
+ std::abs(target.currentPitchPos - target.desiredPitchPos) >
+ static_cast<float>(M_PI / 180.))
{
- ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos << " pitch: "
- << target.currentPitchPos << " - " << target.desiredPitchPos;
+ ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos
+ << " pitch: " << target.currentPitchPos << " - "
+ << target.desiredPitchPos;
syncronizeLocalClone();
target = _calculateJointAngles(pEigen);
_doVelocityControl(target, maxVelocity, acceleration, maxVelocity, acceleration);
@@ -484,46 +571,52 @@ namespace armarx
nanosleep(&req, nullptr);
}
}
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
- {headPitchJoint->getName(), 0.f}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
- {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->setJointVelocities(
+ {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->switchControlMode(
+ {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- void FrameTracking::process()
+ void
+ FrameTracking::process()
{
if (!localRobot->hasRobotNode(frameName))
{
ARMARX_ERROR << frameName << " does not exist. Task will be disabled.";
- std::thread([this]()
- {
- _enableTracking(false);
- }).detach();
+ std::thread([this]() { _enableTracking(false); }).detach();
return;
}
syncronizeLocalClone();
- _doVelocityControl(_calculateJointAnglesContinously(frameName), maxYawVelocity, yawAcceleration,
- maxPitchVelocity, pitchAcceleration);
+ _doVelocityControl(_calculateJointAnglesContinously(frameName),
+ maxYawVelocity,
+ yawAcceleration,
+ maxPitchVelocity,
+ pitchAcceleration);
}
- void FrameTracking::syncronizeLocalClone()
+ void
+ FrameTracking::syncronizeLocalClone()
{
armarx::RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponent);
}
- void FrameTracking::_lookAtFrame(const std::string& frameName)
+ void
+ FrameTracking::_lookAtFrame(const std::string& frameName)
{
auto frame = localRobot->getRobotNode(frameName);
auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
_lookAtPoint(posInRobotFrame);
}
- void FrameTracking::_lookAtPoint(const Eigen::Vector3f& point)
+ void
+ FrameTracking::_lookAtPoint(const Eigen::Vector3f& point)
{
_doPositionControl(_calculateJointAngles(point));
}
- bool FrameTracking::_looksAtPoint(const Eigen::Vector3f& point, float max_diff)
+ bool
+ FrameTracking::_looksAtPoint(const Eigen::Vector3f& point, float max_diff)
{
auto head_state = _calculateJointAngles(point);
float diff = std::abs(head_state.desiredPitchPos - head_state.currentPitchPos) +
@@ -531,20 +624,23 @@ namespace armarx
return max_diff > diff;
}
- FrameTracking::HeadState FrameTracking::_calculateJointAnglesContinously(const std::string& frameName)
+ FrameTracking::HeadState
+ FrameTracking::_calculateJointAnglesContinously(const std::string& frameName)
{
auto frame = localRobot->getRobotNode(frameName);
auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
// do nothing if the robot works above his head
// he should already look upwards because if this component runs continously
- if (std::sqrt(posInRobotFrame.x() * posInRobotFrame.x() + posInRobotFrame.y() * posInRobotFrame.y()) < 300.f)
+ if (std::sqrt(posInRobotFrame.x() * posInRobotFrame.x() +
+ posInRobotFrame.y() * posInRobotFrame.y()) < 300.f)
{
return FrameTracking::HeadState{true, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
}
return _calculateJointAngles(posInRobotFrame);
}
- FrameTracking::HeadState FrameTracking::_calculateJointAngles(const Eigen::Vector3f& point)
+ FrameTracking::HeadState
+ FrameTracking::_calculateJointAngles(const Eigen::Vector3f& point)
{
float yaw = -std::atan2(point.x(), point.y());
// make shure the joint value satisfies the joint limits
@@ -552,18 +648,19 @@ namespace armarx
yaw = std::min(headYawJoint->getJointLimitHi(), yaw);
// we dont want the robot to move from one limit to the other in one step
const float currentYaw = headYawJoint->getJointValue();
- if (!headYawJoint->isLimitless() && std::abs(currentYaw - yaw) >
- headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo() -
- static_cast<float>(M_PI) / 8)
+ if (!headYawJoint->isLimitless() &&
+ std::abs(currentYaw - yaw) > headYawJoint->getJointLimitHi() -
+ headYawJoint->getJointLimitLo() -
+ static_cast<float>(M_PI) / 8)
{
yaw = currentYaw;
}
const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(
- localRobot->toGlobalCoordinateSystemVec(point));
+ localRobot->toGlobalCoordinateSystemVec(point));
const Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z()};
- const float headHeightRealativeToPitchJoint = headPitchJoint->toLocalCoordinateSystemVec(
- cameraNode->getGlobalPosition()).z();
+ const float headHeightRealativeToPitchJoint =
+ headPitchJoint->toLocalCoordinateSystemVec(cameraNode->getGlobalPosition()).z();
float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) +
std::asin(headHeightRealativeToPitchJoint / pj.norm());
// make shure the joint value satisfies the joint limits
@@ -571,15 +668,17 @@ namespace armarx
pitch = std::min(headPitchJoint->getJointLimitHi(), pitch);
const float currentPitch = headPitchJoint->getJointValue();
- ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point << " using yaw=" << yaw
- << " and pitch=" << pitch;
+ ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point
+ << " using yaw=" << yaw << " and pitch=" << pitch;
- const float currentYawVel = DatafieldRefPtr::dynamicCast(
- kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
- headYawJoint->getName()))->getFloat();
- const float currentPitchVel = DatafieldRefPtr::dynamicCast(
- kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
- headPitchJoint->getName()))->getFloat();
+ const float currentYawVel =
+ DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
+ "jointvelocities", headYawJoint->getName()))
+ ->getFloat();
+ const float currentPitchVel =
+ DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName(
+ "jointvelocities", headPitchJoint->getName()))
+ ->getFloat();
FrameTracking::HeadState headState;
headState.currentYawPos = currentYaw;
@@ -591,52 +690,68 @@ namespace armarx
return headState;
}
- void FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState, float maxYawVelocity,
- float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
+ void
+ FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState,
+ float maxYawVelocity,
+ float yawAcceleration,
+ float maxPitchVelocity,
+ float pitchAcceleration)
{
if (headState.stop)
{
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
- {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->setJointVelocities(
+ {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
return;
}
- float desiredYawVelocity = positionThroughVelocityControlWithAccelerationBounds(30.f / 1000, 35.f / 1000,
- headState.currentYawVel,
- maxYawVelocity, yawAcceleration,
- yawAcceleration,
- headState.currentYawPos,
- headState.desiredYawPos, 1.f);
- float desiredPitchVelocity = positionThroughVelocityControlWithAccelerationBounds(30.f / 1000, 35.f / 1000,
- headState.currentPitchVel,
- maxPitchVelocity,
- pitchAcceleration,
- pitchAcceleration,
- headState.currentPitchPos,
- headState.desiredPitchPos,
- 1.f);
+ float desiredYawVelocity =
+ positionThroughVelocityControlWithAccelerationBounds(30.f / 1000,
+ 35.f / 1000,
+ headState.currentYawVel,
+ maxYawVelocity,
+ yawAcceleration,
+ yawAcceleration,
+ headState.currentYawPos,
+ headState.desiredYawPos,
+ 1.f);
+ float desiredPitchVelocity =
+ positionThroughVelocityControlWithAccelerationBounds(30.f / 1000,
+ 35.f / 1000,
+ headState.currentPitchVel,
+ maxPitchVelocity,
+ pitchAcceleration,
+ pitchAcceleration,
+ headState.currentPitchPos,
+ headState.desiredPitchPos,
+ 1.f);
// control mode is set when enable task
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), desiredYawVelocity},
- {headPitchJoint->getName(), desiredPitchVelocity}});
+ kinematicUnitInterfacePrx->setJointVelocities(
+ {{headYawJoint->getName(), desiredYawVelocity},
+ {headPitchJoint->getName(), desiredPitchVelocity}});
}
- void FrameTracking::_doPositionControl(const FrameTracking::HeadState& headState)
+ void
+ FrameTracking::_doPositionControl(const FrameTracking::HeadState& headState)
{
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::ePositionControl},
- {headPitchJoint->getName(), ControlMode::ePositionControl}});
+ kinematicUnitInterfacePrx->switchControlMode(
+ {{headYawJoint->getName(), ControlMode::ePositionControl},
+ {headPitchJoint->getName(), ControlMode::ePositionControl}});
if (headState.stop)
{
return;
}
- kinematicUnitInterfacePrx->setJointAngles({{headYawJoint->getName(), headState.desiredYawPos},
- {headPitchJoint->getName(), headState.desiredPitchPos}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
- {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->setJointAngles(
+ {{headYawJoint->getName(), headState.desiredYawPos},
+ {headPitchJoint->getName(), headState.desiredPitchPos}});
+ kinematicUnitInterfacePrx->switchControlMode(
+ {{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- void FrameTracking::_enableTracking(bool enable)
+ void
+ FrameTracking::_enableTracking(bool enable)
{
if (this->enabled == enable)
{
@@ -645,14 +760,16 @@ namespace armarx
this->enabled = enable;
if (enable)
{
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl},
- {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ kinematicUnitInterfacePrx->switchControlMode(
+ {{headYawJoint->getName(), ControlMode::eVelocityControl},
+ {headPitchJoint->getName(), ControlMode::eVelocityControl}});
processorTask->start();
- } else
+ }
+ else
{
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
- {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->setJointVelocities(
+ {{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
processorTask->stop();
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.h b/source/RobotAPI/components/FrameTracking/FrameTracking.h
index 1ba272f3b92c02ccba92bab83a63fa2d6496f9a5..58814f4554e756434230ed5eb903987836f0a0f2 100644
--- a/source/RobotAPI/components/FrameTracking/FrameTracking.h
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.h
@@ -23,21 +23,21 @@
#pragma once
+#include <Eigen/Core>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
+
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
#include <RobotAPI/interface/components/FrameTrackingInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
-
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-
-#include <Eigen/Core>
namespace armarx
{
@@ -45,26 +45,54 @@ namespace armarx
* @class FrameTrackingPropertyDefinitions
* @brief
*/
- class FrameTrackingPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class FrameTrackingPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- FrameTrackingPropertyDefinitions(std::string prefix):
+ FrameTrackingPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
- defineOptionalProperty<std::string>("KinematicUnitName", "KinematicUnit", "Name of the kinematic unit that should be used");
- defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer that should be used");
- defineRequiredProperty<std::string>("HeadYawJoint", "Name of the yaw joint of the head.");
- defineRequiredProperty<std::string>("HeadPitchJoint", "Name of the pitch joint of the head.");
+ defineOptionalProperty<std::string>(
+ "RobotStateComponentName",
+ "RobotStateComponent",
+ "Name of the robot state component that should be used");
+ defineOptionalProperty<std::string>("KinematicUnitName",
+ "KinematicUnit",
+ "Name of the kinematic unit that should be used");
+ defineOptionalProperty<std::string>(
+ "KinematicUnitObserverName",
+ "KinematicUnitObserver",
+ "Name of the kinematic unit observer that should be used");
+ defineRequiredProperty<std::string>("HeadYawJoint",
+ "Name of the yaw joint of the head.");
+ defineRequiredProperty<std::string>("HeadPitchJoint",
+ "Name of the pitch joint of the head.");
defineRequiredProperty<std::string>("CameraNode", "Name of the camera node.");
- defineOptionalProperty<bool>("EnableTrackingOnStartup", true, "Start Tracking on startup. This needs a frame to be defined.");
- defineOptionalProperty<std::string>("FrameOnStartup", "", "Name of the frame that should be tracked.");
- defineOptionalProperty<std::string>("RemoteGuiName", "RemoteGuiProvider", "Name of the remote gui. Remote gui is disabled if empty.");
- defineOptionalProperty<float>("MaxYawVelocity", 0.8f, "The maximum velocity the yaw joint while tracking objects.");
- defineOptionalProperty<float>("YawAcceleration", 4.f, "The acceleration of the yaw joint while tracking objects.");
- defineOptionalProperty<float>("MaxPitchVelocity", 0.8f, "The maximum velocity the pitch joint while tracking objects.");
- defineOptionalProperty<float>("PitchAcceleration", 4.f, "The acceleration of the pitch joint while tracking objects.");
+ defineOptionalProperty<bool>(
+ "EnableTrackingOnStartup",
+ true,
+ "Start Tracking on startup. This needs a frame to be defined.");
+ defineOptionalProperty<std::string>(
+ "FrameOnStartup", "", "Name of the frame that should be tracked.");
+ defineOptionalProperty<std::string>(
+ "RemoteGuiName",
+ "RemoteGuiProvider",
+ "Name of the remote gui. Remote gui is disabled if empty.");
+ defineOptionalProperty<float>(
+ "MaxYawVelocity",
+ 0.8f,
+ "The maximum velocity the yaw joint while tracking objects.");
+ defineOptionalProperty<float>(
+ "YawAcceleration",
+ 4.f,
+ "The acceleration of the yaw joint while tracking objects.");
+ defineOptionalProperty<float>(
+ "MaxPitchVelocity",
+ 0.8f,
+ "The maximum velocity the pitch joint while tracking objects.");
+ defineOptionalProperty<float>(
+ "PitchAcceleration",
+ 4.f,
+ "The acceleration of the pitch joint while tracking objects.");
}
};
@@ -79,24 +107,29 @@ namespace armarx
*
* Detailed description of class FrameTracking.
*/
- class FrameTracking :
- virtual public armarx::Component,
- public FrameTrackingInterface
+ class FrameTracking : virtual public armarx::Component, public FrameTrackingInterface
{
private:
struct HeadState
{
HeadState() = default;
- HeadState(bool stop, float currentYawPos, float currentYawVel, float desiredYawPos, float currentPitchPos, float currentPitchVel, float desiredPitchPos)
- :
- stop {stop},
- currentYawPos {currentYawPos },
- currentYawVel {currentYawVel },
- desiredYawPos {desiredYawPos },
- currentPitchPos {currentPitchPos},
- currentPitchVel {currentPitchVel},
- desiredPitchPos {desiredPitchPos}
- {}
+
+ HeadState(bool stop,
+ float currentYawPos,
+ float currentYawVel,
+ float desiredYawPos,
+ float currentPitchPos,
+ float currentPitchVel,
+ float desiredPitchPos) :
+ stop{stop},
+ currentYawPos{currentYawPos},
+ currentYawVel{currentYawVel},
+ desiredYawPos{desiredYawPos},
+ currentPitchPos{currentPitchPos},
+ currentPitchVel{currentPitchVel},
+ desiredPitchPos{desiredPitchPos}
+ {
+ }
bool stop = false;
float currentYawPos;
@@ -106,11 +139,13 @@ namespace armarx
float currentPitchVel;
float desiredPitchPos;
};
+
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "FrameTracking";
}
@@ -144,16 +179,29 @@ namespace armarx
// FrameTrackingInterface interface
public:
void enableTracking(bool enable, const Ice::Current& = Ice::emptyCurrent) override;
- void setFrame(const std::string& frameName, const Ice::Current& = Ice::emptyCurrent) override;
+ void setFrame(const std::string& frameName,
+ const Ice::Current& = Ice::emptyCurrent) override;
std::string getFrame(const Ice::Current& = Ice::emptyCurrent) const override;
- void lookAtFrame(const std::string& frameName, const Ice::Current& = Ice::emptyCurrent) override;
- void lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current& = Ice::emptyCurrent) override;
- bool isLookingAtPointInGlobalFrame(const Vector3f& point, float max_diff, const Ice::Current& = Ice::emptyCurrent) override;
- void lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current& = Ice::emptyCurrent) override;
-
- void scanInConfigurationSpace(float yawFrom, float yawTo, const ::Ice::FloatSeq& pitchValues, float velocity, const Ice::Current& = Ice::emptyCurrent) override;
- void scanInWorkspace(const ::armarx::Vector3fSeq& points, float maxVelocity, float acceleration, const Ice::Current& = Ice::emptyCurrent) override;
+ void lookAtFrame(const std::string& frameName,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void lookAtPointInGlobalFrame(const Vector3f& point,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ bool isLookingAtPointInGlobalFrame(const Vector3f& point,
+ float max_diff,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void lookAtPointInRobotFrame(const Vector3f& point,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void scanInConfigurationSpace(float yawFrom,
+ float yawTo,
+ const ::Ice::FloatSeq& pitchValues,
+ float velocity,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void scanInWorkspace(const ::armarx::Vector3fSeq& points,
+ float maxVelocity,
+ float acceleration,
+ const Ice::Current& = Ice::emptyCurrent) override;
void moveJointsTo(float yaw, float pitch, const Ice::Current& = Ice::emptyCurrent) override;
protected:
@@ -165,12 +213,16 @@ namespace armarx
bool _looksAtPoint(const Eigen::Vector3f& point, float max_diff);
HeadState _calculateJointAnglesContinously(const std::string& frame);
HeadState _calculateJointAngles(const Eigen::Vector3f& point);
- void _doVelocityControl(const HeadState& headstate, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration);
+ void _doVelocityControl(const HeadState& headstate,
+ float maxYawVelocity,
+ float yawAcceleration,
+ float maxPitchVelocity,
+ float pitchAcceleration);
void _doPositionControl(const HeadState& headstate);
void _enableTracking(bool enable);
RobotStateComponentInterfacePrx robotStateComponent;
- KinematicUnitInterfacePrx kinematicUnitInterfacePrx;// send commands to kinematic unit
+ KinematicUnitInterfacePrx kinematicUnitInterfacePrx; // send commands to kinematic unit
PeriodicTask<FrameTracking>::pointer_type processorTask;
KinematicUnitObserverInterfacePrx kinematicUnitObserverInterfacePrx;
@@ -186,8 +238,8 @@ namespace armarx
VirtualRobot::RobotNodePtr headPitchJoint;
VirtualRobot::RobotNodePtr cameraNode;
- RemoteGuiInterfacePrx _remoteGui;
- SimplePeriodicTask<>::pointer_type _guiTask;
- RemoteGui::TabProxy _guiTab;
+ RemoteGuiInterfacePrx _remoteGui;
+ SimplePeriodicTask<>::pointer_type _guiTask;
+ RemoteGui::TabProxy _guiTab;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/FrameTracking/test/FrameTrackingTest.cpp b/source/RobotAPI/components/FrameTracking/test/FrameTrackingTest.cpp
index d7fbb0b3e6768337d4dd9084960f84e3f981f9ab..67f432a4081fcb2350ee8d29abd5d2195feca1a0 100644
--- a/source/RobotAPI/components/FrameTracking/test/FrameTrackingTest.cpp
+++ b/source/RobotAPI/components/FrameTracking/test/FrameTrackingTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/FrameTracking/FrameTracking.h>
#include <iostream>
+#include <RobotAPI/components/FrameTracking/FrameTracking.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::FrameTracking instance;
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
index 5113961eeebb95c1af593714fbb1eca93ea84909..986349d1205e484a2f3984420e7a018406d8cd2a 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
@@ -55,9 +55,9 @@ namespace armarx
if (enableHeartBeat)
{
this->heartbeatPlugin->signUp(armarx::core::time::Duration::MilliSeconds(1000),
- armarx::core::time::Duration::MilliSeconds(1500),
- {"Gamepad"},
- "The GamepadControlUnit");
+ armarx::core::time::Duration::MilliSeconds(1500),
+ {"Gamepad"},
+ "The GamepadControlUnit");
}
}
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
index 9438f305fe6a96648eb4d6011112ea637a245369..77c54532de2a9d77193f7bd6b629afb70c86b131 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
@@ -26,13 +26,11 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/interface/units/GamepadUnit.h>
-
#include <RobotAPI/interface/units/HandUnitInterface.h>
-
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
-#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
namespace armarx
{
@@ -40,20 +38,22 @@ namespace armarx
* @class GamepadControlUnitPropertyDefinitions
* @brief
*/
- class GamepadControlUnitPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class GamepadControlUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- GamepadControlUnitPropertyDefinitions(std::string prefix):
+ GamepadControlUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
- defineOptionalProperty<std::string>("PlatformUnitName", "Armar6IcePlatformUnit", "Name of the platform unit to use");
- defineOptionalProperty<std::string>("GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
+ defineOptionalProperty<std::string>(
+ "PlatformUnitName", "Armar6IcePlatformUnit", "Name of the platform unit to use");
+ defineOptionalProperty<std::string>(
+ "GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
defineOptionalProperty<float>("ScaleX", 1000.0f, "scaling factor in mm per second");
defineOptionalProperty<float>("ScaleY", 1000.0f, "scaling factor in mm per second");
- defineOptionalProperty<float>("ScaleAngle", 1.0f, "scaling factor in radian per second");
+ defineOptionalProperty<float>(
+ "ScaleAngle", 1.0f, "scaling factor in radian per second");
}
};
@@ -68,17 +68,16 @@ namespace armarx
*
* Detailed description of class GamepadControlUnit.
*/
- class GamepadControlUnit :
- virtual public armarx::Component,
- virtual public GamepadUnitListener
+ class GamepadControlUnit : virtual public armarx::Component, virtual public GamepadUnitListener
{
public:
GamepadControlUnit();
-
+
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "GamepadControlUnit";
}
@@ -115,22 +114,23 @@ namespace armarx
plugins::HeartbeatComponentPlugin* heartbeatPlugin = nullptr;
-
bool enableHeartBeat = false;
float scaleX;
float scaleY;
float scaleRotation;
EmergencyStopMasterInterfacePrx emergencyStop;
- bool leftHandOpen = true;
- bool rightHandOpen = true;
+ bool leftHandOpen = true;
+ bool rightHandOpen = true;
- long leftHandTime = 0;
- long rightHandTime = 0;
+ long leftHandTime = 0;
+ long rightHandTime = 0;
public:
- void reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data,
- const TimestampBasePtr& timestamp, const Ice::Current& c) override;
-
+ void reportGamepadState(const std::string& device,
+ const std::string& name,
+ const GamepadData& data,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c) override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/GamepadControlUnit/test/GamepadControlUnitTest.cpp b/source/RobotAPI/components/GamepadControlUnit/test/GamepadControlUnitTest.cpp
index c5a6caa4e4d4cecf3f707750164a6a0f9826e025..0b134232db72b7c22b9db06509f2c685d5113361 100644
--- a/source/RobotAPI/components/GamepadControlUnit/test/GamepadControlUnitTest.cpp
+++ b/source/RobotAPI/components/GamepadControlUnit/test/GamepadControlUnitTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h>
#include <iostream>
+#include <RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::GamepadControlUnit instance;
diff --git a/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp b/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp
index fb4cb1d94baf63e8013099a443d18890fb5c6352..2480853851e620aba296a0f5614e49103b6b61f8 100644
--- a/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp
+++ b/source/RobotAPI/components/KITHandUnit/KITHandUnit.cpp
@@ -21,11 +21,13 @@
*/
#include "KITHandUnit.h"
-#include <ArmarXCore/core/ArmarXManager.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <algorithm>
+#include <ArmarXCore/core/ArmarXManager.h>
+
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+
using namespace KITHand;
namespace armarx
@@ -33,22 +35,33 @@ namespace armarx
KITHandUnitPropertyDefinitions::KITHandUnitPropertyDefinitions(std::string prefix) :
armarx::HandUnitPropertyDefinitions(prefix)
{
- defineOptionalProperty<KITHandCommunicationDriver::ScanMode>("ConnectionType", KITHandCommunicationDriver::ScanMode::Bluetooth, "Type of the connection to the hand, either Bluetooth or Serial")
- .map("Bluetooth", KITHandCommunicationDriver::ScanMode::Bluetooth)
- .map("Serial", KITHandCommunicationDriver::ScanMode::Serial);
- defineRequiredProperty<std::string>("MAC-Address", "MAC-Address of the hand to connect to.");
- defineOptionalProperty<bool>("AutomaticReconnectActive", false, "Whether the hand unit should try to reconnect after the connection is lost.");
- defineOptionalProperty<bool>("ScanUntilHandFound", true, "Wheather to keep scanning until the hand with the given MAC-Address is found.");
+ defineOptionalProperty<KITHandCommunicationDriver::ScanMode>(
+ "ConnectionType",
+ KITHandCommunicationDriver::ScanMode::Bluetooth,
+ "Type of the connection to the hand, either Bluetooth or Serial")
+ .map("Bluetooth", KITHandCommunicationDriver::ScanMode::Bluetooth)
+ .map("Serial", KITHandCommunicationDriver::ScanMode::Serial);
+ defineRequiredProperty<std::string>("MAC-Address",
+ "MAC-Address of the hand to connect to.");
+ defineOptionalProperty<bool>(
+ "AutomaticReconnectActive",
+ false,
+ "Whether the hand unit should try to reconnect after the connection is lost.");
+ defineOptionalProperty<bool>(
+ "ScanUntilHandFound",
+ true,
+ "Wheather to keep scanning until the hand with the given MAC-Address is found.");
defineOptionalProperty<int>("ScanTimeout", 5, "Timeout for scanning repeatedly.");
}
-
- std::string KITHandUnit::getDefaultName() const
+ std::string
+ KITHandUnit::getDefaultName() const
{
return "KITHandUnit";
}
- void KITHandUnit::onInitHandUnit()
+ void
+ KITHandUnit::onInitHandUnit()
{
//addShapeName("Open"); //is added by something else already
addShapeName("Close");
@@ -63,13 +76,16 @@ namespace armarx
addShapeName("G8");
_driver = std::make_unique<KITHandCommunicationDriver>();
- _driver->registerConnectionStateChangedCallback(std::bind(&KITHandUnit::connectionStateChangedCallback, this, std::placeholders::_1));
+ _driver->registerConnectionStateChangedCallback(
+ std::bind(&KITHandUnit::connectionStateChangedCallback, this, std::placeholders::_1));
}
- void KITHandUnit::onStartHandUnit()
+ void
+ KITHandUnit::onStartHandUnit()
{
std::string macAddress = getProperty<std::string>("MAC-Address");
- KITHandCommunicationDriver::ScanMode mode = getProperty<KITHandCommunicationDriver::ScanMode>("ConnectionType").getValue();
+ KITHandCommunicationDriver::ScanMode mode =
+ getProperty<KITHandCommunicationDriver::ScanMode>("ConnectionType").getValue();
bool found = false;
auto start = std::chrono::system_clock::now();
do
@@ -89,9 +105,12 @@ namespace armarx
{
ARMARX_INFO << "Found hand"; //TODO
}
- if (d.macAdress == macAddress || (mode == KITHandCommunicationDriver::ScanMode::Serial))
+ if (d.macAdress == macAddress ||
+ (mode == KITHandCommunicationDriver::ScanMode::Serial))
{
- d.hardwareTarget = mode == KITHandCommunicationDriver::ScanMode::Bluetooth ? KITHand::HardwareTarget::Bluetooth : KITHand::HardwareTarget::Serial;
+ d.hardwareTarget = mode == KITHandCommunicationDriver::ScanMode::Bluetooth
+ ? KITHand::HardwareTarget::Bluetooth
+ : KITHand::HardwareTarget::Serial;
_driver->connect(d);
while (!_driver->connected())
{
@@ -100,39 +119,42 @@ namespace armarx
found = true;
//gui
- createOrUpdateRemoteGuiTab(buildGui(), [this](RemoteGui::TabProxy & prx)
- {
- processGui(prx);
- });
+ createOrUpdateRemoteGuiTab(
+ buildGui(), [this](RemoteGui::TabProxy& prx) { processGui(prx); });
return;
}
}
- }
- while (getProperty<bool>("ScanUntilHandFound").getValue()
- && !found
- && std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - start).count() < getProperty<int>("ScanTimeout").getValue());
+ } while (getProperty<bool>("ScanUntilHandFound").getValue() && !found &&
+ std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() -
+ start)
+ .count() < getProperty<int>("ScanTimeout").getValue());
if (mode == KITHandCommunicationDriver::ScanMode::Bluetooth)
{
- ARMARX_WARNING << "Could not find hand with the given MAC-Address: " << macAddress << " Shutting down this KITHandUnit.";
+ ARMARX_WARNING << "Could not find hand with the given MAC-Address: " << macAddress
+ << " Shutting down this KITHandUnit.";
}
else
{
- ARMARX_WARNING << "Could not find hand ove serial ports. Shutting down this KITHHandUnit";
+ ARMARX_WARNING
+ << "Could not find hand ove serial ports. Shutting down this KITHHandUnit";
}
getArmarXManager()->asyncShutdown();
}
- void KITHandUnit::onExitHandUnit()
+ void
+ KITHandUnit::onExitHandUnit()
{
ARMARX_IMPORTANT << "Calling driver disconnect";
_driver->disconnect();
_driver.reset();
}
- void KITHandUnit::setShape(const std::string& shapeName, const Ice::Current&)
+ void
+ KITHandUnit::setShape(const std::string& shapeName, const Ice::Current&)
{
- if (std::regex_match(shapeName, std::regex{"[gG](0|[1-9][0-9]*)"}) && _driver->getCurrentConnectedDevice()->abilities.receivesAdditionalGraspCommands)
+ if (std::regex_match(shapeName, std::regex{"[gG](0|[1-9][0-9]*)"}) &&
+ _driver->getCurrentConnectedDevice()->abilities.receivesAdditionalGraspCommands)
{
_driver->sendGrasp(std::stoul(shapeName.substr(1)));
}
@@ -146,8 +168,7 @@ namespace armarx
}
else if (!_shapes.count(shapeName))
{
- ARMARX_WARNING << "Unknown shape name '" << shapeName
- << "'\nKnown shapes: " << _shapes;
+ ARMARX_WARNING << "Unknown shape name '" << shapeName << "'\nKnown shapes: " << _shapes;
}
else
{
@@ -155,7 +176,8 @@ namespace armarx
}
}
- void KITHandUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current&)
+ void
+ KITHandUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_driver);
@@ -163,19 +185,20 @@ namespace armarx
{
if (pair.first == "Fingers")
{
- const std::uint64_t pos = std::clamp(
- static_cast<std::uint64_t>(pair.second * KITHand::ControlOptions::maxPosFingers),
- static_cast<std::uint64_t>(0),
- KITHand::ControlOptions::maxPosFingers);
+ const std::uint64_t pos =
+ std::clamp(static_cast<std::uint64_t>(pair.second *
+ KITHand::ControlOptions::maxPosFingers),
+ static_cast<std::uint64_t>(0),
+ KITHand::ControlOptions::maxPosFingers);
ARMARX_DEBUG << deactivateSpam(1, std::to_string(pos)) << "set fingers " << pos;
_driver->sendFingersPosition(pos);
}
else if (pair.first == "Thumb")
{
const std::uint64_t pos = std::clamp(
- static_cast<std::uint64_t>(pair.second * KITHand::ControlOptions::maxPosThumb),
- static_cast<std::uint64_t>(0),
- KITHand::ControlOptions::maxPosThumb);
+ static_cast<std::uint64_t>(pair.second * KITHand::ControlOptions::maxPosThumb),
+ static_cast<std::uint64_t>(0),
+ KITHand::ControlOptions::maxPosThumb);
ARMARX_DEBUG << deactivateSpam(1, std::to_string(pos)) << "set thumb " << pos;
_driver->sendThumbPosition(pos);
}
@@ -187,35 +210,40 @@ namespace armarx
}
}
- NameValueMap KITHandUnit::getCurrentJointValues(const Ice::Current&)
+ NameValueMap
+ KITHandUnit::getCurrentJointValues(const Ice::Current&)
{
NameValueMap jointValues;
- jointValues["Fingers"] = _driver->getFingersPos() * 1.f / KITHand::ControlOptions::maxPosFingers;
+ jointValues["Fingers"] =
+ _driver->getFingersPos() * 1.f / KITHand::ControlOptions::maxPosFingers;
jointValues["Thumb"] = _driver->getThumbPos() * 1.f / KITHand::ControlOptions::maxPosThumb;
return jointValues;
}
- void KITHandUnit::addShape(const std::string& name, const std::map<std::string, float>& shape)
+ void
+ KITHandUnit::addShape(const std::string& name, const std::map<std::string, float>& shape)
{
_shapes[name] = shape;
addShapeName(name);
}
- void KITHandUnit::addShapeName(const std::string& name)
+ void
+ KITHandUnit::addShapeName(const std::string& name)
{
Variant currentPreshape;
currentPreshape.setString(name);
shapeNames->addVariant(currentPreshape);
}
-
- armarx::PropertyDefinitionsPtr KITHandUnit::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ KITHandUnit::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new KITHandUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new KITHandUnitPropertyDefinitions(getConfigIdentifier()));
}
- void KITHandUnit::connectionStateChangedCallback(const State state)
+ void
+ KITHandUnit::connectionStateChangedCallback(const State state)
{
if (state == State::DeviceLost)
{
@@ -223,29 +251,30 @@ namespace armarx
}
}
- RemoteGui::WidgetPtr KITHandUnit::buildGui()
+ RemoteGui::WidgetPtr
+ KITHandUnit::buildGui()
{
- return RemoteGui::makeSimpleGridLayout().cols(3)
- .addTextLabel("Fingers")
- .addChild(RemoteGui::makeFloatSpinBox("Fingers")
- .min(0).max(1).value(0))
- .addChild(new RemoteGui::HSpacer)
+ return RemoteGui::makeSimpleGridLayout()
+ .cols(3)
+ .addTextLabel("Fingers")
+ .addChild(RemoteGui::makeFloatSpinBox("Fingers").min(0).max(1).value(0))
+ .addChild(new RemoteGui::HSpacer)
- .addTextLabel("Thumb")
- .addChild(RemoteGui::makeFloatSpinBox("Thumb")
- .min(0).max(1).value(0))
- .addChild(new RemoteGui::HSpacer)
+ .addTextLabel("Thumb")
+ .addChild(RemoteGui::makeFloatSpinBox("Thumb").min(0).max(1).value(0))
+ .addChild(new RemoteGui::HSpacer)
- .addChild(RemoteGui::makeCheckBox("AutoSendValues").value(false).label("Auto send"))
- .addChild(RemoteGui::makeButton("SendValues").label("Send"))
- .addChild(new RemoteGui::HSpacer)
+ .addChild(RemoteGui::makeCheckBox("AutoSendValues").value(false).label("Auto send"))
+ .addChild(RemoteGui::makeButton("SendValues").label("Send"))
+ .addChild(new RemoteGui::HSpacer)
- .addChild(RemoteGui::makeLineEdit("Raw").value("M2,1000,1000,100000"))
- .addChild(RemoteGui::makeButton("SendRaw").label("Send Raw"))
- .addChild(new RemoteGui::HSpacer);
+ .addChild(RemoteGui::makeLineEdit("Raw").value("M2,1000,1000,100000"))
+ .addChild(RemoteGui::makeButton("SendRaw").label("Send Raw"))
+ .addChild(new RemoteGui::HSpacer);
}
- void KITHandUnit::processGui(RemoteGui::TabProxy& prx)
+ void
+ KITHandUnit::processGui(RemoteGui::TabProxy& prx)
{
prx.receiveUpdates();
if (prx.getValue<bool>("AutoSendValues").get() || prx.getButtonClicked("SendValues"))
@@ -260,4 +289,4 @@ namespace armarx
_driver->sendRaw(prx.getValue<std::string>("Raw").get());
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/KITHandUnit/KITHandUnit.h b/source/RobotAPI/components/KITHandUnit/KITHandUnit.h
index 5e443b9dff5c0cf9513f6126e77af7ab28d11f51..3dcbe4d5e61848eb517497b366e6f9e532968423 100644
--- a/source/RobotAPI/components/KITHandUnit/KITHandUnit.h
+++ b/source/RobotAPI/components/KITHandUnit/KITHandUnit.h
@@ -37,15 +37,12 @@ namespace armarx
* @class KITHandUnitPropertyDefinitions
* @brief
*/
- class KITHandUnitPropertyDefinitions :
- public armarx::HandUnitPropertyDefinitions
+ class KITHandUnitPropertyDefinitions : public armarx::HandUnitPropertyDefinitions
{
public:
KITHandUnitPropertyDefinitions(std::string prefix);
};
-
-
/**
* @defgroup Component-KITHandUnit KITHandUnit
* @ingroup RobotAPI-Components
@@ -57,9 +54,7 @@ namespace armarx
*
* Detailed description of class KITHandUnit.
*/
- class KITHandUnit :
- virtual public RemoteGuiComponentPluginUser,
- virtual public armarx::HandUnit
+ class KITHandUnit : virtual public RemoteGuiComponentPluginUser, virtual public armarx::HandUnit
{
public:
/// @see armarx::ManagedIceObject::getDefaultName()
@@ -68,15 +63,16 @@ namespace armarx
void onInitHandUnit() override;
void onStartHandUnit() override;
void onExitHandUnit() override;
- void setShape(const std::string& shapeName, const Ice::Current& = Ice::emptyCurrent) override;
- void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& = Ice::emptyCurrent) override;
+ void setShape(const std::string& shapeName,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ void setJointAngles(const NameValueMap& targetJointAngles,
+ const Ice::Current& = Ice::emptyCurrent) override;
NameValueMap getCurrentJointValues(const Ice::Current& = Ice::emptyCurrent) override;
void addShape(const std::string& name, const std::map<std::string, float>& shape);
void addShapeName(const std::string& name);
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -89,6 +85,5 @@ namespace armarx
KITHand::KITHandCommunicationDriverPtr _driver;
std::map<std::string, std::map<std::string, float>> _shapes;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/KITHandUnit/test/KITHandUnitTest.cpp b/source/RobotAPI/components/KITHandUnit/test/KITHandUnitTest.cpp
index 4350346ba4141202887160e311aecac65e227d0e..e22425284239c9162516fbdc07f9d2e1026ac13a 100644
--- a/source/RobotAPI/components/KITHandUnit/test/KITHandUnitTest.cpp
+++ b/source/RobotAPI/components/KITHandUnit/test/KITHandUnitTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/KITHandUnit/KITHandUnit.h>
#include <iostream>
+#include <RobotAPI/components/KITHandUnit/KITHandUnit.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::KITHandUnit instance;
diff --git a/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.cpp b/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.cpp
index b6877129cba3009130bcda5715cf77d85d007f05..12a79429af9297afb67ab9fb62a723b5eadc8bec 100644
--- a/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.cpp
+++ b/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.cpp
@@ -22,35 +22,32 @@
#include "KITProstheticHandObserver.h"
-
namespace armarx
{
- void KITProstheticHandObserver::onInitComponent()
+ void
+ KITProstheticHandObserver::onInitComponent()
{
-
}
-
- void KITProstheticHandObserver::onConnectComponent()
+ void
+ KITProstheticHandObserver::onConnectComponent()
{
-
}
-
- void KITProstheticHandObserver::onDisconnectComponent()
+ void
+ KITProstheticHandObserver::onDisconnectComponent()
{
-
}
-
- void KITProstheticHandObserver::onExitComponent()
+ void
+ KITProstheticHandObserver::onExitComponent()
{
-
}
- armarx::PropertyDefinitionsPtr KITProstheticHandObserver::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ KITProstheticHandObserver::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new KITProstheticHandObserverPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new KITProstheticHandObserverPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.h b/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.h
index 04565b86c3b7b63f1099a71905bd1d90ce973675..3cdf0c40c5a8564ab57d855706f3bdc89e1f70fb 100644
--- a/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.h
+++ b/source/RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.h
@@ -31,11 +31,10 @@ namespace armarx
* @class KITProstheticHandObserverPropertyDefinitions
* @brief
*/
- class KITProstheticHandObserverPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class KITProstheticHandObserverPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- KITProstheticHandObserverPropertyDefinitions(std::string prefix):
+ KITProstheticHandObserverPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
@@ -54,14 +53,14 @@ namespace armarx
*
* Detailed description of class KITProstheticHandObserver.
*/
- class KITProstheticHandObserver :
- virtual public armarx::Component
+ class KITProstheticHandObserver : virtual public armarx::Component
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "KITProstheticHandObserver";
}
@@ -92,4 +91,4 @@ namespace armarx
*/
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/KITProstheticHandObserver/test/KITProstheticHandObserverTest.cpp b/source/RobotAPI/components/KITProstheticHandObserver/test/KITProstheticHandObserverTest.cpp
index 3715a91c06db60dd8b73a92d44be9ddf44c34d37..b3c5ea231770579d05180fe23e70e8969073f015 100644
--- a/source/RobotAPI/components/KITProstheticHandObserver/test/KITProstheticHandObserverTest.cpp
+++ b/source/RobotAPI/components/KITProstheticHandObserver/test/KITProstheticHandObserverTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.h>
#include <iostream>
+#include <RobotAPI/components/KITProstheticHandObserver/KITProstheticHandObserver.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::KITProstheticHandObserver instance;
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
index 906fc21433a7e5f6f6aec6898da7d55c6667e2cd..7fe07389ff1b482f5556364a6d934adbcae8f4bd 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
+++ b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.cpp
@@ -22,22 +22,25 @@
#include "KITProstheticHandUnit.h"
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include <algorithm>
-#include <thread>
#include <regex>
+#include <thread>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
namespace armarx
{
- armarx::PropertyDefinitionsPtr KITProstheticHandUnit::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ KITProstheticHandUnit::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new KITProstheticHandUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new KITProstheticHandUnitPropertyDefinitions(getConfigIdentifier()));
}
- void KITProstheticHandUnit::onInitHandUnit()
+ void
+ KITProstheticHandUnit::onInitHandUnit()
{
_driver = std::make_unique<BLEProthesisInterface>(getProperty<std::string>("MAC"));
//addShapeName("Open"); //is added by something else already
@@ -59,12 +62,15 @@ namespace armarx
}
}
- void KITProstheticHandUnit::onStartHandUnit()
+ void
+ KITProstheticHandUnit::onStartHandUnit()
{
- _debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName"));
+ _debugObserver =
+ getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName"));
if (!getProperty<std::string>("RemoteGuiName").getValue().empty())
{
- _remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
+ _remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(
+ getProperty<std::string>("RemoteGuiName").getValue());
RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
@@ -73,23 +79,22 @@ namespace armarx
{
rootLayoutBuilder.addChild(
RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel(name))
- .addChild(RemoteGui::makeTextLabel("min " + std::to_string(min)))
- .addChild(RemoteGui::makeFloatSlider(name).min(min).max(max).value(val).steps(steps))
- .addChild(RemoteGui::makeTextLabel("max " + std::to_string(max)))
- );
+ .addChild(RemoteGui::makeTextLabel(name))
+ .addChild(RemoteGui::makeTextLabel("min " + std::to_string(min)))
+ .addChild(
+ RemoteGui::makeFloatSlider(name).min(min).max(max).value(val).steps(
+ steps))
+ .addChild(RemoteGui::makeTextLabel("max " + std::to_string(max))));
rootLayoutBuilder.addChild(
RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel(name + " Pos "))
- .addChild(RemoteGui::makeLabel(name + "_pos").value("0"))
- .addChild(new RemoteGui::HSpacer())
- );
+ .addChild(RemoteGui::makeTextLabel(name + " Pos "))
+ .addChild(RemoteGui::makeLabel(name + "_pos").value("0"))
+ .addChild(new RemoteGui::HSpacer()));
rootLayoutBuilder.addChild(
RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel(name + " PWM "))
- .addChild(RemoteGui::makeLabel(name + "_pwm").value("0"))
- .addChild(new RemoteGui::HSpacer())
- );
+ .addChild(RemoteGui::makeTextLabel(name + " PWM "))
+ .addChild(RemoteGui::makeLabel(name + "_pwm").value("0"))
+ .addChild(new RemoteGui::HSpacer()));
};
addFinger("Thumb", 0, 1, _lastGuiValueThumb, _driver->getMaxPosThumb());
@@ -97,37 +102,43 @@ namespace armarx
rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
- _guiTask = new SimplePeriodicTask<>([&]()
- {
- _guiTab.receiveUpdates();
- _driver->getMaxPosThumb();
- const float t = _guiTab.getValue<float>("Thumb").get();
- const float f = _guiTab.getValue<float>("Fingers").get();
-
- bool updateT = t != _lastGuiValueThumb;
- bool updateF = f != _lastGuiValueFingers;
- _lastGuiValueThumb = t;
- _lastGuiValueFingers = f;
-
- if (updateT && updateF)
+ _guiTask = new SimplePeriodicTask<>(
+ [&]()
{
- setJointAngles({{"Thumb", t}, {"Fingers", f}});
- }
- else if (updateT)
- {
- setJointAngles({{"Thumb", t}});
- }
- else if (updateF)
- {
- setJointAngles({{"Fingers", f}});
- }
-
- _guiTab.getValue<std::string>("Thumb_pos").set(std::to_string(_driver->getThumbPos()));
- _guiTab.getValue<std::string>("Thumb_pwm").set(std::to_string(_driver->getThumbPWM()));
- _guiTab.getValue<std::string>("Fingers_pos").set(std::to_string(_driver->getFingerPos()));
- _guiTab.getValue<std::string>("Fingers_pwm").set(std::to_string(_driver->getFingerPWM()));
- _guiTab.sendUpdates();
- }, 10);
+ _guiTab.receiveUpdates();
+ _driver->getMaxPosThumb();
+ const float t = _guiTab.getValue<float>("Thumb").get();
+ const float f = _guiTab.getValue<float>("Fingers").get();
+
+ bool updateT = t != _lastGuiValueThumb;
+ bool updateF = f != _lastGuiValueFingers;
+ _lastGuiValueThumb = t;
+ _lastGuiValueFingers = f;
+
+ if (updateT && updateF)
+ {
+ setJointAngles({{"Thumb", t}, {"Fingers", f}});
+ }
+ else if (updateT)
+ {
+ setJointAngles({{"Thumb", t}});
+ }
+ else if (updateF)
+ {
+ setJointAngles({{"Fingers", f}});
+ }
+
+ _guiTab.getValue<std::string>("Thumb_pos")
+ .set(std::to_string(_driver->getThumbPos()));
+ _guiTab.getValue<std::string>("Thumb_pwm")
+ .set(std::to_string(_driver->getThumbPWM()));
+ _guiTab.getValue<std::string>("Fingers_pos")
+ .set(std::to_string(_driver->getFingerPos()));
+ _guiTab.getValue<std::string>("Fingers_pwm")
+ .set(std::to_string(_driver->getFingerPWM()));
+ _guiTab.sendUpdates();
+ },
+ 10);
RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
@@ -138,12 +149,15 @@ namespace armarx
}
}
- void KITProstheticHandUnit::onExitHandUnit()
+ void
+ KITProstheticHandUnit::onExitHandUnit()
{
_driver.reset();
}
- void KITProstheticHandUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current&)
+ void
+ KITProstheticHandUnit::setJointAngles(const NameValueMap& targetJointAngles,
+ const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_driver);
@@ -152,8 +166,9 @@ namespace armarx
if (pair.first == "Fingers")
{
const std::uint64_t pos = std::clamp(
- static_cast<std::uint64_t>(pair.second * _driver->getMaxPosFingers()),
- 0ul, _driver->getMaxPosFingers());
+ static_cast<std::uint64_t>(pair.second * _driver->getMaxPosFingers()),
+ 0ul,
+ _driver->getMaxPosFingers());
ARMARX_INFO << "set fingers " << pos;
_driver->sendFingerPWM(200, 2999, pos);
// fix until hw driver is fixed to handle multiple commands at the same time
@@ -161,9 +176,10 @@ namespace armarx
}
else if (pair.first == "Thumb")
{
- const std::uint64_t pos = std::clamp(
- static_cast<std::uint64_t>(pair.second * _driver->getMaxPosThumb()),
- 0ul, _driver->getMaxPosThumb());
+ const std::uint64_t pos =
+ std::clamp(static_cast<std::uint64_t>(pair.second * _driver->getMaxPosThumb()),
+ 0ul,
+ _driver->getMaxPosThumb());
ARMARX_INFO << "set thumb " << pos;
_driver->sendThumbPWM(200, 2999, pos);
// fix until hw driver is fixed to handle multiple commands at the same time
@@ -176,7 +192,8 @@ namespace armarx
}
}
- NameValueMap KITProstheticHandUnit::getCurrentJointValues(const Ice::Current&)
+ NameValueMap
+ KITProstheticHandUnit::getCurrentJointValues(const Ice::Current&)
{
NameValueMap jointValues;
jointValues["Fingers"] = _driver->getFingerPos() * 1.f / _driver->getMaxPosFingers();
@@ -184,20 +201,24 @@ namespace armarx
return jointValues;
}
- void KITProstheticHandUnit::addShape(const std::string& name, const std::map<std::string, float>& shape)
+ void
+ KITProstheticHandUnit::addShape(const std::string& name,
+ const std::map<std::string, float>& shape)
{
_shapes[name] = shape;
addShapeName(name);
}
- void KITProstheticHandUnit::addShapeName(const std::string& name)
+ void
+ KITProstheticHandUnit::addShapeName(const std::string& name)
{
Variant currentPreshape;
currentPreshape.setString(name);
shapeNames->addVariant(currentPreshape);
}
- void KITProstheticHandUnit::setShape(const std::string& shapeName, const Ice::Current&)
+ void
+ KITProstheticHandUnit::setShape(const std::string& shapeName, const Ice::Current&)
{
if (std::regex_match(shapeName, std::regex{"[gG](0|[1-9][0-9]*)"}))
{
@@ -213,12 +234,11 @@ namespace armarx
}
else if (!_shapes.count(shapeName))
{
- ARMARX_WARNING << "Unknown shape name '" << shapeName
- << "'\nKnown shapes: " << _shapes;
+ ARMARX_WARNING << "Unknown shape name '" << shapeName << "'\nKnown shapes: " << _shapes;
}
else
{
setJointAngles(_shapes.at(shapeName));
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h
index 1cba8b8eb3e34e163780298b3fda9af46c4f4985..81a0179bda0f92378692da81161b5539667fe0a2 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h
+++ b/source/RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h
@@ -38,16 +38,17 @@ namespace armarx
* @class KITProstheticHandUnitPropertyDefinitions
* @brief
*/
- class KITProstheticHandUnitPropertyDefinitions:
- public armarx::HandUnitPropertyDefinitions
+ class KITProstheticHandUnitPropertyDefinitions : public armarx::HandUnitPropertyDefinitions
{
public:
- KITProstheticHandUnitPropertyDefinitions(std::string prefix):
+ KITProstheticHandUnitPropertyDefinitions(std::string prefix) :
armarx::HandUnitPropertyDefinitions(prefix)
{
defineRequiredProperty<std::string>("MAC", "The KIT Prosthetic Hand's Bluetooth MAC");
defineOptionalProperty<std::string>("RemoteGuiName", "", "Name of the remote gui");
- defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the debug observer that should be used");
+ defineOptionalProperty<std::string>("DebugObserverName",
+ "DebugObserver",
+ "Name of the debug observer that should be used");
}
};
@@ -62,14 +63,14 @@ namespace armarx
*
* Detailed description of class KITProstheticHandUnit.
*/
- class KITProstheticHandUnit :
- virtual public armarx::HandUnit
+ class KITProstheticHandUnit : virtual public armarx::HandUnit
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "KITProstheticHandUnit";
}
@@ -77,21 +78,23 @@ namespace armarx
void onInitHandUnit() override;
void onStartHandUnit() override;
void onExitHandUnit() override;
- void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& = Ice::emptyCurrent) override;
+ void setJointAngles(const NameValueMap& targetJointAngles,
+ const Ice::Current& = Ice::emptyCurrent) override;
NameValueMap getCurrentJointValues(const Ice::Current&) override;
void addShape(const std::string& name, const std::map<std::string, float>& shape);
void addShapeName(const std::string& name);
- void setShape(const std::string& shapeName, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setShape(const std::string& shapeName,
+ const Ice::Current& c = Ice::emptyCurrent) override;
protected:
-
/**
* @see PropertyUser::createPropertyDefinitions()
*/
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
private:
std::unique_ptr<BLEProthesisInterface> _driver;
std::map<std::string, std::map<std::string, float>> _shapes;
@@ -104,4 +107,4 @@ namespace armarx
float _lastGuiValueThumb{0};
float _lastGuiValueFingers{0};
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/KITProstheticHandUnit/test/KITProstheticHandUnitTest.cpp b/source/RobotAPI/components/KITProstheticHandUnit/test/KITProstheticHandUnitTest.cpp
index 96aba46deda34d9b8b193f756c695bfc0f26e976..04b3e82f4d728d61c0c332c023e672b8ee405036 100644
--- a/source/RobotAPI/components/KITProstheticHandUnit/test/KITProstheticHandUnitTest.cpp
+++ b/source/RobotAPI/components/KITProstheticHandUnit/test/KITProstheticHandUnitTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h>
#include <iostream>
+#include <RobotAPI/components/KITProstheticHandUnit/KITProstheticHandUnit.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::KITProstheticHandUnit instance;
diff --git a/source/RobotAPI/components/MultiHandUnit/MultiHandUnit.cpp b/source/RobotAPI/components/MultiHandUnit/MultiHandUnit.cpp
index 066d9187b1a74d0a3c29e1cf4c1bded515adc51e..321f9081cc0f8503f93f3e46b02c29c6000b5624 100644
--- a/source/RobotAPI/components/MultiHandUnit/MultiHandUnit.cpp
+++ b/source/RobotAPI/components/MultiHandUnit/MultiHandUnit.cpp
@@ -22,7 +22,6 @@
#include "MultiHandUnit.h"
-
namespace armarx
{
MultiHandUnitPropertyDefinitions::MultiHandUnitPropertyDefinitions(std::string prefix) :
@@ -31,14 +30,14 @@ namespace armarx
defineOptionalProperty<std::string>("HandUnitNameCSV", "", "");
}
-
- std::string MultiHandUnit::getDefaultName() const
+ std::string
+ MultiHandUnit::getDefaultName() const
{
return "MultiHandUnit";
}
-
- void MultiHandUnit::onInitComponent()
+ void
+ MultiHandUnit::onInitComponent()
{
const auto units = getPropertyAsCSV<std::string>("HandUnitNameCSV");
if (units.empty())
@@ -54,7 +53,8 @@ namespace armarx
}
}
- void MultiHandUnit::onConnectComponent()
+ void
+ MultiHandUnit::onConnectComponent()
{
for (auto& info : _handInfos)
{
@@ -64,34 +64,41 @@ namespace armarx
}
}
- void MultiHandUnit::onDisconnectComponent()
+ void
+ MultiHandUnit::onDisconnectComponent()
{
_hands.clear();
}
- void MultiHandUnit::onExitComponent()
+ void
+ MultiHandUnit::onExitComponent()
{
-
}
- armarx::PropertyDefinitionsPtr MultiHandUnit::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ MultiHandUnit::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new MultiHandUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new MultiHandUnitPropertyDefinitions(getConfigIdentifier()));
}
- HandInfoSeq MultiHandUnit::getHandInfos(const Ice::Current&)
+ HandInfoSeq
+ MultiHandUnit::getHandInfos(const Ice::Current&)
{
return _handInfos;
}
- void MultiHandUnit::setJointValues(const std::string& handName, const NameValueMap& jointValues, const Ice::Current&)
+ void
+ MultiHandUnit::setJointValues(const std::string& handName,
+ const NameValueMap& jointValues,
+ const Ice::Current&)
{
_hands.at(handName)->setJointAngles(jointValues);
}
- NameValueMap MultiHandUnit::getJointValues(const std::string& handName, const Ice::Current&)
+ NameValueMap
+ MultiHandUnit::getJointValues(const std::string& handName, const Ice::Current&)
{
return _hands.at(handName)->getCurrentJointValues();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/MultiHandUnit/MultiHandUnit.h b/source/RobotAPI/components/MultiHandUnit/MultiHandUnit.h
index b493beab1704e39157cc38cb6769e4162ee2b677..2c47100efa4659842d0a6d042fb06f742e359fd3 100644
--- a/source/RobotAPI/components/MultiHandUnit/MultiHandUnit.h
+++ b/source/RobotAPI/components/MultiHandUnit/MultiHandUnit.h
@@ -26,22 +26,18 @@
#include <RobotAPI/interface/units/MultiHandUnitInterface.h>
-
namespace armarx
{
/**
* @class MultiHandUnitPropertyDefinitions
* @brief
*/
- class MultiHandUnitPropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
+ class MultiHandUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
MultiHandUnitPropertyDefinitions(std::string prefix);
};
-
-
/**
* @defgroup Component-MultiHandUnit MultiHandUnit
* @ingroup RobotAPI-Components
@@ -53,18 +49,14 @@ namespace armarx
*
* Detailed description of class MultiHandUnit.
*/
- class MultiHandUnit :
- virtual public armarx::Component,
- virtual public MultiHandUnitInterface
+ class MultiHandUnit : virtual public armarx::Component, virtual public MultiHandUnitInterface
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
virtual std::string getDefaultName() const override;
protected:
-
/// @see armarx::ManagedIceObject::onInitComponent()
virtual void onInitComponent() override;
@@ -81,10 +73,14 @@ namespace armarx
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
HandInfoSeq getHandInfos(const Ice::Current& = Ice::emptyCurrent) override;
- void setJointValues(const std::string& handName, const NameValueMap& jointValues, const Ice::Current& = Ice::emptyCurrent) override;
- NameValueMap getJointValues(const std::string& handName, const Ice::Current& = Ice::emptyCurrent) override;
+ void setJointValues(const std::string& handName,
+ const NameValueMap& jointValues,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ NameValueMap getJointValues(const std::string& handName,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
private:
HandInfoSeq _handInfos;
std::map<std::string, HandUnitInterfacePrx> _hands;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/MultiHandUnit/test/MultiHandUnitTest.cpp b/source/RobotAPI/components/MultiHandUnit/test/MultiHandUnitTest.cpp
index d43a425a4a5aac6d47b48c3bd245fe4ee676e373..c8b91e5c5406c100d7ad53b2a921a28a9a63a193 100644
--- a/source/RobotAPI/components/MultiHandUnit/test/MultiHandUnitTest.cpp
+++ b/source/RobotAPI/components/MultiHandUnit/test/MultiHandUnitTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/MultiHandUnit/MultiHandUnit.h>
#include <iostream>
+#include <RobotAPI/components/MultiHandUnit/MultiHandUnit.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::MultiHandUnit instance;
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
index 17292ba93b00511d251c50a3241859c38ce127c4..04d84b1212b16286db6fd8032f3c521e331a9d67 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.cpp
@@ -21,28 +21,28 @@
*/
#include "NaturalIKTest.h"
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-#include <RobotAPI/components/ArViz/Client/Client.h>
-#include <RobotAPI/libraries/natik/NaturalIK.h>
-#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/math/Helpers.h>
#include <random>
#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
-
#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
#include <VirtualRobot/IK/constraints/PoseConstraint.h>
#include <VirtualRobot/IK/constraints/PositionConstraint.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/math/Helpers.h>
-#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/diffik/CompositeDiffIK.h>
+#include <RobotAPI/libraries/natik/NaturalIK.h>
namespace armarx
{
@@ -52,20 +52,22 @@ namespace armarx
//defineRequiredProperty<std::string>("PropertyName", "Description");
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
- defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
- defineOptionalProperty<std::string>("ArVizTopicName", "ArVizTopic", "Layer updates are sent over this topic.");
+ defineOptionalProperty<std::string>(
+ "DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
+ defineOptionalProperty<std::string>(
+ "ArVizTopicName", "ArVizTopic", "Layer updates are sent over this topic.");
//defineOptionalProperty<std::string>("MyProxyName", "MyProxy", "Name of the proxy to be used");
}
-
- std::string NaturalIKTest::getDefaultName() const
+ std::string
+ NaturalIKTest::getDefaultName() const
{
return "NaturalIKTest";
}
-
- void NaturalIKTest::onInitComponent()
+ void
+ NaturalIKTest::onInitComponent()
{
// Register offered topices and used proxies here.
offeringTopicFromProperty("DebugObserverName");
@@ -77,8 +79,8 @@ namespace armarx
offeringTopicFromProperty("ArVizTopicName");
}
-
- void NaturalIKTest::onConnectComponent()
+ void
+ NaturalIKTest::onConnectComponent()
{
// Get topics and proxies here. Pass the *InterfacePrx type as template argument.
debugObserver = getTopicFromProperty<DebugObserverInterfacePrx>("DebugObserverName");
@@ -86,25 +88,27 @@ namespace armarx
// getProxyFromProperty<MyProxyInterfacePrx>("MyProxyName");
- createOrUpdateRemoteGuiTab(buildGui(), [this](RemoteGui::TabProxy & prx)
- {
- processGui(prx);
- });
+ createOrUpdateRemoteGuiTab(buildGui(),
+ [this](RemoteGui::TabProxy& prx) { processGui(prx); });
vizTask = new RunningTask<NaturalIKTest>(this, &NaturalIKTest::vizTaskRun);
vizTask->start();
}
- void NaturalIKTest::onDisconnectComponent()
+ void
+ NaturalIKTest::onDisconnectComponent()
{
vizTask->stop();
vizTask = nullptr;
}
- RemoteGui::WidgetPtr NaturalIKTest::buildGui()
+ RemoteGui::WidgetPtr
+ NaturalIKTest::buildGui()
{
- RemoteGui::detail::GroupBoxBuilder leftBox = RemoteGui::makeGroupBox().label("Left").addChild(
- RemoteGui::makeSimpleGridLayout().cols(2)
+ RemoteGui::detail::GroupBoxBuilder leftBox =
+ RemoteGui::makeGroupBox().label("Left").addChild(
+ RemoteGui::makeSimpleGridLayout()
+ .cols(2)
.addTextLabel("L.X")
.addChild(RemoteGui::makeFloatSlider("L.X").min(-600).max(600).value(0))
@@ -130,10 +134,11 @@ namespace armarx
.addChild(RemoteGui::makeFloatSlider("L.rY").min(-180).max(180).value(0))
.addTextLabel("L.rZ")
- .addChild(RemoteGui::makeFloatSlider("L.rZ").min(-180).max(180).value(0))
- );
- RemoteGui::detail::GroupBoxBuilder rightBox = RemoteGui::makeGroupBox().label("Right").addChild(
- RemoteGui::makeSimpleGridLayout().cols(2)
+ .addChild(RemoteGui::makeFloatSlider("L.rZ").min(-180).max(180).value(0)));
+ RemoteGui::detail::GroupBoxBuilder rightBox =
+ RemoteGui::makeGroupBox().label("Right").addChild(
+ RemoteGui::makeSimpleGridLayout()
+ .cols(2)
.addTextLabel("R.X")
.addChild(RemoteGui::makeFloatSlider("R.X").min(-600).max(600).value(0))
@@ -159,24 +164,25 @@ namespace armarx
.addChild(RemoteGui::makeFloatSlider("R.rY").min(-180).max(180).value(0))
.addTextLabel("R.rZ")
- .addChild(RemoteGui::makeFloatSlider("R.rZ").min(-180).max(180).value(0))
- );
+ .addChild(RemoteGui::makeFloatSlider("R.rZ").min(-180).max(180).value(0)));
return RemoteGui::makeVBoxLayout()
- .addChild(RemoteGui::makeCheckBox("useCompositeIK").value(false))
- .addChild(RemoteGui::makeSimpleGridLayout().cols(2).addChild(leftBox).addChild(rightBox))
- .addChild(RemoteGui::makeSimpleGridLayout().cols(2)
- .addTextLabel("elbowKp")
- .addChild(RemoteGui::makeFloatSlider("elbowKp").min(0).max(1).value(1))
-
- .addTextLabel("jlaKp")
- .addChild(RemoteGui::makeFloatSlider("jlaKp").min(0).max(1).value(0))
- )
- .addChild(RemoteGui::makeLabel("errors").value("<errors>"))
- .addChild(RemoteGui::makeButton("runTest").label("run test"));
+ .addChild(RemoteGui::makeCheckBox("useCompositeIK").value(false))
+ .addChild(
+ RemoteGui::makeSimpleGridLayout().cols(2).addChild(leftBox).addChild(rightBox))
+ .addChild(RemoteGui::makeSimpleGridLayout()
+ .cols(2)
+ .addTextLabel("elbowKp")
+ .addChild(RemoteGui::makeFloatSlider("elbowKp").min(0).max(1).value(1))
+
+ .addTextLabel("jlaKp")
+ .addChild(RemoteGui::makeFloatSlider("jlaKp").min(0).max(1).value(0)))
+ .addChild(RemoteGui::makeLabel("errors").value("<errors>"))
+ .addChild(RemoteGui::makeButton("runTest").label("run test"));
}
- void NaturalIKTest::processGui(RemoteGui::TabProxy& prx)
+ void
+ NaturalIKTest::processGui(RemoteGui::TabProxy& prx)
{
prx.receiveUpdates();
@@ -237,12 +243,17 @@ namespace armarx
struct IKStats
{
- IKStats(const std::string& name) : name(name) {}
+ IKStats(const std::string& name) : name(name)
+ {
+ }
+
std::string name;
int solved = 0;
float duration = 0;
std::vector<Eigen::Vector3f> elbow;
- Eigen::Vector3f averageElb()
+
+ Eigen::Vector3f
+ averageElb()
{
Eigen::Vector3f elb = Eigen::Vector3f ::Zero();
for (auto e : elbow)
@@ -256,23 +267,31 @@ namespace armarx
struct SoftPositionConstraint : public VirtualRobot::PositionConstraint
{
public:
- SoftPositionConstraint(const VirtualRobot::RobotPtr& robot, const VirtualRobot::RobotNodeSetPtr& nodeSet, const VirtualRobot::SceneObjectPtr& node, const VirtualRobot::SceneObjectPtr& tcp, const Eigen::Vector3f& target,
- VirtualRobot::IKSolver::CartesianSelection cartesianSelection = VirtualRobot::IKSolver::All, float tolerance = 3.0f)
- : PositionConstraint(robot, nodeSet, node, target, cartesianSelection, tolerance), tcp(tcp)
+ SoftPositionConstraint(const VirtualRobot::RobotPtr& robot,
+ const VirtualRobot::RobotNodeSetPtr& nodeSet,
+ const VirtualRobot::SceneObjectPtr& node,
+ const VirtualRobot::SceneObjectPtr& tcp,
+ const Eigen::Vector3f& target,
+ VirtualRobot::IKSolver::CartesianSelection cartesianSelection =
+ VirtualRobot::IKSolver::All,
+ float tolerance = 3.0f) :
+ PositionConstraint(robot, nodeSet, node, target, cartesianSelection, tolerance),
+ tcp(tcp)
{
optimizationFunctions.clear();
addOptimizationFunction(0, true);
}
- VirtualRobot::SceneObjectPtr tcp;
+ VirtualRobot::SceneObjectPtr tcp;
- Eigen::VectorXf optimizationGradient(unsigned int id) override
+ Eigen::VectorXf
+ optimizationGradient(unsigned int id) override
{
int size = nodeSet->getSize();
- (void) size;
+ (void)size;
- Eigen::MatrixXf J = ik->getJacobianMatrix(tcp);//.block(0, 0, 3, size);
+ Eigen::MatrixXf J = ik->getJacobianMatrix(tcp); //.block(0, 0, 3, size);
//Eigen::Vector3f d = eef->getGlobalPose().block<3,1>(0,3) - target;
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(J);
@@ -287,22 +306,26 @@ namespace armarx
// Prevent division by zero
if (squaredNorm > 1.0e-32f)
{
- mapped += nullspace.col(i) * nullspace.col(i).dot(grad) / nullspace.col(i).squaredNorm();
+ mapped += nullspace.col(i) * nullspace.col(i).dot(grad) /
+ nullspace.col(i).squaredNorm();
}
}
return mapped;
}
- bool checkTolerances() override
+ bool
+ checkTolerances() override
{
return true;
}
};
- void NaturalIKTest::testTaskRun()
+ void
+ NaturalIKTest::testTaskRun()
{
CMakePackageFinder finder("armar6_rt");
- std::string robotFile = finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
+ std::string robotFile =
+ finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
ARMARX_IMPORTANT << "loading robot from " << robotFile;
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
@@ -317,18 +340,18 @@ namespace armarx
VirtualRobot::RobotNodePtr tcp_R = rns_R->getTCP();
- float upper_arm_length = (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
+ float upper_arm_length =
+ (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
//float lower_arm_length = (elb_R->getPositionInRootFrame() - wri1_R->getPositionInRootFrame()).norm();
- float lower_arm_length = (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
-
-
+ float lower_arm_length =
+ (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
Eigen::Vector3f shoulder_R = sho1_R->getPositionInRootFrame();
Eigen::Vector3f shoulder_L = sho1_L->getPositionInRootFrame();
Eigen::Vector3f offset = (shoulder_R + shoulder_L) / 2;
- (void) offset;
+ (void)offset;
NaturalIK ik_R("Right", shoulder_R, 1);
NaturalIK ik_L("Left", shoulder_L, 1);
@@ -345,7 +368,11 @@ namespace armarx
arm_R.tcp = rns_R->getTCP();
std::vector<NaturalIK::Parameters> ikParamList;
- std::vector<IKStats> ikStats = {IKStats("NaturalIK"), IKStats("SimpleDiffIK"), IKStats("OptIK"), IKStats("DiffIK"), IKStats("CompositeIK")};
+ std::vector<IKStats> ikStats = {IKStats("NaturalIK"),
+ IKStats("SimpleDiffIK"),
+ IKStats("OptIK"),
+ IKStats("DiffIK"),
+ IKStats("CompositeIK")};
NaturalIK::Parameters p0;
p0.diffIKparams.resetRnsValues = false;
@@ -373,8 +400,7 @@ namespace armarx
pc.resetRnsValues = false;
-
- std::random_device rd; //Will be used to obtain a seed for the random number engine
+ std::random_device rd; //Will be used to obtain a seed for the random number engine
std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
std::uniform_real_distribution<> dis(0.0, 1.0);
@@ -409,7 +435,6 @@ namespace armarx
initialConfigurations.emplace_back(rns_R->getJointValuesEigen());
-
std::vector<Eigen::Matrix4f> targets;
while (targets.size() < 100)
{
@@ -443,7 +468,8 @@ namespace armarx
for (const Eigen::VectorXf& initjv : initialConfigurations)
{
rns_R->setJointValues(initjv);
- NaturalDiffIK::Result ikResult = ik_R.calculateIK(targets.at(n), arm_R, ikParamList.at(i));
+ NaturalDiffIK::Result ikResult =
+ ik_R.calculateIK(targets.at(n), arm_R, ikParamList.at(i));
if (ikResult.reached)
{
ikStats.at(i).solved = ikStats.at(i).solved + 1;
@@ -461,26 +487,42 @@ namespace armarx
{
elbJoints_R.push_back(rns_R->getNode(i));
}
- VirtualRobot::RobotNodeSetPtr rnsElb_R = VirtualRobot::RobotNodeSet::createRobotNodeSet(robot, "ELB_R", elbJoints_R, rns_R->getKinematicRoot(), elb_R);
+ VirtualRobot::RobotNodeSetPtr rnsElb_R = VirtualRobot::RobotNodeSet::createRobotNodeSet(
+ robot, "ELB_R", elbJoints_R, rns_R->getKinematicRoot(), elb_R);
for (size_t n = 0; n < targets.size(); n++)
{
rns_R->setJointValues(initialConfigurations.at(0));
VirtualRobot::ConstrainedOptimizationIK optIK(robot, rns_R);
//VirtualRobot::ConstraintPtr conPose(new VirtualRobot::PoseConstraint(robot, rns_R, tcp_R, targets.at(n)));
//optIK.addConstraint(conPose);
- VirtualRobot::ConstraintPtr posConstraint(new VirtualRobot::PositionConstraint(robot, rns_R, tcp_R,
- math::Helpers::GetPosition(targets.at(n)), VirtualRobot::IKSolver::CartesianSelection::All));
+ VirtualRobot::ConstraintPtr posConstraint(new VirtualRobot::PositionConstraint(
+ robot,
+ rns_R,
+ tcp_R,
+ math::Helpers::GetPosition(targets.at(n)),
+ VirtualRobot::IKSolver::CartesianSelection::All));
posConstraint->setOptimizationFunctionFactor(1);
- VirtualRobot::ConstraintPtr oriConstraint(new VirtualRobot::OrientationConstraint(robot, rns_R, tcp_R,
- math::Helpers::GetOrientation(targets.at(n)), VirtualRobot::IKSolver::CartesianSelection::All, 2.f / 180 * M_PI)); // was VirtualRobot::MathTools::deg2rad(...)
+ VirtualRobot::ConstraintPtr oriConstraint(new VirtualRobot::OrientationConstraint(
+ robot,
+ rns_R,
+ tcp_R,
+ math::Helpers::GetOrientation(targets.at(n)),
+ VirtualRobot::IKSolver::CartesianSelection::All,
+ 2.f / 180 * M_PI)); // was VirtualRobot::MathTools::deg2rad(...)
oriConstraint->setOptimizationFunctionFactor(1000);
- Eigen::Vector3f elbowPos = ik_R.solveSoechtingIK(math::Helpers::GetPosition(targets.at(n))).elbow;
+ Eigen::Vector3f elbowPos =
+ ik_R.solveSoechtingIK(math::Helpers::GetPosition(targets.at(n))).elbow;
- VirtualRobot::ConstraintPtr elbConstraint(new SoftPositionConstraint(robot, rns_R, elb_R, tcp_R,
- elbowPos, VirtualRobot::IKSolver::CartesianSelection::All));
+ VirtualRobot::ConstraintPtr elbConstraint(
+ new SoftPositionConstraint(robot,
+ rns_R,
+ elb_R,
+ tcp_R,
+ elbowPos,
+ VirtualRobot::IKSolver::CartesianSelection::All));
elbConstraint->setOptimizationFunctionFactor(0.01);
//elbConstraint->
@@ -527,12 +569,15 @@ namespace armarx
{
rns_R->setJointValues(initjv);
CompositeDiffIK ik(rns_R);
- CompositeDiffIK::TargetPtr t(new CompositeDiffIK::Target(rns_R, tcp_R, targets.at(n), VirtualRobot::IKSolver::CartesianSelection::All));
+ CompositeDiffIK::TargetPtr t(new CompositeDiffIK::Target(
+ rns_R, tcp_R, targets.at(n), VirtualRobot::IKSolver::CartesianSelection::All));
ik.addTarget(t);
- CompositeDiffIK::NullspaceJointTargetPtr nsjt(new CompositeDiffIK::NullspaceJointTarget(rns_R));
+ CompositeDiffIK::NullspaceJointTargetPtr nsjt(
+ new CompositeDiffIK::NullspaceJointTarget(rns_R));
nsjt->set(2, 0.2, 1);
ik.addNullspaceGradient(nsjt);
- CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(new CompositeDiffIK::NullspaceJointLimitAvoidance(rns_R));
+ CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
+ new CompositeDiffIK::NullspaceJointLimitAvoidance(rns_R));
nsjla->setWeight(2, 0);
ik.addNullspaceGradient(nsjla);
CompositeDiffIK::Result result = ik.solve(pc);
@@ -548,7 +593,8 @@ namespace armarx
for (size_t i = 0; i < ikStats.size(); i++)
{
- ARMARX_IMPORTANT << ikStats.at(i).name << " solved: " << ikStats.at(i).solved << ", T: " << ikStats.at(i).duration;
+ ARMARX_IMPORTANT << ikStats.at(i).name << " solved: " << ikStats.at(i).solved
+ << ", T: " << ikStats.at(i).duration;
}
//ARMARX_IMPORTANT << "NaturalIK solved: " << ikStats.at(0).solved << ", T: " << durations.at(0);
@@ -559,14 +605,12 @@ namespace armarx
//ARMARX_IMPORTANT << VAROUT(ikStats.at(0).averageElb());
//ARMARX_IMPORTANT << VAROUT(ikStats.at(1).solved);
//ARMARX_IMPORTANT << VAROUT(ikStats.at(1).averageElb());
-
}
struct Side
{
viz::Layer layer;
Eigen::Vector3f shoulder;
-
};
struct ShoulderAngles
@@ -575,7 +619,8 @@ namespace armarx
NaturalIK::SoechtingForwardPositions fwd;
};
- void NaturalIKTest::vizTaskRun()
+ void
+ NaturalIKTest::vizTaskRun()
{
//ARMARX_IMPORTANT << "vizTask starts";
viz::Client arviz(*this);
@@ -583,14 +628,16 @@ namespace armarx
viz::Layer layer_iksteps = arviz.layer("ikSteps");
viz::Layer layer_robot = arviz.layer("Robot");
- viz::Robot vizrobot = viz::Robot("robot")
- .position(Eigen::Vector3f::Zero())
- .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
+ viz::Robot vizrobot =
+ viz::Robot("robot")
+ .position(Eigen::Vector3f::Zero())
+ .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
vizrobot.useFullModel();
layer_robot.add(vizrobot);
CMakePackageFinder finder("armar6_rt");
- std::string robotFile = finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
+ std::string robotFile =
+ finder.getDataDir() + "/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml";
ARMARX_IMPORTANT << "loading robot from " << robotFile;
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile);
@@ -612,10 +659,11 @@ namespace armarx
//layer.add(robot);
- float upper_arm_length = (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
+ float upper_arm_length =
+ (sho1_R->getPositionInRootFrame() - elb_R->getPositionInRootFrame()).norm();
//float lower_arm_length = (elb_R->getPositionInRootFrame() - wri1_R->getPositionInRootFrame()).norm();
- float lower_arm_length = (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
-
+ float lower_arm_length =
+ (elb_R->getPositionInRootFrame() - rns_R->getTCP()->getPositionInRootFrame()).norm();
viz::Layer layer_R = arviz.layer("Right");
@@ -637,7 +685,6 @@ namespace armarx
ik_L.setLowerArmLength(lower_arm_length);
-
NaturalIK::ArmJoints arm_R;
arm_R.rns = rns_R;
arm_R.elbow = elb_R;
@@ -649,7 +696,8 @@ namespace armarx
arm_L.tcp = rns_L->getTCP();
- auto makeVizSide = [&](viz::Layer & layer, NaturalIK & ik, Eigen::Vector3f target, Eigen::Vector3f & err)
+ auto makeVizSide =
+ [&](viz::Layer& layer, NaturalIK& ik, Eigen::Vector3f target, Eigen::Vector3f& err)
{
ik.setScale(p.p_R.scale);
@@ -659,8 +707,10 @@ namespace armarx
err = target - fwd.wrist;
vtgt = vtgt + 1.0 * err;
- layer.add(viz::Box("Wrist_orig").position(fwd.wrist)
- .size(20).color(viz::Color::fromRGBA(0, 0, 120)));
+ layer.add(viz::Box("Wrist_orig")
+ .position(fwd.wrist)
+ .size(20)
+ .color(viz::Color::fromRGBA(0, 0, 120)));
fwd = ik.forwardKinematics(ik.CalculateSoechtingAngles(vtgt));
err = target - fwd.wrist;
@@ -668,15 +718,17 @@ namespace armarx
layer.clear();
- layer.add(viz::Box("Shoulder").position(ik.getShoulderPos())
- .size(20).color(viz::Color::fromRGBA(255, 0, 0)));
- layer.add(viz::Box("Elbow").position(fwd.elbow)
- .size(20).color(viz::Color::fromRGBA(255, 0, 255)));
- layer.add(viz::Box("Wrist").position(fwd.wrist)
- .size(20).color(viz::Color::fromRGBA(0, 0, 255)));
+ layer.add(viz::Box("Shoulder")
+ .position(ik.getShoulderPos())
+ .size(20)
+ .color(viz::Color::fromRGBA(255, 0, 0)));
+ layer.add(viz::Box("Elbow").position(fwd.elbow).size(20).color(
+ viz::Color::fromRGBA(255, 0, 255)));
+ layer.add(viz::Box("Wrist").position(fwd.wrist).size(20).color(
+ viz::Color::fromRGBA(0, 0, 255)));
- layer.add(viz::Box("Target").position(target)
- .size(20).color(viz::Color::fromRGBA(255, 165, 0)));
+ layer.add(viz::Box("Target").position(target).size(20).color(
+ viz::Color::fromRGBA(255, 165, 0)));
viz::Cylinder arrUpperArm = viz::Cylinder("UpperArm");
arrUpperArm.fromTo(ik.getShoulderPos(), fwd.elbow);
@@ -688,8 +740,6 @@ namespace armarx
layer.add(arrUpperArm);
layer.add(arrLowerArm);
-
-
};
CycleUtil c(20);
@@ -721,12 +771,16 @@ namespace armarx
0.22703713178634644);
Eigen::Quaternionf referenceOri_L = mirrorOri(referenceOri_R);
- Eigen::AngleAxisf aa_R(p.p_R.targetRotation.norm() / 180 * M_PI, p.p_R.targetRotation.normalized());
- Eigen::Matrix3f targetOri_R = referenceOri_R.toRotationMatrix() * aa_R.toRotationMatrix();
+ Eigen::AngleAxisf aa_R(p.p_R.targetRotation.norm() / 180 * M_PI,
+ p.p_R.targetRotation.normalized());
+ Eigen::Matrix3f targetOri_R =
+ referenceOri_R.toRotationMatrix() * aa_R.toRotationMatrix();
Eigen::Matrix4f target_R = math::Helpers::Pose(p.p_R.target + offset, targetOri_R);
- Eigen::AngleAxisf aa_L(p.p_L.targetRotation.norm() / 180 * M_PI, p.p_L.targetRotation.normalized());
- Eigen::Matrix3f targetOri_L = referenceOri_L.toRotationMatrix() * aa_L.toRotationMatrix();
+ Eigen::AngleAxisf aa_L(p.p_L.targetRotation.norm() / 180 * M_PI,
+ p.p_L.targetRotation.normalized());
+ Eigen::Matrix3f targetOri_L =
+ referenceOri_L.toRotationMatrix() * aa_L.toRotationMatrix();
Eigen::Matrix4f target_L = math::Helpers::Pose(p.p_L.target + offset, targetOri_L);
// X_Platform
@@ -749,9 +803,10 @@ namespace armarx
// ArmR7_Wri1
// ArmR8_Wri2
- auto calcShoulderAngles = [&](NaturalIK & natik, Eigen::Matrix4f target)
+ auto calcShoulderAngles = [&](NaturalIK& natik, Eigen::Matrix4f target)
{
- NaturalIK::SoechtingForwardPositions fwd = natik.solveSoechtingIK(math::Helpers::Position(target));
+ NaturalIK::SoechtingForwardPositions fwd =
+ natik.solveSoechtingIK(math::Helpers::Position(target));
Eigen::Vector3f elb = fwd.elbow - fwd.shoulder;
float SE = fwd.soechtingAngles.SE;
elb = Eigen::AngleAxisf(-SE, Eigen::Vector3f::UnitX()) * elb;
@@ -771,12 +826,17 @@ namespace armarx
CompositeDiffIK cik(rnsP);
- VirtualRobot::IKSolver::CartesianSelection mode_R = p.p_R.setOri ? VirtualRobot::IKSolver::CartesianSelection::All : VirtualRobot::IKSolver::CartesianSelection::Position;
- VirtualRobot::IKSolver::CartesianSelection mode_L = p.p_L.setOri ? VirtualRobot::IKSolver::CartesianSelection::All : VirtualRobot::IKSolver::CartesianSelection::Position;
+ VirtualRobot::IKSolver::CartesianSelection mode_R =
+ p.p_R.setOri ? VirtualRobot::IKSolver::CartesianSelection::All
+ : VirtualRobot::IKSolver::CartesianSelection::Position;
+ VirtualRobot::IKSolver::CartesianSelection mode_L =
+ p.p_L.setOri ? VirtualRobot::IKSolver::CartesianSelection::All
+ : VirtualRobot::IKSolver::CartesianSelection::Position;
CompositeDiffIK::TargetPtr tgt_R = cik.addTarget(arm_R.tcp, target_R, mode_R);
CompositeDiffIK::TargetPtr tgt_L = cik.addTarget(arm_L.tcp, target_L, mode_L);
- CompositeDiffIK::NullspaceJointTargetPtr nsjt(new CompositeDiffIK::NullspaceJointTarget(rnsP));
+ CompositeDiffIK::NullspaceJointTargetPtr nsjt(
+ new CompositeDiffIK::NullspaceJointTarget(rnsP));
//NaturalIK::SoechtingForwardPositions fwd_R = ik_R.solveSoechtingIK(math::Helpers::Position(target_R));
//Eigen::Vector3f elb_R = fwd_R.elbow - fwd_R.shoulder;
//float SE = fwd_R.soechtingAngles.SE;
@@ -808,12 +868,15 @@ namespace armarx
// VirtualRobot::IKSolver::CartesianSelection::Position));
//nst_R->kP = 0;
//cik.addNullspaceGradient(nst_R);
- CompositeDiffIK::NullspaceTargetPtr nst_R = cik.addNullspacePositionTarget(arm_R.elbow, sa_R.fwd.elbow);
+ CompositeDiffIK::NullspaceTargetPtr nst_R =
+ cik.addNullspacePositionTarget(arm_R.elbow, sa_R.fwd.elbow);
nst_R->kP = 0;
- CompositeDiffIK::NullspaceTargetPtr nst_L = cik.addNullspacePositionTarget(arm_L.elbow, sa_L.fwd.elbow);
+ CompositeDiffIK::NullspaceTargetPtr nst_L =
+ cik.addNullspacePositionTarget(arm_L.elbow, sa_L.fwd.elbow);
nst_L->kP = 0;
- CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(new CompositeDiffIK::NullspaceJointLimitAvoidance(rnsP));
+ CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
+ new CompositeDiffIK::NullspaceJointLimitAvoidance(rnsP));
nsjla->setWeight(0, 0);
nsjla->setWeight(1, 0);
nsjla->setWeight(2, 0);
@@ -830,25 +893,36 @@ namespace armarx
int i = 0;
for (const CompositeDiffIK::TargetStep& s : tgt_R->ikSteps)
{
- layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
+ layer_iksteps.add(viz::Box("tcp_" + std::to_string(i))
+ .size(20)
+ .pose(s.tcpPose)
+ .color(viz::Color::blue()));
i++;
}
for (const CompositeDiffIK::TargetStep& s : tgt_L->ikSteps)
{
- layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
+ layer_iksteps.add(viz::Box("tcp_" + std::to_string(i))
+ .size(20)
+ .pose(s.tcpPose)
+ .color(viz::Color::blue()));
i++;
}
for (const CompositeDiffIK::NullspaceTargetStep& s : nst_R->ikSteps)
{
- layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
+ layer_iksteps.add(viz::Box("elb_" + std::to_string(i))
+ .size(20)
+ .pose(s.tcpPose)
+ .color(viz::Color::blue()));
i++;
}
for (const CompositeDiffIK::NullspaceTargetStep& s : nst_L->ikSteps)
{
- layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
+ layer_iksteps.add(viz::Box("elb_" + std::to_string(i))
+ .size(20)
+ .pose(s.tcpPose)
+ .color(viz::Color::blue()));
i++;
}
-
}
else
{
@@ -862,7 +936,8 @@ namespace armarx
}
else
{
- ikResult = ik_R.calculateIKpos(math::Helpers::Position(target_R), arm_R, p.p_R.ikparams);
+ ikResult = ik_R.calculateIKpos(
+ math::Helpers::Position(target_R), arm_R, p.p_R.ikparams);
}
layer_iksteps.clear();
@@ -870,16 +945,22 @@ namespace armarx
int i = 0;
for (const NaturalDiffIK::IKStep& s : ikResult.ikSteps)
{
- ss << s.pdTcp.norm() << "; " << s.odTcp.norm() << "; " << s.pdElb.norm() << "\n";
- layer_iksteps.add(viz::Box("tcp_" + std::to_string(i)).size(20).pose(s.tcpPose).color(viz::Color::blue()));
- layer_iksteps.add(viz::Box("elb_" + std::to_string(i)).size(20).pose(s.elbPose).color(viz::Color::blue()));
+ ss << s.pdTcp.norm() << "; " << s.odTcp.norm() << "; " << s.pdElb.norm()
+ << "\n";
+ layer_iksteps.add(viz::Box("tcp_" + std::to_string(i))
+ .size(20)
+ .pose(s.tcpPose)
+ .color(viz::Color::blue()));
+ layer_iksteps.add(viz::Box("elb_" + std::to_string(i))
+ .size(20)
+ .pose(s.elbPose)
+ .color(viz::Color::blue()));
i++;
}
//ARMARX_IMPORTANT << ss.str();
}
-
vizrobot.joints(rnsP->getJointValueMap());
@@ -891,8 +972,8 @@ namespace armarx
}
}
-
- Eigen::Matrix4f NaturalIKTest::mirrorPose(Eigen::Matrix4f oldPose)
+ Eigen::Matrix4f
+ NaturalIKTest::mirrorPose(Eigen::Matrix4f oldPose)
{
Eigen::Quaternionf oriOld(oldPose.block<3, 3>(0, 0));
Eigen::Quaternionf ori;
@@ -903,14 +984,17 @@ namespace armarx
ori.z() = oriOld.w();
Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
- pose.block<3, 3>(0, 0) = (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())).toRotationMatrix();
+ pose.block<3, 3>(0, 0) =
+ (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())).toRotationMatrix();
pose(0, 3) = -oldPose(0, 3);
pose(1, 3) = oldPose(1, 3);
pose(2, 3) = oldPose(2, 3);
return pose;
}
- Eigen::Quaternionf NaturalIKTest::mirrorOri(Eigen::Quaternionf oriOld)
+
+ Eigen::Quaternionf
+ NaturalIKTest::mirrorOri(Eigen::Quaternionf oriOld)
{
Eigen::Quaternionf ori;
@@ -922,19 +1006,15 @@ namespace armarx
return (ori * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()));
}
-
-
- void NaturalIKTest::onExitComponent()
+ void
+ NaturalIKTest::onExitComponent()
{
-
}
-
-
-
- armarx::PropertyDefinitionsPtr NaturalIKTest::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ NaturalIKTest::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new NaturalIKTestPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new NaturalIKTestPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
index ca8a91ddf88ef9ce10c92027f10e2f1f69cd8b56..a24db33047840f5f120dcadabb2a63af0139fdc9 100644
--- a/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
+++ b/source/RobotAPI/components/NaturalIKTest/NaturalIKTest.h
@@ -24,7 +24,6 @@
#include <ArmarXCore/core/Component.h>
-
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/RemoteGuiComponentPlugin.h>
@@ -32,25 +31,20 @@
#include <Eigen/Dense>
#include <RobotAPI/libraries/diffik/NaturalDiffIK.h>
-
#include <RobotAPI/libraries/natik/NaturalIK.h>
-
namespace armarx
{
/**
* @class NaturalIKTestPropertyDefinitions
* @brief
*/
- class NaturalIKTestPropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
+ class NaturalIKTestPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
NaturalIKTestPropertyDefinitions(std::string prefix);
};
-
-
/**
* @defgroup Component-NaturalIKTest NaturalIKTest
* @ingroup RobotAPI-Components
@@ -67,7 +61,6 @@ namespace armarx
virtual public RemoteGuiComponentPluginUser
{
public:
-
struct GuiSideParams
{
Eigen::Vector3f target;
@@ -86,7 +79,6 @@ namespace armarx
Eigen::Vector3f err_R = Eigen::Vector3f::Zero(), err_L = Eigen::Vector3f::Zero();
std::atomic_bool useCompositeIK;
-
};
/// @see armarx::ManagedIceObject::getDefaultName()
@@ -95,8 +87,8 @@ namespace armarx
RemoteGui::WidgetPtr buildGui();
void processGui(RemoteGui::TabProxy& prx);
- protected:
+ protected:
/// @see armarx::ManagedIceObject::onInitComponent()
virtual void onInitComponent() override;
@@ -119,7 +111,6 @@ namespace armarx
private:
-
// Private methods and member variables go here.
/// Debug observer. Used to visualize e.g. time series.
@@ -131,4 +122,4 @@ namespace armarx
RunningTask<NaturalIKTest>::pointer_type runTestTask;
GuiParams p;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/NaturalIKTest/test/NaturalIKTestTest.cpp b/source/RobotAPI/components/NaturalIKTest/test/NaturalIKTestTest.cpp
index 486e0b8d5a631a4260988a6afe947c66f486656a..94a1da70e62889fe4fef8cd6acd0236726b8ca2f 100644
--- a/source/RobotAPI/components/NaturalIKTest/test/NaturalIKTestTest.cpp
+++ b/source/RobotAPI/components/NaturalIKTest/test/NaturalIKTestTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/NaturalIKTest/NaturalIKTest.h>
#include <iostream>
+#include <RobotAPI/components/NaturalIKTest/NaturalIKTest.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::NaturalIKTest instance;
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/Editor.cpp b/source/RobotAPI/components/ObjectMemoryEditor/Editor.cpp
index 74bdd96793868911322b960af2973a0576809e93..d3c715a2a9d988893d46ef2b59ea77f368d717f8 100644
--- a/source/RobotAPI/components/ObjectMemoryEditor/Editor.cpp
+++ b/source/RobotAPI/components/ObjectMemoryEditor/Editor.cpp
@@ -1,24 +1,24 @@
#include "Editor.h"
+#include <utility>
+
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <utility>
-
namespace armarx
{
Editor::Editor(viz::Client& client,
Properties properties,
- std::function<void(objpose::ProvidedObjectPoseSeq &)> pushToMemory,
- std::function<objpose::ObjectPoseSeq(void)> pullFromMemory)
- : properties(std::move(properties))
- , client(client)
- , pushToMemory(std::move(pushToMemory))
- , pullFromMemory(std::move(pullFromMemory))
- , isCommitRequired(false)
- , isUpdateRequired(true)
- , isMemoryVizRequired(true)
- , isMetaVizRequired(true)
+ std::function<void(objpose::ProvidedObjectPoseSeq&)> pushToMemory,
+ std::function<objpose::ObjectPoseSeq(void)> pullFromMemory) :
+ properties(std::move(properties)),
+ client(client),
+ pushToMemory(std::move(pushToMemory)),
+ pullFromMemory(std::move(pullFromMemory)),
+ isCommitRequired(false),
+ isUpdateRequired(true),
+ isMemoryVizRequired(true),
+ isMetaVizRequired(true)
{
placeholderOptions.emplace_back("Remove");
placeholderOptions.emplace_back(lineString);
@@ -29,7 +29,8 @@ namespace armarx
}
}
- void Editor::step()
+ void
+ Editor::step()
{
viz::StagedCommit stage = client.stage();
@@ -87,14 +88,16 @@ namespace armarx
return newRequestedPoses;
}
- void Editor::reset()
+ void
+ Editor::reset()
{
changes.clear();
isMemoryVizRequired = true;
}
- void Editor::commit()
+ void
+ Editor::commit()
{
changes.moveNewObjectsTo(storedPoses);
@@ -102,7 +105,7 @@ namespace armarx
objpose::ObjectPoseSeq remainingPoses;
remainingPoses.reserve(storedPoses.size());
- for (objpose::ObjectPose & current : storedPoses)
+ for (objpose::ObjectPose& current : storedPoses)
{
bool isChanged = changes.applyTo(current);
@@ -129,11 +132,12 @@ namespace armarx
isMemoryVizRequired = true;
}
- void Editor::visualizeMemory()
+ void
+ Editor::visualizeMemory()
{
observer.clearObservedLayer(memoryLayer);
- for (objpose::ObjectPose & objectPose: storedPoses)
+ for (objpose::ObjectPose& objectPose : storedPoses)
{
if (objectPose.confidence > properties.confidenceThreshold)
{
@@ -144,7 +148,8 @@ namespace armarx
changes.visualizeNewObjects();
}
- void Editor::visualizeObject(objpose::ObjectPose& objectPose)
+ void
+ Editor::visualizeObject(objpose::ObjectPose& objectPose)
{
VisualizationDescription description = changes.buildVisualizationDescription(objectPose);
@@ -160,12 +165,11 @@ namespace armarx
interaction.contextMenu(description.options);
}
- viz::Object object =
- viz::Object(objectPose.objectID.str())
- .pose(description.transform * objectPose.objectPoseGlobal)
- .scale(properties.objectScaling)
- .fileByObjectFinder(objectPose.objectID)
- .enable(interaction);
+ viz::Object object = viz::Object(objectPose.objectID.str())
+ .pose(description.transform * objectPose.objectPoseGlobal)
+ .scale(properties.objectScaling)
+ .fileByObjectFinder(objectPose.objectID)
+ .enable(interaction);
if (description.alpha.has_value())
{
@@ -178,107 +182,114 @@ namespace armarx
}
observer.addObserved(memoryLayer, object)
- .onContextMenu(description.cloneIndex, [this, &objectPose]
- {
- changes.cloneObject(objectPose);
- isMemoryVizRequired = true;
- })
- .onContextMenu(description.deleteIndex, [this, &objectPose]
- {
- changes.deleteObject(objectPose);
- isMemoryVizRequired = true;
- })
- .onContextMenu(description.resetIndex, [this, &objectPose]
- {
- changes.resetObject(objectPose);
- isMemoryVizRequired = true;
- })
- .onContextMenu(description.prototypeIndex, [this, &objectPose]
- {
- const float defaultExtents = 100;
- simox::OrientedBoxf box(objectPose.objectPoseGlobal, Eigen::Vector3f(defaultExtents, defaultExtents, defaultExtents));
-
- box = objectPose.oobbGlobal().value_or(box);
- box = box.transformed(changes.getTransform(objectPose));
-
- placeholders.addPlaceholder(box);
- isMetaVizRequired = true;
- })
- .onContextMenu(description.commitIndex, [this]
- {
- isCommitRequired = true;
- })
- .onContextMenu(description.updateIndex, [this]
- {
- isUpdateRequired = true;
- })
- .onContextMenu(description.resetAllIndex, [this]
- {
- isResetRequired = true;
- })
- .onTransformEnd([this, &objectPose](const Eigen::Matrix4f& transform)
- {
- changes.moveObject(objectPose, transform);
- isMemoryVizRequired = true;
- });
+ .onContextMenu(description.cloneIndex,
+ [this, &objectPose]
+ {
+ changes.cloneObject(objectPose);
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(description.deleteIndex,
+ [this, &objectPose]
+ {
+ changes.deleteObject(objectPose);
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(description.resetIndex,
+ [this, &objectPose]
+ {
+ changes.resetObject(objectPose);
+ isMemoryVizRequired = true;
+ })
+ .onContextMenu(description.prototypeIndex,
+ [this, &objectPose]
+ {
+ const float defaultExtents = 100;
+ simox::OrientedBoxf box(
+ objectPose.objectPoseGlobal,
+ Eigen::Vector3f(defaultExtents, defaultExtents, defaultExtents));
+
+ box = objectPose.oobbGlobal().value_or(box);
+ box = box.transformed(changes.getTransform(objectPose));
+
+ placeholders.addPlaceholder(box);
+ isMetaVizRequired = true;
+ })
+ .onContextMenu(description.commitIndex, [this] { isCommitRequired = true; })
+ .onContextMenu(description.updateIndex, [this] { isUpdateRequired = true; })
+ .onContextMenu(description.resetAllIndex, [this] { isResetRequired = true; })
+ .onTransformEnd(
+ [this, &objectPose](const Eigen::Matrix4f& transform)
+ {
+ changes.moveObject(objectPose, transform);
+ isMemoryVizRequired = true;
+ });
}
- void Editor::visualizeMeta()
+ void
+ Editor::visualizeMeta()
{
observer.clearObservedLayer(metaLayer);
placeholders.visualizePlaceholders();
}
- void Editor::visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id)
+ void
+ Editor::visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id)
{
- viz::InteractionDescription interaction = viz::interaction()
- .selection().transform().hideDuringTransform().contextMenu(placeholderOptions);
+ viz::InteractionDescription interaction =
+ viz::interaction().selection().transform().hideDuringTransform().contextMenu(
+ placeholderOptions);
viz::Box box = viz::Box("placeholder_" + std::to_string(id))
- .set(placeholder.box.transformed(placeholder.transform))
- .color(simox::Color::yellow(255, 128))
- .enable(interaction);
+ .set(placeholder.box.transformed(placeholder.transform))
+ .color(simox::Color::yellow(255, 128))
+ .enable(interaction);
auto& observation = observer.addObserved(metaLayer, box)
- .onContextMenu(0, [this, id]()
- {
- placeholders.removePlaceholder(id);
- isMetaVizRequired = true;
- })
- .onTransformEnd([this, id](Eigen::Matrix4f const& transform)
- {
- placeholders.movePlaceholder(id, transform);
- isMetaVizRequired = true;
- });
+ .onContextMenu(0,
+ [this, id]()
+ {
+ placeholders.removePlaceholder(id);
+ isMetaVizRequired = true;
+ })
+ .onTransformEnd(
+ [this, id](Eigen::Matrix4f const& transform)
+ {
+ placeholders.movePlaceholder(id, transform);
+ isMetaVizRequired = true;
+ });
for (size_t index = 2; index < placeholderOptions.size(); index++)
{
std::string const& object = placeholderOptions[index];
- observation.onContextMenu(index, [this, id, &object]
- {
- placeholders.specifyObject(id, object, changes);
-
- isMetaVizRequired = true;
- isMemoryVizRequired = true;
- });
+ observation.onContextMenu(index,
+ [this, id, &object]
+ {
+ placeholders.specifyObject(id, object, changes);
+
+ isMetaVizRequired = true;
+ isMemoryVizRequired = true;
+ });
}
}
- void ChangeState::clear()
+ void
+ ChangeState::clear()
{
changed.clear();
newPoses.clear();
}
- void ChangeState::moveNewObjectsTo(objpose::ObjectPoseSeq& seq)
+ void
+ ChangeState::moveNewObjectsTo(objpose::ObjectPoseSeq& seq)
{
seq.insert(seq.begin(), newPoses.begin(), newPoses.end());
newPoses.clear();
}
- bool ChangeState::applyTo(objpose::ObjectPose& pose)
+ bool
+ ChangeState::applyTo(objpose::ObjectPose& pose)
{
auto iterator = changed.find(pose.objectID.str());
bool isChanged = iterator != changed.end();
@@ -298,15 +309,17 @@ namespace armarx
return isChanged;
}
- void ChangeState::visualizeNewObjects()
+ void
+ ChangeState::visualizeNewObjects()
{
- for (objpose::ObjectPose & objectPose: newPoses)
+ for (objpose::ObjectPose& objectPose : newPoses)
{
editor->visualizeObject(objectPose);
}
}
- VisualizationDescription ChangeState::buildVisualizationDescription(objpose::ObjectPose& object)
+ VisualizationDescription
+ ChangeState::buildVisualizationDescription(objpose::ObjectPose& object)
{
VisualizationDescription description;
@@ -378,19 +391,23 @@ namespace armarx
return description;
}
- void ChangeState::cloneObject(objpose::ObjectPose const& object)
+ void
+ ChangeState::cloneObject(objpose::ObjectPose const& object)
{
std::string suffix = std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
- std::chrono::system_clock::now().time_since_epoch()).count());
+ std::chrono::system_clock::now().time_since_epoch())
+ .count());
objpose::ObjectPose& newPose = newPoses.emplace_back(object);
- newPose.objectID = object.objectID.withInstanceName(object.objectID.instanceName() + suffix);
+ newPose.objectID =
+ object.objectID.withInstanceName(object.objectID.instanceName() + suffix);
const float minOffset = 100;
float offset = minOffset;
if (object.localOOBB.has_value())
{
- Eigen::Vector3f size = object.localOOBB.value().corner_max() - object.localOOBB.value().corner_min();
+ Eigen::Vector3f size =
+ object.localOOBB.value().corner_max() - object.localOOBB.value().corner_min();
float objectOffset = size.maxCoeff() / 2;
offset = std::max(minOffset, objectOffset);
@@ -409,11 +426,13 @@ namespace armarx
clonedChange.transform *= originalChange.transform;
}
}
-
- void ChangeState::createObject(const std::string &objectID, const Eigen::Matrix4f &pose)
+
+ void
+ ChangeState::createObject(const std::string& objectID, const Eigen::Matrix4f& pose)
{
std::string suffix = std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
- std::chrono::system_clock::now().time_since_epoch()).count());
+ std::chrono::system_clock::now().time_since_epoch())
+ .count());
objpose::ObjectPose& newPose = newPoses.emplace_back();
@@ -435,7 +454,8 @@ namespace armarx
if (newPose.localOOBB.has_value())
{
- newPose.objectPoseGlobal = pose * newPose.localOOBB->transformation_centered().inverse();
+ newPose.objectPoseGlobal =
+ pose * newPose.localOOBB->transformation_centered().inverse();
}
}
@@ -445,12 +465,14 @@ namespace armarx
createdChange.iterator = std::prev(newPoses.end());
}
- void ChangeState::deleteObject(const objpose::ObjectPose& object)
+ void
+ ChangeState::deleteObject(const objpose::ObjectPose& object)
{
changed[object.objectID.str()].kind = DELETE;
}
- void ChangeState::resetObject(const objpose::ObjectPose& object)
+ void
+ ChangeState::resetObject(const objpose::ObjectPose& object)
{
auto iterator = changed.find(object.objectID.str());
if (iterator != changed.end())
@@ -466,13 +488,15 @@ namespace armarx
}
}
- void ChangeState::moveObject(const objpose::ObjectPose& object, const Eigen::Matrix4f& transform)
+ void
+ ChangeState::moveObject(const objpose::ObjectPose& object, const Eigen::Matrix4f& transform)
{
Change& change = changed[object.objectID.str()];
change.transform = transform * change.transform;
}
- Eigen::Matrix4f ChangeState::getTransform(const objpose::ObjectPose &object)
+ Eigen::Matrix4f
+ ChangeState::getTransform(const objpose::ObjectPose& object)
{
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
@@ -486,16 +510,18 @@ namespace armarx
return transform;
}
- void PlaceholderState::addPlaceholder(simox::OrientedBoxf box)
+ void
+ PlaceholderState::addPlaceholder(simox::OrientedBoxf box)
{
size_t id = getID();
auto& entry = placeholders[id];
- entry.placeholder = { .box = std::move(box), .transform = Eigen::Matrix4f::Identity() };
+ entry.placeholder = {.box = std::move(box), .transform = Eigen::Matrix4f::Identity()};
entry.isActive = true;
}
- void PlaceholderState::visualizePlaceholders()
+ void
+ PlaceholderState::visualizePlaceholders()
{
for (size_t id = 0; id < placeholders.size(); id++)
{
@@ -508,20 +534,23 @@ namespace armarx
}
}
- void PlaceholderState::movePlaceholder(size_t id, Eigen::Matrix4f const& transform)
+ void
+ PlaceholderState::movePlaceholder(size_t id, Eigen::Matrix4f const& transform)
{
auto& placeholder = placeholders[id].placeholder;
placeholder.transform = transform * placeholder.transform;
}
- void PlaceholderState::removePlaceholder(size_t id)
+ void
+ PlaceholderState::removePlaceholder(size_t id)
{
placeholders[id].isActive = false;
unusedIDs.push(id);
}
- size_t PlaceholderState::getID()
+ size_t
+ PlaceholderState::getID()
{
if (unusedIDs.empty())
{
@@ -535,13 +564,17 @@ namespace armarx
return id;
}
- void PlaceholderState::specifyObject(size_t id, std::string const& objectID, ChangeState& changeState)
+ void
+ PlaceholderState::specifyObject(size_t id,
+ std::string const& objectID,
+ ChangeState& changeState)
{
auto& placeholder = placeholders[id].placeholder;
- Eigen::Matrix4f pose = placeholder.box.transformed(placeholder.transform).transformation_centered();
+ Eigen::Matrix4f pose =
+ placeholder.box.transformed(placeholder.transform).transformation_centered();
changeState.createObject(objectID, pose);
-
+
removePlaceholder(id);
}
-} // namespace armarx
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/Editor.h b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
index 783675ba07f54dfe80ab569c8fe65a5b836ca23f..b2e16a2a91140c96b3f8f0002f9beedbc76703fa 100644
--- a/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
+++ b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
@@ -3,26 +3,25 @@
#include <list>
#include <queue>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
namespace armarx
{
class Editor;
-
struct VisualizationDescription
{
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
bool allowTransforming = true;
- std::vector<std::string> options {};
- std::optional<float> alpha {};
- std::optional<simox::Color> color {};
+ std::vector<std::string> options{};
+ std::optional<float> alpha{};
+ std::optional<simox::Color> color{};
// Context menu indices.
size_t cloneIndex = std::numeric_limits<size_t>::max();
@@ -34,12 +33,10 @@ namespace armarx
size_t resetAllIndex = std::numeric_limits<size_t>::max();
};
-
-
class ChangeState
{
public:
- explicit ChangeState(Editor* editor) : editor(editor) {} ;
+ explicit ChangeState(Editor* editor) : editor(editor){};
void clear();
void moveNewObjectsTo(objpose::ObjectPoseSeq& seq);
@@ -68,7 +65,7 @@ namespace armarx
{
ChangeKind kind = MOVE;
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
- ObjectPoseList::iterator iterator {};
+ ObjectPoseList::iterator iterator{};
};
Editor* editor;
@@ -77,12 +74,10 @@ namespace armarx
std::map<std::string, Change> changed;
};
-
-
class PlaceholderState
{
public:
- explicit PlaceholderState(Editor* editor) : editor(editor) {} ;
+ explicit PlaceholderState(Editor* editor) : editor(editor){};
struct Placeholder
{
@@ -102,16 +97,16 @@ namespace armarx
Editor* editor;
std::priority_queue<size_t, std::vector<size_t>, std::greater<>> unusedIDs;
+
struct PlaceholderEntry
{
Placeholder placeholder;
bool isActive;
};
+
std::vector<PlaceholderEntry> placeholders;
};
-
-
class Editor
{
public:
@@ -127,7 +122,6 @@ namespace armarx
public:
-
explicit Editor(viz::Client& client,
Properties properties,
std::function<void(objpose::ProvidedObjectPoseSeq&)> pushToMemory,
@@ -135,7 +129,6 @@ namespace armarx
void step();
private:
-
void visualizeMemory();
void visualizeMeta();
@@ -143,7 +136,7 @@ namespace armarx
objpose::ObjectPoseSeq update();
void reset();
- void visualizeObject(objpose::ObjectPose &objectPose);
+ void visualizeObject(objpose::ObjectPose& objectPose);
void visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id);
private:
@@ -160,14 +153,14 @@ namespace armarx
const std::function<void(objpose::ProvidedObjectPoseSeq&)> pushToMemory;
const std::function<objpose::ObjectPoseSeq(void)> pullFromMemory;
- viz::Layer memoryLayer {client.layer(memoryLayerName)};
- viz::Layer metaLayer {client.layer(metaLayerName)};
+ viz::Layer memoryLayer{client.layer(memoryLayerName)};
+ viz::Layer metaLayer{client.layer(metaLayerName)};
InteractionObserver observer;
objpose::ObjectPoseSeq storedPoses;
- ChangeState changes{this};
+ ChangeState changes{this};
PlaceholderState placeholders{this};
bool isCommitRequired;
@@ -179,4 +172,4 @@ namespace armarx
friend ChangeState;
friend PlaceholderState;
};
-} // namespace armarx
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.cpp b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.cpp
index c226e57a72b071eaf93c45ea52e5b1906789a54c..255bf815593f6f589f6cd4e487cbd8c13f62cea1 100644
--- a/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.cpp
+++ b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.cpp
@@ -2,21 +2,23 @@
#include <utility>
-armarx::InteractionObserver::Observation &
+armarx::InteractionObserver::Observation&
armarx::InteractionObserver::Observation::onContextMenu(size_t index, std::function<void()> action)
{
contextMenuActions[index] = std::move(action);
return *this;
}
-armarx::InteractionObserver::Observation &
-armarx::InteractionObserver::Observation::onTransformEnd(std::function<void(Eigen::Matrix4f const&)> action)
+armarx::InteractionObserver::Observation&
+armarx::InteractionObserver::Observation::onTransformEnd(
+ std::function<void(Eigen::Matrix4f const&)> action)
{
transformEndAction = std::make_optional(std::move(action));
return *this;
}
-void armarx::InteractionObserver::Observation::process(viz::InteractionFeedback const& interaction)
+void
+armarx::InteractionObserver::Observation::process(viz::InteractionFeedback const& interaction)
{
if (interaction.type() == viz::InteractionFeedbackType::ContextMenuChosen)
{
@@ -29,7 +31,8 @@ void armarx::InteractionObserver::Observation::process(viz::InteractionFeedback
}
}
- if (interaction.type() == viz::InteractionFeedbackType::Transform && interaction.isTransformEnd())
+ if (interaction.type() == viz::InteractionFeedbackType::Transform &&
+ interaction.isTransformEnd())
{
if (transformEndAction.has_value())
{
@@ -38,13 +41,15 @@ void armarx::InteractionObserver::Observation::process(viz::InteractionFeedback
}
}
-void armarx::InteractionObserver::clearObservedLayer(armarx::viz::Layer & layer)
+void
+armarx::InteractionObserver::clearObservedLayer(armarx::viz::Layer& layer)
{
layer.clear();
observedLayers.erase(layer.data_.name);
}
-void armarx::InteractionObserver::requestInteractions(armarx::viz::StagedCommit& stage)
+void
+armarx::InteractionObserver::requestInteractions(armarx::viz::StagedCommit& stage)
{
for (auto& [name, observedLayer] : observedLayers)
{
@@ -52,9 +57,10 @@ void armarx::InteractionObserver::requestInteractions(armarx::viz::StagedCommit&
}
}
-void armarx::InteractionObserver::process(viz::InteractionFeedbackRange const& interactions)
+void
+armarx::InteractionObserver::process(viz::InteractionFeedbackRange const& interactions)
{
- for (viz::InteractionFeedback const& interaction: interactions)
+ for (viz::InteractionFeedback const& interaction : interactions)
{
auto layerIterator = observedLayers.find(interaction.layer());
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h
index b26ed2aca18754f22f5eead0ad345981b5ec0ae4..d9b5cc23d463463b83a99d201f4ed776b2de98e8 100644
--- a/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h
+++ b/source/RobotAPI/components/ObjectMemoryEditor/InteractionObserver.h
@@ -21,7 +21,8 @@ namespace armarx
};
template <typename ElementT>
- Observation& addObserved(viz::Layer & layer, ElementT const& element)
+ Observation&
+ addObserved(viz::Layer& layer, ElementT const& element)
{
layer.add(element);
@@ -36,15 +37,17 @@ namespace armarx
return observedLayer.observations[element.data_->id];
}
- void clearObservedLayer(viz::Layer & layer);
+ void clearObservedLayer(viz::Layer& layer);
- void requestInteractions(viz::StagedCommit & stage);
+ void requestInteractions(viz::StagedCommit& stage);
void process(viz::InteractionFeedbackRange const& interactions);
private:
struct ObservedLayer
{
- explicit ObservedLayer(viz::Layer & layer) : layer(layer) {}
+ explicit ObservedLayer(viz::Layer& layer) : layer(layer)
+ {
+ }
std::reference_wrapper<viz::Layer> layer;
std::map<std::string, Observation> observations;
@@ -52,4 +55,4 @@ namespace armarx
std::map<std::string, ObservedLayer> observedLayers;
};
-} // namespace armarx
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.cpp b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.cpp
index e723f5e467d7d6588084ee5366753d64ca112a03..95c0d40aaa044f959721107d799ffe47d52d4b41 100644
--- a/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.cpp
+++ b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.cpp
@@ -2,32 +2,38 @@
#include <ArmarXCore/core/time/Metronome.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include "Editor.h"
namespace armarx
{
- armarx::PropertyDefinitionsPtr ObjectMemoryEditor::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ ObjectMemoryEditor::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
- defs->optional(this->objectScaling, "Editor.ObjectScaling",
+ defs->optional(this->objectScaling,
+ "Editor.ObjectScaling",
"Scaling factor that is applied to all intractable objects.");
- defs->optional(this->confidenceThreshold, "Editor.ConfidenceThreshold",
+ defs->optional(this->confidenceThreshold,
+ "Editor.ConfidenceThreshold",
"Only objects with a confidence greater than this value are shown.");
return defs;
}
- std::string ObjectMemoryEditor::getDefaultName() const
+ std::string
+ ObjectMemoryEditor::getDefaultName() const
{
return "InteractiveMemoryEditor";
}
- void ObjectMemoryEditor::onInitComponent()
+ void
+ ObjectMemoryEditor::onInitComponent()
{
{
providerInfo.objectType = objpose::ObjectType::KnownObject;
@@ -36,7 +42,7 @@ namespace armarx
{
std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset(dataset);
- for (const auto& obj: objects)
+ for (const auto& obj : objects)
{
providerInfo.supportedObjects.push_back(armarx::toIce(obj.id()));
}
@@ -44,47 +50,43 @@ namespace armarx
}
}
- void ObjectMemoryEditor::onConnectComponent()
+ void
+ ObjectMemoryEditor::onConnectComponent()
{
setDebugObserverBatchModeEnabled(true);
- objectVizTask = new SimpleRunningTask<>([this]()
- {
- this->run();
- });
+ objectVizTask = new SimpleRunningTask<>([this]() { this->run(); });
objectVizTask->start();
}
- void ObjectMemoryEditor::onDisconnectComponent()
+ void
+ ObjectMemoryEditor::onDisconnectComponent()
{
}
- void ObjectMemoryEditor::onExitComponent()
+ void
+ ObjectMemoryEditor::onExitComponent()
{
}
-
- void ObjectMemoryEditor::run()
+ void
+ ObjectMemoryEditor::run()
{
objpose::ObjectPoseClient client = getClient();
- Editor::Properties properties =
- {
+ Editor::Properties properties = {
.providerName = getName(),
.objectScaling = objectScaling,
.confidenceThreshold = confidenceThreshold,
.availableObjects = providerInfo.supportedObjects,
};
- Editor editor(arviz, properties,
- [this](objpose::ProvidedObjectPoseSeq &poses)
- {
- objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(poses));
- },
- [client]() -> objpose::ObjectPoseSeq
- {
- return client.fetchObjectPoses();
- });
+ Editor editor(
+ arviz,
+ properties,
+ [this](objpose::ProvidedObjectPoseSeq& poses)
+ { objectPoseTopic->reportObjectPoses(getName(), objpose::toIce(poses)); },
+ [client]() -> objpose::ObjectPoseSeq { return client.fetchObjectPoses(); });
Metronome metronome(Frequency::Hertz(20));
while (objectVizTask and not objectVizTask->isStopped())
@@ -95,17 +97,19 @@ namespace armarx
}
}
- objpose::ProviderInfo ObjectMemoryEditor::getProviderInfo(const Ice::Current & /*unused*/)
+ objpose::ProviderInfo
+ ObjectMemoryEditor::getProviderInfo(const Ice::Current& /*unused*/)
{
return providerInfo;
}
- objpose::provider::RequestObjectsOutput ObjectMemoryEditor::requestObjects(
- const objpose::provider::RequestObjectsInput &input, const Ice::Current & /*unused*/)
+ objpose::provider::RequestObjectsOutput
+ ObjectMemoryEditor::requestObjects(const objpose::provider::RequestObjectsInput& input,
+ const Ice::Current& /*unused*/)
{
objpose::provider::RequestObjectsOutput output;
- for (const auto &id: input.objectIDs)
+ for (const auto& id : input.objectIDs)
{
output.results[id].success = false;
}
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.h b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.h
index 1cc90b97906890dced6bf29bd25a3c26fa164f3f..8c219c09c8f8fd49c0420fba4370c991492d1462 100644
--- a/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.h
+++ b/source/RobotAPI/components/ObjectMemoryEditor/ObjectMemoryEditor.h
@@ -3,22 +3,22 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
#include "InteractionObserver.h"
namespace armarx
{
class ObjectMemoryEditor :
- virtual public armarx::Component,
- virtual public armarx::DebugObserverComponentPluginUser,
- virtual public armarx::ArVizComponentPluginUser,
- virtual public armarx::ObjectPoseClientPluginUser,
- virtual public armarx::ObjectPoseProviderPluginUser
+ virtual public armarx::Component,
+ virtual public armarx::DebugObserverComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::ObjectPoseClientPluginUser,
+ virtual public armarx::ObjectPoseProviderPluginUser
{
public:
std::string getDefaultName() const override;
@@ -32,7 +32,9 @@ namespace armarx
public:
objpose::ProviderInfo getProviderInfo(const Ice::Current& = Ice::emptyCurrent) override;
- objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
+ objpose::provider::RequestObjectsOutput
+ requestObjects(const objpose::provider::RequestObjectsInput& input,
+ const Ice::Current&) override;
private:
void run();
@@ -45,4 +47,4 @@ namespace armarx
armarx::SimpleRunningTask<>::pointer_type objectVizTask;
};
-} // namespace armarx
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
index 10a8b7ae1b665966787ea6a056ab3f1056f8abeb..1c709c9f9645ba7bed10622d0fbf0a460ef356d9 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
@@ -30,53 +30,59 @@
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-
namespace armarx
{
- armarx::PropertyDefinitionsPtr ObjectPoseClientExample::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ ObjectPoseClientExample::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
- defs->optional(p.predictionsPerObject, "predictions.NumberPerObject",
+ defs->optional(p.predictionsPerObject,
+ "predictions.NumberPerObject",
"How many predictions with increasing time offsets to make per object.");
- defs->optional(p.millisecondPredictionIncrement, "predictions.TimeIncrement",
+ defs->optional(p.millisecondPredictionIncrement,
+ "predictions.TimeIncrement",
"The size of the prediction time offset increment in milliseconds.");
return defs;
}
- std::string ObjectPoseClientExample::getDefaultName() const
+ std::string
+ ObjectPoseClientExample::getDefaultName() const
{
return "ObjectPoseClientExample";
}
- void ObjectPoseClientExample::onInitComponent()
+ void
+ ObjectPoseClientExample::onInitComponent()
{
}
- void ObjectPoseClientExample::onConnectComponent()
+ void
+ ObjectPoseClientExample::onConnectComponent()
{
setDebugObserverBatchModeEnabled(true);
- objectProcessingTask = new SimpleRunningTask<>([this]()
- {
- this->objectProcessingTaskRun();
- });
+ objectProcessingTask =
+ new SimpleRunningTask<>([this]() { this->objectProcessingTaskRun(); });
objectProcessingTask->start();
}
- void ObjectPoseClientExample::onDisconnectComponent()
+ void
+ ObjectPoseClientExample::onDisconnectComponent()
{
}
- void ObjectPoseClientExample::onExitComponent()
+ void
+ ObjectPoseClientExample::onExitComponent()
{
}
-
- void ObjectPoseClientExample::objectProcessingTaskRun()
+ void
+ ObjectPoseClientExample::objectProcessingTaskRun()
{
CycleUtil cycle(50);
@@ -100,9 +106,9 @@ namespace armarx
for (const objpose::ObjectPose& objectPose : objectPoses)
{
layer.add(viz::Object(objectPose.objectID.str())
- .pose(objectPose.objectPoseGlobal)
- .fileByObjectFinder(objectPose.objectID)
- .alpha(objectPose.confidence));
+ .pose(objectPose.objectPoseGlobal)
+ .fileByObjectFinder(objectPose.objectID)
+ .alpha(objectPose.confidence));
}
stage.add(layer);
}
@@ -113,7 +119,6 @@ namespace armarx
}
}
-
viz::Layer
ObjectPoseClientExample::visualizePredictions(const objpose::ObjectPoseClient& client,
const objpose::ObjectPoseSeq& objectPoses)
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
index 8cc667b26bad490c8fb314a6ef759e52f850430d..ecee87a4cef1715ad73ddf7192a4cf2be0b9ef22 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
@@ -31,7 +31,6 @@
// Include the ClientPlugin
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
-
namespace armarx
{
@@ -48,21 +47,20 @@ namespace armarx
* Gets and visualizes object poses from the ObjectPoseStorage.
*/
class ObjectPoseClientExample :
- virtual public armarx::Component
- , virtual public armarx::DebugObserverComponentPluginUser
- , virtual public armarx::ArVizComponentPluginUser
+ virtual public armarx::Component,
+ virtual public armarx::DebugObserverComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser
// Derive from the client plugin.
- , virtual public armarx::ObjectPoseClientPluginUser
+ ,
+ virtual public armarx::ObjectPoseClientPluginUser
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
- protected:
-
+ protected:
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -72,7 +70,6 @@ namespace armarx
private:
-
void objectProcessingTaskRun();
viz::Layer visualizePredictions(const objpose::ObjectPoseClient& client,
@@ -80,7 +77,6 @@ namespace armarx
private:
-
armarx::SimpleRunningTask<>::pointer_type objectProcessingTask;
struct Properties
@@ -88,7 +84,7 @@ namespace armarx
int predictionsPerObject = 5; // NOLINT
int millisecondPredictionIncrement = 200; // NOLINT
};
- Properties p;
+ Properties p;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
index 9eb2a0dac0aadc72946c163c9d79148f799e5aaf..c04e69d8444ac76c025cde868841a3259dff6249 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseClientPlugin.h
@@ -1,5 +1,6 @@
#pragma once
-#pragma message("This header is deprecated. Use <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h> instead.")
+#pragma message( \
+ "This header is deprecated. Use <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h> instead.")
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
diff --git a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
index 55f6366e85ef20f50833f0889c6cf4b88c3614d2..91ad8594342f037e74263960f423919ce5a7a160 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/plugins/ObjectPoseProviderPlugin.h
@@ -1,5 +1,6 @@
#pragma once
-#pragma message("This header is deprecated. Use <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h> instead.")
+#pragma message( \
+ "This header is deprecated. Use <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h> instead.")
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
index bbf6438cb32566dae0436d22770f8f89dd7d43c9..c768a3c17e8e8d7bceb19788da4f89d05294ce84 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.cpp
@@ -27,35 +27,38 @@
#include <ArmarXCore/core/time/CycleUtil.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx
{
- armarx::PropertyDefinitionsPtr ObjectPoseProviderExample::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ ObjectPoseProviderExample::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
defs->optional(initialObjectIDs, "Objects", "Object IDs of objects to be tracked.")
- .map(simox::alg::join(initialObjectIDs, ", "), initialObjectIDs);
+ .map(simox::alg::join(initialObjectIDs, ", "), initialObjectIDs);
defs->optional(singleShot, "SingleShot", "If true, publishes only one snapshot.");
return defs;
}
- std::string ObjectPoseProviderExample::getDefaultName() const
+ std::string
+ ObjectPoseProviderExample::getDefaultName() const
{
return "ObjectPoseProviderExample";
}
- void ObjectPoseProviderExample::onInitComponent()
+ void
+ ObjectPoseProviderExample::onInitComponent()
{
for (const auto& initial : initialObjectIDs)
{
@@ -73,33 +76,35 @@ namespace armarx
}
}
- void ObjectPoseProviderExample::onConnectComponent()
+ void
+ ObjectPoseProviderExample::onConnectComponent()
{
- poseEstimationTask = new SimpleRunningTask<>([this]()
- {
- this->poseEstimationTaskRun();
- });
+ poseEstimationTask = new SimpleRunningTask<>([this]() { this->poseEstimationTaskRun(); });
poseEstimationTask->start();
}
- void ObjectPoseProviderExample::onDisconnectComponent()
+ void
+ ObjectPoseProviderExample::onDisconnectComponent()
{
}
- void ObjectPoseProviderExample::onExitComponent()
+ void
+ ObjectPoseProviderExample::onExitComponent()
{
}
- objpose::ProviderInfo ObjectPoseProviderExample::getProviderInfo(const Ice::Current&)
+ objpose::ProviderInfo
+ ObjectPoseProviderExample::getProviderInfo(const Ice::Current&)
{
return providerInfo;
}
- objpose::provider::RequestObjectsOutput ObjectPoseProviderExample::requestObjects(
- const objpose::provider::RequestObjectsInput& input, const Ice::Current&)
+ objpose::provider::RequestObjectsOutput
+ ObjectPoseProviderExample::requestObjects(const objpose::provider::RequestObjectsInput& input,
+ const Ice::Current&)
{
- ARMARX_INFO << "Requested object IDs for " << input.relativeTimeoutMS << " ms: "
- << input.objectIDs;
+ ARMARX_INFO << "Requested object IDs for " << input.relativeTimeoutMS
+ << " ms: " << input.objectIDs;
{
std::scoped_lock lock(requestedObjectsMutex);
requestedObjects.requestObjects(input.objectIDs, input.relativeTimeoutMS);
@@ -114,7 +119,8 @@ namespace armarx
return output;
}
- void ObjectPoseProviderExample::poseEstimationTaskRun()
+ void
+ ObjectPoseProviderExample::poseEstimationTaskRun()
{
CycleUtil cycle(50);
IceUtil::Time start = TimeUtil::GetTime();
@@ -134,8 +140,7 @@ namespace armarx
if (update.added.size() > 0 || update.removed.size() > 0)
{
- ARMARX_INFO << "Added: " << update.added
- << "Removed: " << update.removed;
+ ARMARX_INFO << "Added: " << update.added << "Removed: " << update.removed;
}
int i = 0;
@@ -161,7 +166,8 @@ namespace armarx
pose.objectID = info.id();
- Eigen::Vector3f pos = 200 * Eigen::Vector3f(std::sin(t - i), std::cos(t - i), 1 + i);
+ Eigen::Vector3f pos =
+ 200 * Eigen::Vector3f(std::sin(t - i), std::cos(t - i), 1 + i);
Eigen::AngleAxisf ori((t - i) / M_PI_2, Eigen::Vector3f::UnitZ());
pose.objectPose = simox::math::pose(pos, ori);
// pose.objectPoseFrame = armarx::GlobalFrame;
@@ -177,21 +183,25 @@ namespace armarx
pose.objectPoseGaussian->covariance.diagonal()(2) = 2.0 * posVar;
if (i % 2 == 1)
{
- Eigen::Matrix3f rot = Eigen::AngleAxisf(0.25 * (t - i), Eigen::Vector3f::UnitZ()).toRotationMatrix();
- pose.objectPoseGaussian->positionCovariance()
- = rot * pose.objectPoseGaussian->positionCovariance() * rot.transpose();
+ Eigen::Matrix3f rot =
+ Eigen::AngleAxisf(0.25 * (t - i), Eigen::Vector3f::UnitZ())
+ .toRotationMatrix();
+ pose.objectPoseGaussian->positionCovariance() =
+ rot * pose.objectPoseGaussian->positionCovariance() * rot.transpose();
}
// Rotational (co)variance.
- const float oriVar = (M_PI_4) + (M_PI_4) * std::sin(t - i);
+ const float oriVar = (M_PI_4) + (M_PI_4)*std::sin(t - i);
pose.objectPoseGaussian->covariance.diagonal()(3) = 1.0 * oriVar;
pose.objectPoseGaussian->covariance.diagonal()(4) = 4.0 * oriVar;
pose.objectPoseGaussian->covariance.diagonal()(5) = 2.0 * oriVar;
if (i % 2 == 1)
{
- Eigen::Matrix3f rot = Eigen::AngleAxisf(0.25 * (t - i), Eigen::Vector3f::UnitZ()).toRotationMatrix();
- pose.objectPoseGaussian->orientationCovariance()
- = rot * pose.objectPoseGaussian->orientationCovariance() * rot.transpose();
+ Eigen::Matrix3f rot =
+ Eigen::AngleAxisf(0.25 * (t - i), Eigen::Vector3f::UnitZ())
+ .toRotationMatrix();
+ pose.objectPoseGaussian->orientationCovariance() =
+ rot * pose.objectPoseGaussian->orientationCovariance() * rot.transpose();
}
pose.confidence = 0.75 + 0.25 * std::sin(t - i);
@@ -211,4 +221,4 @@ namespace armarx
cycle.waitForCycleDuration();
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
index 74cc8bb1bab63c7baf7373d084866597a7f0d000..8384bc019d4217f8d3aa6bd31987e7fc6f054aa8 100644
--- a/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
+++ b/source/RobotAPI/components/ObjectPoseProviderExample/ObjectPoseProviderExample.h
@@ -25,18 +25,15 @@
#include <mutex>
#include <ArmarXCore/core/Component.h>
-
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
// Include the ProviderPlugin
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-
namespace armarx
{
@@ -54,10 +51,10 @@ namespace armarx
class ObjectPoseProviderExample :
virtual public armarx::Component
// Derive from the provider plugin.
- , virtual public armarx::ObjectPoseProviderPluginUser
+ ,
+ virtual public armarx::ObjectPoseProviderPluginUser
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -65,11 +62,12 @@ namespace armarx
// Implement the ObjectPoseProvider interface
public:
objpose::ProviderInfo getProviderInfo(const Ice::Current& = Ice::emptyCurrent) override;
- objpose::provider::RequestObjectsOutput requestObjects(const objpose::provider::RequestObjectsInput& input, const Ice::Current&) override;
+ objpose::provider::RequestObjectsOutput
+ requestObjects(const objpose::provider::RequestObjectsInput& input,
+ const Ice::Current&) override;
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -87,12 +85,10 @@ namespace armarx
private:
-
void poseEstimationTaskRun();
private:
-
// To be moved to plugin
std::mutex requestedObjectsMutex;
objpose::RequestedObjects requestedObjects;
@@ -101,7 +97,7 @@ namespace armarx
objpose::ProviderInfo providerInfo;
armarx::ObjectFinder objectFinder;
- std::vector<std::string> initialObjectIDs = { "KIT/Amicelli", "KIT/YellowSaltCylinder" };
+ std::vector<std::string> initialObjectIDs = {"KIT/Amicelli", "KIT/YellowSaltCylinder"};
bool singleShot = false;
@@ -109,6 +105,5 @@ namespace armarx
/// Debug observer. Used to visualize e.g. time series.
armarx::DebugObserverInterfacePrx debugObserver;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotDefinition/test/RobotNameServiceTest.cpp b/source/RobotAPI/components/RobotDefinition/test/RobotNameServiceTest.cpp
index 6e3e6a973a04d105c2cf67478bab189ba7fbc2f4..6e3e4af634092c4fed53e14c5feefd26e7b1e4a1 100644
--- a/source/RobotAPI/components/RobotDefinition/test/RobotNameServiceTest.cpp
+++ b/source/RobotAPI/components/RobotDefinition/test/RobotNameServiceTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/RobotNameService/RobotNameService.h>
#include <iostream>
+#include <RobotAPI/components/RobotNameService/RobotNameService.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::RobotNameService instance;
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index 8c14b9de7a9d80bcf2b09d45ae3253b94a2c8b5b..80145abaaf12ff37d1daa9f6589f0c1c4ae2ffa0 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -21,22 +21,23 @@
*/
#include "RobotHealth.h"
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/time/Clock.h"
-#include "ArmarXCore/core/time/DateTime.h"
-#include "ArmarXCore/core/time/Duration.h"
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
#include <algorithm>
+#include <mutex>
+#include <optional>
#include <boost/regex.hpp>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/DateTime.h"
+#include "ArmarXCore/core/time/Duration.h"
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <mutex>
-#include <optional>
namespace armarx
{
@@ -56,7 +57,8 @@ namespace armarx
const std::vector<std::string> requiredTags = simox::alg::split(p.requiredTags, ",");
// a special requester: cannot be changed during runtime.
- tagsPerRequester[PROPERTY_REQUESTER_ID] = std::set<std::string>(requiredTags.begin(), requiredTags.end());
+ tagsPerRequester[PROPERTY_REQUESTER_ID] =
+ std::set<std::string>(requiredTags.begin(), requiredTags.end());
setDebugObserverBatchModeEnabled(true);
}
@@ -77,7 +79,7 @@ namespace armarx
ARMARX_TRACE;
auto now = armarx::core::time::DateTime::Now();
- for (auto & e : updateEntries)
+ for (auto& e : updateEntries)
{
ARMARX_TRACE;
const std::scoped_lock elock(e.mutex);
@@ -87,7 +89,7 @@ namespace armarx
continue;
}
- if(e.history.empty())
+ if (e.history.empty())
{
continue;
}
@@ -118,8 +120,8 @@ namespace armarx
}
else
{
- // If everything is OK (again), update health
- e.state = HealthOK;
+ // If everything is OK (again), update health
+ e.state = HealthOK;
}
}
@@ -128,15 +130,15 @@ namespace armarx
// get aggregated status
- for (auto & e : updateEntries)
+ for (auto& e : updateEntries)
{
ARMARX_TRACE;
- std::scoped_lock elock(e.mutex);
+ std::scoped_lock elock(e.mutex);
// trim history
while (e.history.size() > 20)
{
- e.history.pop_front();
+ e.history.pop_front();
}
if (e.required)
@@ -193,16 +195,21 @@ namespace armarx
new armarx::ComponentPropertyDefinitions(getConfigIdentifier()));
- defs->topic(p.emergencyStopTopicPrx, "EmergencyStop",
+ defs->topic(p.emergencyStopTopicPrx,
+ "EmergencyStop",
"The name of the topic over which changes of the emergencyStopState are sent.");
- defs->topic<RobotHealthInterfacePrx>(p.robotHealthTopicName, "RobotHealthTopic",
- "Name of the RobotHealth topic");
+ defs->topic<RobotHealthInterfacePrx>(
+ p.robotHealthTopicName, "RobotHealthTopic", "Name of the RobotHealth topic");
- defs->topic(p.aggregatedRobotHealthTopicPrx, "AggregatedRobotHealthTopic", "Name of the AggregatedRobotHealthTopic");
+ defs->topic(p.aggregatedRobotHealthTopicPrx,
+ "AggregatedRobotHealthTopic",
+ "Name of the AggregatedRobotHealthTopic");
- defs->optional(p.maximumCycleTimeWarnMS, "MaximumCycleTimeWarnMS",
+ defs->optional(p.maximumCycleTimeWarnMS,
+ "MaximumCycleTimeWarnMS",
"Default value of the maximum cycle time for warnings");
- defs->optional(p.maximumCycleTimeErrMS, "MaximumCycleTimeErrMS",
+ defs->optional(p.maximumCycleTimeErrMS,
+ "MaximumCycleTimeErrMS",
"Default value of the maximum cycle time for error");
defs->optional(p.requiredTags, "RequiredTags", "Tags that should be requested.");
@@ -215,7 +222,7 @@ namespace armarx
{
ARMARX_TRACE;
- for (auto & updateEntrie : updateEntries)
+ for (auto& updateEntrie : updateEntries)
{
if (updateEntrie.name == name)
{
@@ -231,7 +238,7 @@ namespace armarx
{
ARMARX_TRACE;
{
- for (auto & updateEntrie : updateEntries)
+ for (auto& updateEntrie : updateEntries)
{
if (updateEntrie.name == name)
{
@@ -264,7 +271,8 @@ namespace armarx
}
// else no messsage as components may spam sign up for legacy reasons (when there was no sign up)
- armarx::core::time::fromIce(args.maximumCycleTimeWarning, e.second.maximumCycleTimeWarn);
+ armarx::core::time::fromIce(args.maximumCycleTimeWarning,
+ e.second.maximumCycleTimeWarn);
armarx::core::time::fromIce(args.maximumCycleTimeError, e.second.maximumCycleTimeErr);
e.second.tags = args.tags;
@@ -280,8 +288,8 @@ namespace armarx
void
RobotHealth::heartbeat(const std::string& identifier,
- const core::time::dto::DateTime& referenceTime,
- const Ice::Current& current)
+ const core::time::dto::DateTime& referenceTime,
+ const Ice::Current& current)
{
ARMARX_TRACE;
ARMARX_VERBOSE << "Finding update entry";
@@ -290,13 +298,13 @@ namespace armarx
// We hold a reference to 'o' which is an element in a vector.
- // If we don't lock until the end of this scope, the vector size might change and 'o' will be invalidated.
- std::scoped_lock lockU(updateMutex);
+ // If we don't lock until the end of this scope, the vector size might change and 'o' will be invalidated.
+ std::scoped_lock lockU(updateMutex);
auto* const entry = findUpdateEntry(identifier);
if (entry == nullptr)
{
- ARMARX_WARNING << deactivateSpam() << "Attention. Component `" << identifier
+ ARMARX_WARNING << deactivateSpam() << "Attention. Component `" << identifier
<< "` was not signed up for heartbeat. Ignoring heartbeat for now...";
return;
}
@@ -318,7 +326,8 @@ namespace armarx
armarx::core::time::DateTime refTime;
fromIce(referenceTime, refTime);
- entry->history.push_back(UpdateEntry::TimeInfo{.referenceTime = refTime, .arrivalTime = arrivalTimestamp});
+ entry->history.push_back(
+ UpdateEntry::TimeInfo{.referenceTime = refTime, .arrivalTime = arrivalTimestamp});
}
void
@@ -329,7 +338,7 @@ namespace armarx
bool found = false;
{
- for (auto & e : updateEntries)
+ for (auto& e : updateEntries)
{
if (e.name == identifier)
{
@@ -357,13 +366,13 @@ namespace armarx
void
armarx::RobotHealth::addRequiredTags(const std::string& requesterIdentifier,
- const std::vector<std::string>& tags,
- const Ice::Current& current)
+ const std::vector<std::string>& tags,
+ const Ice::Current& current)
{
std::scoped_lock lock(updateMutex);
// add newly requested tags
- for(const auto& tag : tags)
+ for (const auto& tag : tags)
{
tagsPerRequester[requesterIdentifier].insert(tag);
}
@@ -376,42 +385,47 @@ namespace armarx
{
// obtain a list of all tags that are relevant / requested at the moment
std::set<std::string> allRequestedTags;
- for(const auto& tgs: simox::alg::get_values(tagsPerRequester))
+ for (const auto& tgs : simox::alg::get_values(tagsPerRequester))
{
- for(const auto& tag : tgs)
+ for (const auto& tag : tgs)
{
allRequestedTags.insert(tag);
}
- }
+ }
return allRequestedTags;
}
// checks whether any element in 'search' is in 'values'
- bool containsAnyOf(const std::set<std::string>& values, const std::vector<std::string>& searchKeys)
+ bool
+ containsAnyOf(const std::set<std::string>& values, const std::vector<std::string>& searchKeys)
{
- return std::any_of(searchKeys.begin(), searchKeys.end(), [&values](const auto& key){
- return values.count(key) > 0;
- });
+ return std::any_of(searchKeys.begin(),
+ searchKeys.end(),
+ [&values](const auto& key) { return values.count(key) > 0; });
}
- void armarx::RobotHealth::updateRequiredElements()
+ void
+ armarx::RobotHealth::updateRequiredElements()
{
ARMARX_IMPORTANT << "updateRequiredElements";
const std::set<std::string> allRequestedTags = requestedTags();
- for (auto &e : updateEntries) {
+ for (auto& e : updateEntries)
+ {
ARMARX_INFO << e.name;
e.required = false; // if required, will be set in the following
std::unique_lock lock(e.mutex);
- if (containsAnyOf(allRequestedTags, e.tags)) {
+ if (containsAnyOf(allRequestedTags, e.tags))
+ {
ARMARX_INFO << e.name << " is required.";
e.required = true;
- if (not e.enabled) {
- ARMARX_WARNING << "You are requiring the disabled component "
- << e.name << ". Enabling it...";
+ if (not e.enabled)
+ {
+ ARMARX_WARNING << "You are requiring the disabled component " << e.name
+ << ". Enabling it...";
e.enabled = true;
}
}
@@ -420,13 +434,13 @@ namespace armarx
void
armarx::RobotHealth::removeRequiredTags(const std::string& requesterIdentifier,
- const std::vector<std::string>& tags,
- const Ice::Current& current)
+ const std::vector<std::string>& tags,
+ const Ice::Current& current)
{
std::scoped_lock lock(updateMutex);
// update the required tags list / map
- for(const auto& tag : tags)
+ for (const auto& tag : tags)
{
tagsPerRequester[requesterIdentifier].erase(tag);
}
@@ -439,7 +453,7 @@ namespace armarx
{
std::scoped_lock lock(updateMutex);
- // treat the special provider (see onInitComponent()) appropriately
+ // treat the special provider (see onInitComponent()) appropriately
ARMARX_CHECK(tagsPerRequester.count(PROPERTY_REQUESTER_ID) > 0);
const auto propertyProviderTags = tagsPerRequester.at(PROPERTY_REQUESTER_ID);
@@ -483,19 +497,25 @@ namespace armarx
}
}
- const Duration timeSinceLastUpdateArrival = e.history.empty() ? Duration() : Clock::Now() - e.history.back().arrivalTime;
- const Duration timeSinceLastUpdateReference = e.history.empty() ? Duration() : Clock::Now() - e.history.back().referenceTime;
+ const Duration timeSinceLastUpdateArrival =
+ e.history.empty() ? Duration() : Clock::Now() - e.history.back().arrivalTime;
+ const Duration timeSinceLastUpdateReference =
+ e.history.empty() ? Duration() : Clock::Now() - e.history.back().referenceTime;
- const DateTime lastReferenceTime = e.history.empty() ? armarx::core::time::DateTime::Invalid() : e.history.back().referenceTime;
+ const DateTime lastReferenceTime = e.history.empty()
+ ? armarx::core::time::DateTime::Invalid()
+ : e.history.back().referenceTime;
- const Duration timeSyncDelayAndIce = e.history.empty() ? armarx::core::time::Duration() : e.history.back().arrivalTime - e.history.back().referenceTime;
+ const Duration timeSyncDelayAndIce =
+ e.history.empty() ? armarx::core::time::Duration()
+ : e.history.back().arrivalTime - e.history.back().referenceTime;
healthEntry.identifier = e.name;
healthEntry.state = e.state;
healthEntry.enabled = e.enabled;
healthEntry.required = e.required;
toIce(healthEntry.minDelta, minDelta);
- toIce(healthEntry.maxDelta,maxDelta);
+ toIce(healthEntry.maxDelta, maxDelta);
toIce(healthEntry.lastReferenceTimestamp, lastReferenceTime);
toIce(healthEntry.timeSinceLastArrival, timeSinceLastUpdateArrival);
toIce(healthEntry.timeSyncDelayAndIce, timeSyncDelayAndIce);
@@ -516,7 +536,7 @@ namespace armarx
std::string
RobotHealth::getTopicName(const Ice::Current& current)
{
- return p.robotHealthTopicName;
+ return p.robotHealthTopicName;
}
void
@@ -530,9 +550,12 @@ namespace armarx
auto pre = entry.history[entry.history.size() - 2];
const Duration delta = later.arrivalTime - pre.arrivalTime;
- const Duration timeSinceLastArrival = Clock::Now() - entry.history.back().arrivalTime;
- const Duration timeToLastReference = Clock::Now() - entry.history.back().referenceTime;
- const Duration timeSyncDelay = entry.history.back().arrivalTime - entry.history.back().referenceTime;
+ const Duration timeSinceLastArrival =
+ Clock::Now() - entry.history.back().arrivalTime;
+ const Duration timeToLastReference =
+ Clock::Now() - entry.history.back().referenceTime;
+ const Duration timeSyncDelay =
+ entry.history.back().arrivalTime - entry.history.back().referenceTime;
setDebugObserverDatafield("RobotHealth_" + entry.name + "_lastDelta",
delta.toMilliSecondsDouble());
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index 1e45f31116382a9761b879eaffb80375541fdaf7..0b749662f2b8dcefc67a16766337d6b11ebb6986 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -73,14 +73,14 @@ namespace armarx
// RobotHealthInterface interface
void signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current) override;
void unregister(const std::string& identifier, const Ice::Current&) override;
-
+
void addRequiredTags(const std::string& requesterIdentifier,
- const std::vector<std::string>& tags,
- const Ice::Current& current) override;
+ const std::vector<std::string>& tags,
+ const Ice::Current& current) override;
void removeRequiredTags(const std::string& requesterIdentifier,
- const std::vector<std::string>& tags,
- const Ice::Current& current) override;
+ const std::vector<std::string>& tags,
+ const Ice::Current& current) override;
void resetRequiredTags(const Ice::Current& current) override;
@@ -147,7 +147,7 @@ namespace armarx
armarx::core::time::DateTime referenceTime;
//< Timestamp on this PC, set by this component
- armarx::core::time::DateTime arrivalTime;
+ armarx::core::time::DateTime arrivalTime;
};
std::deque<TimeInfo> history;
@@ -163,12 +163,12 @@ namespace armarx
void reportDebugObserver();
-
+
void updateRequiredElements();
std::set<std::string> requestedTags() const;
// Mutex to restrict access to all interface / public methods. This ensures that all requests are
- // handled sequentially.
+ // handled sequentially.
mutable std::mutex updateMutex;
std::deque<UpdateEntry> updateEntries;
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
index 34894b6ea9c449d13f47af00d2c012a47f764f1d..d222f6c1e759ad59bde6bd81e80b009ba298de44 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
@@ -95,8 +95,10 @@ namespace armarx
{
auto args = RobotHealthHeartbeatArgs();
args.identifier = getName();
- armarx::core::time::toIce(args.maximumCycleTimeError, armarx::core::time::Duration::MilliSeconds(1000));
- armarx::core::time::toIce(args.maximumCycleTimeWarning, armarx::core::time::Duration::MilliSeconds(500));
+ armarx::core::time::toIce(args.maximumCycleTimeError,
+ armarx::core::time::Duration::MilliSeconds(1000));
+ armarx::core::time::toIce(args.maximumCycleTimeWarning,
+ armarx::core::time::Duration::MilliSeconds(500));
robotHealthTopicPrx->signUp(args);
ARMARX_INFO << "starting rinning task";
diff --git a/source/RobotAPI/components/RobotHealth/test/RobotHealthTest.cpp b/source/RobotAPI/components/RobotHealth/test/RobotHealthTest.cpp
index 283f425e15f99972a8125d3d03cb61590672bd66..7b154c2a9eb600f4442437225b4833fb1bd2414a 100644
--- a/source/RobotAPI/components/RobotHealth/test/RobotHealthTest.cpp
+++ b/source/RobotAPI/components/RobotHealth/test/RobotHealthTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/RobotHealth/RobotHealth.h>
#include <iostream>
+#include <RobotAPI/components/RobotHealth/RobotHealth.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::RobotHealth instance;
diff --git a/source/RobotAPI/components/RobotHealthDummy/test/RobotHealthDummyTest.cpp b/source/RobotAPI/components/RobotHealthDummy/test/RobotHealthDummyTest.cpp
index d56cb42e56099694523bd2daa968a720cc4b9627..89eec0a0ffffca850e4647516ae594f1b79d3743 100644
--- a/source/RobotAPI/components/RobotHealthDummy/test/RobotHealthDummyTest.cpp
+++ b/source/RobotAPI/components/RobotHealthDummy/test/RobotHealthDummyTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/RobotHealthDummy/RobotHealthDummy.h>
#include <iostream>
+#include <RobotAPI/components/RobotHealthDummy/RobotHealthDummy.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::RobotHealthDummy instance;
diff --git a/source/RobotAPI/components/RobotNameService/test/RobotNameServiceTest.cpp b/source/RobotAPI/components/RobotNameService/test/RobotNameServiceTest.cpp
index 6e3e6a973a04d105c2cf67478bab189ba7fbc2f4..6e3e4af634092c4fed53e14c5feefd26e7b1e4a1 100644
--- a/source/RobotAPI/components/RobotNameService/test/RobotNameServiceTest.cpp
+++ b/source/RobotAPI/components/RobotNameService/test/RobotNameServiceTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/RobotNameService/RobotNameService.h>
#include <iostream>
+#include <RobotAPI/components/RobotNameService/RobotNameService.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::RobotNameService instance;
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index d646b7ae3a9d5a6224b25470d10bba6ecff0fadc..3f2be256c9488b0bf51d5068bd8bf17cae5bc195 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -26,10 +26,10 @@
#include <Ice/ObjectAdapter.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
@@ -37,15 +37,15 @@
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
+
#include <RobotAPI/components/units/RobotUnit/util/NonRtTiming.h>
+#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
using namespace Eigen;
using namespace Ice;
using namespace VirtualRobot;
-
namespace armarx
{
RobotStateComponent::~RobotStateComponent()
@@ -58,35 +58,47 @@ namespace armarx
}
}
catch (...)
- {}
+ {
+ }
}
-
RobotStatePropertyDefinitions::RobotStatePropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("RobotNodeSetName", "Set of nodes that is controlled by the KinematicUnit");
- defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
- defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
- defineOptionalProperty<std::string>("RobotStateReportingTopic", "RobotStateUpdates", "Name of the topic on which updates of the robot state are reported.");
- defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history").setMin(0);
+ defineRequiredProperty<std::string>("RobotNodeSetName",
+ "Set of nodes that is controlled by the KinematicUnit");
+ defineRequiredProperty<std::string>(
+ "RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
+ defineRequiredProperty<std::string>(
+ "AgentName", "Name of the agent for which the sensor values are provided");
+ defineOptionalProperty<std::string>(
+ "RobotStateReportingTopic",
+ "RobotStateUpdates",
+ "Name of the topic on which updates of the robot state are reported.");
+ defineOptionalProperty<int>(
+ "HistoryLength", 10000, "Number of entries in the robot state history")
+ .setMin(0);
defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
- defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
- defineOptionalProperty<std::string>("GlobalRobotPoseLocalizationTopicName", "GlobalRobotPoseLocalization", "Topic where the global robot pose can be reported.");
+ defineOptionalProperty<std::string>(
+ "TopicPrefix", "", "Prefix for the sensor value topic name.");
+ defineOptionalProperty<std::string>("GlobalRobotPoseLocalizationTopicName",
+ "GlobalRobotPoseLocalization",
+ "Topic where the global robot pose can be reported.");
}
-
- std::string RobotStateComponent::getDefaultName() const
+ std::string
+ RobotStateComponent::getDefaultName() const
{
return "RobotStateComponent";
}
-
- void RobotStateComponent::onInitComponent()
+ void
+ RobotStateComponent::onInitComponent()
{
robotStateTopicName = getProperty<std::string>("RobotStateReportingTopic").getValue();
offeringTopic(getProperty<std::string>("RobotStateReportingTopic"));
- const std::size_t historyLength = static_cast<std::size_t>(getProperty<int>("HistoryLength").getValue());
+ const std::size_t historyLength =
+ static_cast<std::size_t>(getProperty<int>("HistoryLength").getValue());
jointHistory.clear();
jointHistoryLength = historyLength;
@@ -101,7 +113,8 @@ namespace armarx
throw UserException("Could not find robot file " + robotFile);
}
- this->_synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+ this->_synchronized =
+ VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
_synchronized->setName(getProperty<std::string>("AgentName").getValue());
robotModelScaling = getProperty<float>("RobotModelScaling").getValue();
@@ -109,12 +122,14 @@ namespace armarx
if (robotModelScaling != 1.0f)
{
ARMARX_INFO << "Scaling robot model with scale factor " << robotModelScaling;
- _synchronized = _synchronized->clone(_synchronized->getName(), _synchronized->getCollisionChecker(), robotModelScaling);
+ _synchronized = _synchronized->clone(
+ _synchronized->getName(), _synchronized->getCollisionChecker(), robotModelScaling);
}
if (this->_synchronized)
{
- ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: " << this->_synchronized->getName();
+ ARMARX_VERBOSE << "Loaded robot from file " << robotFile
+ << ". Robot name: " << this->_synchronized->getName();
this->_synchronized->setPropagatingJointValuesEnabled(false);
}
else
@@ -130,7 +145,7 @@ namespace armarx
throw UserException("RobotNodeSet not defined");
}
- VirtualRobot::RobotNodeSetPtr rns = this->_synchronized->getRobotNodeSet(robotNodeSetName);
+ VirtualRobot::RobotNodeSetPtr rns = this->_synchronized->getRobotNodeSet(robotNodeSetName);
if (!rns)
{
@@ -139,7 +154,8 @@ namespace armarx
std::vector<RobotNodePtr> nodes = rns->getAllRobotNodes();
- ARMARX_VERBOSE << "Using RobotNodeSet " << robotNodeSetName << " with " << nodes.size() << " robot nodes.";
+ ARMARX_VERBOSE << "Using RobotNodeSet " << robotNodeSetName << " with " << nodes.size()
+ << " robot nodes.";
for (const RobotNodePtr& node : nodes)
{
ARMARX_VERBOSE << "Node: " << node->getName();
@@ -147,7 +163,8 @@ namespace armarx
const auto topicPrefix = getProperty<std::string>("TopicPrefix").getValue();
usingTopic(topicPrefix + robotNodeSetName + "State");
- usingTopic(topicPrefix + getProperty<std::string>("GlobalRobotPoseLocalizationTopicName").getValue());
+ usingTopic(topicPrefix +
+ getProperty<std::string>("GlobalRobotPoseLocalizationTopicName").getValue());
try
{
@@ -155,23 +172,27 @@ namespace armarx
}
catch (const std::exception& e)
{
- ARMARX_WARNING << "Failed to read robot info from file: " << robotFile << ".\nReason: " << e.what();
+ ARMARX_WARNING << "Failed to read robot info from file: " << robotFile
+ << ".\nReason: " << e.what();
}
catch (...)
{
- ARMARX_WARNING << "Failed to read robot info from file: " << robotFile << " for unknown reason.";
+ ARMARX_WARNING << "Failed to read robot info from file: " << robotFile
+ << " for unknown reason.";
}
usingTopic("SimulatorResetEvent");
}
- void RobotStateComponent::readRobotInfo(const std::string& robotFile)
+ void
+ RobotStateComponent::readRobotInfo(const std::string& robotFile)
{
RapidXmlReaderPtr reader = RapidXmlReader::FromFile(robotFile);
RapidXmlReaderNode robotNode = reader->getRoot("Robot");
robotInfo = readRobotInfo(robotNode.first_node("RobotInfo"));
}
- RobotInfoNodePtr RobotStateComponent::readRobotInfo(const RapidXmlReaderNode& infoNode)
+ RobotInfoNodePtr
+ RobotStateComponent::readRobotInfo(const RapidXmlReaderNode& infoNode)
{
std::string name = infoNode.name();
std::string profile = infoNode.attribute_value_or_default("profile", "");
@@ -185,15 +206,21 @@ namespace armarx
return new RobotInfoNode(name, profile, value, children);
}
-
- void RobotStateComponent::onConnectComponent()
- {
- robotStateListenerPrx = getTopic<RobotStateListenerInterfacePrx>(getProperty<std::string>("RobotStateReportingTopic"));
- _sharedRobotServant = new SharedRobotServant(this->_synchronized, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), robotStateListenerPrx);
+ void
+ RobotStateComponent::onConnectComponent()
+ {
+ robotStateListenerPrx = getTopic<RobotStateListenerInterfacePrx>(
+ getProperty<std::string>("RobotStateReportingTopic"));
+ _sharedRobotServant =
+ new SharedRobotServant(this->_synchronized,
+ RobotStateComponentInterfacePrx::uncheckedCast(getProxy()),
+ robotStateListenerPrx);
_sharedRobotServant->ref();
- _sharedRobotServant->setRobotStateComponent(RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
- this->_synchronizedPrx = SharedRobotInterfacePrx::uncheckedCast(getObjectAdapter()->addWithUUID(_sharedRobotServant));
+ _sharedRobotServant->setRobotStateComponent(
+ RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
+ this->_synchronizedPrx = SharedRobotInterfacePrx::uncheckedCast(
+ getObjectAdapter()->addWithUUID(_sharedRobotServant));
ARMARX_INFO << "Started RobotStateComponent" << flush;
if (robotStateObs)
{
@@ -201,12 +228,13 @@ namespace armarx
}
}
- void RobotStateComponent::onDisconnectComponent()
+ void
+ RobotStateComponent::onDisconnectComponent()
{
}
-
- SharedRobotInterfacePrx RobotStateComponent::getSynchronizedRobot(const Current&) const
+ SharedRobotInterfacePrx
+ RobotStateComponent::getSynchronizedRobot(const Current&) const
{
if (!this->_synchronizedPrx)
{
@@ -215,10 +243,10 @@ namespace armarx
return this->_synchronizedPrx;
}
-
- SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshot(const std::string& deprecated, const Current&)
+ SharedRobotInterfacePrx
+ RobotStateComponent::getRobotSnapshot(const std::string& deprecated, const Current&)
{
- (void) deprecated;
+ (void)deprecated;
if (!_synchronized)
{
@@ -227,15 +255,18 @@ namespace armarx
auto clone = this->_synchronized->clone(_synchronized->getName());
- SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), nullptr);
- p->setTimestamp(IceUtil::Time::microSecondsDouble(_sharedRobotServant->getTimestamp()->timestamp));
+ SharedRobotServantPtr p = new SharedRobotServant(
+ clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), nullptr);
+ p->setTimestamp(
+ IceUtil::Time::microSecondsDouble(_sharedRobotServant->getTimestamp()->timestamp));
auto result = getObjectAdapter()->addWithUUID(p);
// virtal robot clone is buggy -> set global pose here
p->setGlobalPose(new Pose(_synchronized->getGlobalPose()));
return SharedRobotInterfacePrx::uncheckedCast(result);
}
- SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshotAtTimestamp(double _time, const Current&)
+ SharedRobotInterfacePrx
+ RobotStateComponent::getRobotSnapshotAtTimestamp(double _time, const Current&)
{
const IceUtil::Time time = IceUtil::Time::microSecondsDouble(_time);
@@ -248,12 +279,14 @@ namespace armarx
auto config = interpolate(time);
if (!config)
{
- ARMARX_WARNING << "Could not find or interpolate a robot state for time " << time.toDateTime();
+ ARMARX_WARNING << "Could not find or interpolate a robot state for time "
+ << time.toDateTime();
return nullptr;
}
clone->setJointValues(config->jointMap);
- SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), nullptr);
+ SharedRobotServantPtr p = new SharedRobotServant(
+ clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), nullptr);
p->setTimestamp(time);
auto result = getObjectAdapter()->addWithUUID(p);
// Virtal robot clone is buggy -> set global pose here.
@@ -262,39 +295,43 @@ namespace armarx
return SharedRobotInterfacePrx::uncheckedCast(result);
}
-
- NameValueMap RobotStateComponent::getJointConfigAtTimestamp(double timestamp, const Current&) const
+ NameValueMap
+ RobotStateComponent::getJointConfigAtTimestamp(double timestamp, const Current&) const
{
auto jointAngles = interpolateJoints(IceUtil::Time::microSecondsDouble(timestamp));
return jointAngles ? jointAngles->value : NameValueMap();
}
-
- RobotStateConfig RobotStateComponent::getRobotStateAtTimestamp(double timestamp, const Current&) const
+ RobotStateConfig
+ RobotStateComponent::getRobotStateAtTimestamp(double timestamp, const Current&) const
{
auto config = interpolate(IceUtil::Time::microSecondsDouble(timestamp));
return config ? *config : RobotStateConfig();
}
-
- std::string RobotStateComponent::getRobotFilename(const Ice::Current&) const
+ std::string
+ RobotStateComponent::getRobotFilename(const Ice::Current&) const
{
return relativeRobotFile;
}
- float RobotStateComponent::getScaling(const Ice::Current&) const
+ float
+ RobotStateComponent::getScaling(const Ice::Current&) const
{
return robotModelScaling;
}
- RobotInfoNodePtr RobotStateComponent::getRobotInfo(const Ice::Current&) const
+ RobotInfoNodePtr
+ RobotStateComponent::getRobotInfo(const Ice::Current&) const
{
return robotInfo;
}
-
- void RobotStateComponent::reportJointAngles(
- const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Current&)
+ void
+ RobotStateComponent::reportJointAngles(const NameValueMap& jointAngles,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Current&)
{
if (timestamp <= 0)
{
@@ -338,19 +375,19 @@ namespace armarx
robotStateListenerPrx->reportJointValues(jointAngles, timestamp, aValueChanged);
}
-
void
- RobotStateComponent::reportGlobalRobotPose(
- const TransformStamped& globalRobotPose,
- const Ice::Current&)
+ RobotStateComponent::reportGlobalRobotPose(const TransformStamped& globalRobotPose,
+ const Ice::Current&)
{
if (_synchronized)
{
std::string localRobotName = _synchronized->getName();
- ARMARX_DEBUG << "Comparing " << localRobotName << " and " << globalRobotPose.header.agent << ".";
+ ARMARX_DEBUG << "Comparing " << localRobotName << " and "
+ << globalRobotPose.header.agent << ".";
if (localRobotName == globalRobotPose.header.agent)
{
- const IceUtil::Time time = IceUtil::Time::microSeconds(globalRobotPose.header.timestampInMicroSeconds);
+ const IceUtil::Time time =
+ IceUtil::Time::microSeconds(globalRobotPose.header.timestampInMicroSeconds);
insertPose(time, globalRobotPose.transform);
_synchronized->setGlobalPose(globalRobotPose.transform);
@@ -368,8 +405,8 @@ namespace armarx
}
}
-
- std::vector<std::string> RobotStateComponent::getArmarXPackages(const Current&) const
+ std::vector<std::string>
+ RobotStateComponent::getArmarXPackages(const Current&) const
{
std::vector<std::string> result;
auto packages = armarx::Application::GetProjectDependencies();
@@ -388,48 +425,82 @@ namespace armarx
return result;
}
- void RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Current&)
+ void
+ RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Current&)
{
// Unused.
- (void) jointModes, (void) timestamp, (void) aValueChanged;
+ (void)jointModes, (void)timestamp, (void)aValueChanged;
}
- void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Current&)
+
+ void
+ RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Current&)
{
// Unused.
- (void) aValueChanged;
+ (void)aValueChanged;
if (robotStateObs)
{
robotStateObs->updateNodeVelocities(jointVelocities, timestamp);
}
}
- void RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Current&)
+
+ void
+ RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Current&)
{
// Unused.
- (void) jointTorques, (void) timestamp, (void) aValueChanged;
+ (void)jointTorques, (void)timestamp, (void)aValueChanged;
}
- void RobotStateComponent::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Current&)
+ void
+ RobotStateComponent::reportJointAccelerations(const NameValueMap& jointAccelerations,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Current&)
{
// Unused.
- (void) jointAccelerations, (void) timestamp, (void) aValueChanged;
+ (void)jointAccelerations, (void)timestamp, (void)aValueChanged;
}
- void RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Current&)
+
+ void
+ RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Current&)
{
// Unused.
- (void) jointCurrents, (void) timestamp, (void) aValueChanged;
+ (void)jointCurrents, (void)timestamp, (void)aValueChanged;
}
- void RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Current&)
+
+ void
+ RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Current&)
{
// Unused.
- (void) jointMotorTemperatures, (void) timestamp, (void) aValueChanged;
+ (void)jointMotorTemperatures, (void)timestamp, (void)aValueChanged;
}
- void RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Current&)
+
+ void
+ RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Current&)
{
// Unused.
- (void) jointStatuses, (void) timestamp, (void) aValueChanged;
+ (void)jointStatuses, (void)timestamp, (void)aValueChanged;
}
- void RobotStateComponent::simulatorWasReset(const Current&)
+ void
+ RobotStateComponent::simulatorWasReset(const Current&)
{
{
std::lock_guard lock(poseHistoryMutex);
@@ -441,31 +512,33 @@ namespace armarx
}
}
- PropertyDefinitionsPtr RobotStateComponent::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ RobotStateComponent::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new RobotStatePropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(new RobotStatePropertyDefinitions(getConfigIdentifier()));
}
- void RobotStateComponent::setRobotStateObserver(RobotStateObserverPtr observer)
+ void
+ RobotStateComponent::setRobotStateObserver(RobotStateObserverPtr observer)
{
robotStateObs = observer;
}
- std::string RobotStateComponent::getRobotName(const Current&) const
+ std::string
+ RobotStateComponent::getRobotName(const Current&) const
{
if (_synchronized)
{
- return _synchronized->getName(); // _synchronizedPrx->getName();
+ return _synchronized->getName(); // _synchronizedPrx->getName();
}
else
{
throw NotInitializedException("Robot Ptr is NULL", "getName");
}
-
}
- std::string RobotStateComponent::getRobotNodeSetName(const Current&) const
+ std::string
+ RobotStateComponent::getRobotNodeSetName(const Current&) const
{
if (robotNodeSetName.empty())
{
@@ -474,13 +547,14 @@ namespace armarx
return robotNodeSetName;
}
- std::string RobotStateComponent::getRobotStateTopicName(const Current&) const
+ std::string
+ RobotStateComponent::getRobotStateTopicName(const Current&) const
{
return robotStateTopicName;
}
-
- void RobotStateComponent::insertPose(IceUtil::Time timestamp, const Matrix4f& globalPose)
+ void
+ RobotStateComponent::insertPose(IceUtil::Time timestamp, const Matrix4f& globalPose)
{
IceUtil::Time duration;
{
@@ -488,8 +562,8 @@ namespace armarx
std::unique_lock lock(poseHistoryMutex);
duration = IceUtil::Time::now() - start;
- poseHistory.emplace_hint(poseHistory.end(),
- timestamp, new FramedPose(globalPose, GlobalFrame, ""));
+ poseHistory.emplace_hint(
+ poseHistory.end(), timestamp, new FramedPose(globalPose, GlobalFrame, ""));
if (poseHistory.size() > poseHistoryLength)
{
@@ -503,7 +577,8 @@ namespace armarx
}
}
- std::optional<RobotStateConfig> RobotStateComponent::interpolate(IceUtil::Time time) const
+ std::optional<RobotStateConfig>
+ RobotStateComponent::interpolate(IceUtil::Time time) const
{
auto joints = interpolateJoints(time);
auto globalPose = interpolatePose(time);
@@ -524,12 +599,15 @@ namespace armarx
}
}
- auto RobotStateComponent::interpolateJoints(IceUtil::Time time) const -> std::optional<Timestamped<NameValueMap>>
+ auto
+ RobotStateComponent::interpolateJoints(IceUtil::Time time) const
+ -> std::optional<Timestamped<NameValueMap>>
{
std::shared_lock lock(jointHistoryMutex);
if (jointHistory.empty())
{
- ARMARX_WARNING << deactivateSpam(1) << "Joint history of robot state component is empty!";
+ ARMARX_WARNING << deactivateSpam(1)
+ << "Joint history of robot state component is empty!";
return std::nullopt;
}
@@ -540,8 +618,10 @@ namespace armarx
const IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
if (time > newestTimeInHistory + maxOffset)
{
- ARMARX_WARNING << deactivateSpam(5) << "Requested joint timestamp is substantially newer (>"
- << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
+ ARMARX_WARNING << deactivateSpam(5)
+ << "Requested joint timestamp is substantially newer (>"
+ << maxOffset.toSecondsDouble()
+ << " sec) than newest available timestamp!"
<< "\n- requested timestamp: \t" << time.toDateTime()
<< "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime();
return std::nullopt;
@@ -552,10 +632,12 @@ namespace armarx
<< "Requested joint timestamp is newer than newest available timestamp!"
<< "\n- requested timestamp: \t" << time.toDateTime()
<< "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime()
- << "\n- difference: \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
+ << "\n- difference: \t"
+ << (time - newestTimeInHistory).toMicroSeconds() << " us";
}
- return Timestamped<NameValueMap> {jointHistory.rbegin()->first, jointHistory.rbegin()->second};
+ return Timestamped<NameValueMap>{jointHistory.rbegin()->first,
+ jointHistory.rbegin()->second};
}
// Get the oldest entry whose time >= time.
@@ -563,8 +645,9 @@ namespace armarx
if (nextIt == jointHistory.end())
{
ARMARX_WARNING << deactivateSpam(1)
- << "Could not find or interpolate a robot joint angles for time " << time.toDateTime()
- << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
+ << "Could not find or interpolate a robot joint angles for time "
+ << time.toDateTime() << "\n- oldest available value: "
+ << jointHistory.begin()->first.toDateTime()
<< "\n- newest available value: " << newestTimeInHistory.toDateTime();
return std::nullopt;
}
@@ -572,7 +655,7 @@ namespace armarx
if (nextIt == jointHistory.begin())
{
// Next was oldest entry.
- return Timestamped<NameValueMap> {nextIt->first, nextIt->second};
+ return Timestamped<NameValueMap>{nextIt->first, nextIt->second};
}
const NameValueMap& next = nextIt->second;
@@ -584,8 +667,8 @@ namespace armarx
IceUtil::Time prevTime = prevIt->first;
IceUtil::Time nextTime = nextIt->first;
- float t = static_cast<float>((time - prevTime).toSecondsDouble()
- / (nextTime - prevTime).toSecondsDouble());
+ float t = static_cast<float>((time - prevTime).toSecondsDouble() /
+ (nextTime - prevTime).toSecondsDouble());
NameValueMap jointAngles;
auto prevJointIt = prevIt->second.begin();
@@ -597,10 +680,12 @@ namespace armarx
jointAngles.emplace(name, value);
}
- return Timestamped<NameValueMap> {time, std::move(jointAngles)};
+ return Timestamped<NameValueMap>{time, std::move(jointAngles)};
}
- auto RobotStateComponent::interpolatePose(IceUtil::Time time) const -> std::optional<Timestamped<FramedPosePtr>>
+ auto
+ RobotStateComponent::interpolatePose(IceUtil::Time time) const
+ -> std::optional<Timestamped<FramedPosePtr>>
{
std::shared_lock lock(poseHistoryMutex);
@@ -611,8 +696,9 @@ namespace armarx
ReadLockPtr readLock = _synchronized->getReadLock();
- FramedPosePtr pose = new FramedPose(_synchronized->getGlobalPose(), armarx::GlobalFrame, "");
- return Timestamped<FramedPosePtr> {IceUtil::Time::seconds(0), pose};
+ FramedPosePtr pose =
+ new FramedPose(_synchronized->getGlobalPose(), armarx::GlobalFrame, "");
+ return Timestamped<FramedPosePtr>{IceUtil::Time::seconds(0), pose};
}
@@ -626,17 +712,21 @@ namespace armarx
<< "Requested pose timestamp is newer than newest available timestamp!"
<< "\n- requested timestamp: \t" << time.toDateTime()
<< "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime()
- << "\n- difference: \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
+ << "\n- difference: \t"
+ << (time - newestTimeInHistory).toMicroSeconds() << " us";
}
else
{
- ARMARX_WARNING << deactivateSpam(1) << "Requested pose timestamp is substantially newer (>"
- << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
+ ARMARX_WARNING << deactivateSpam(1)
+ << "Requested pose timestamp is substantially newer (>"
+ << maxOffset.toSecondsDouble()
+ << " sec) than newest available timestamp!"
<< "\n- requested timestamp: \t" << time.toDateTime()
<< "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime();
return std::nullopt;
}
- return Timestamped<FramedPosePtr> {poseHistory.rbegin()->first, poseHistory.rbegin()->second};
+ return Timestamped<FramedPosePtr>{poseHistory.rbegin()->first,
+ poseHistory.rbegin()->second};
}
@@ -644,8 +734,9 @@ namespace armarx
if (nextIt == poseHistory.end())
{
ARMARX_WARNING << deactivateSpam(1)
- << "Could not find or interpolate platform pose for time " << time.toDateTime()
- << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
+ << "Could not find or interpolate platform pose for time "
+ << time.toDateTime() << "\n- oldest available value: "
+ << jointHistory.begin()->first.toDateTime()
<< "\n- newest available value: " << newestTimeInHistory.toDateTime();
return std::nullopt;
}
@@ -654,7 +745,7 @@ namespace armarx
if (nextIt == poseHistory.begin())
{
// Next was oldest entry.
- return Timestamped<FramedPosePtr> {nextIt->first, nextIt->second};
+ return Timestamped<FramedPosePtr>{nextIt->first, nextIt->second};
}
auto prevIt = std::prev(nextIt);
@@ -669,8 +760,8 @@ namespace armarx
IceUtil::Time prevTime = prevIt->first;
IceUtil::Time nextTime = nextIt->first;
- float t = static_cast<float>((time - prevTime).toSecondsDouble()
- / (nextTime - prevTime).toSecondsDouble());
+ float t = static_cast<float>((time - prevTime).toSecondsDouble() /
+ (nextTime - prevTime).toSecondsDouble());
Eigen::Matrix4f globalPoseNext = next->toEigen();
Eigen::Matrix4f globalPosePrev = prev->toEigen();
@@ -678,8 +769,8 @@ namespace armarx
Eigen::Matrix4f globalPose;
- math::Helpers::Position(globalPose) =
- (1 - t) * math::Helpers::Position(globalPosePrev) + t * math::Helpers::Position(globalPoseNext);
+ math::Helpers::Position(globalPose) = (1 - t) * math::Helpers::Position(globalPosePrev) +
+ t * math::Helpers::Position(globalPoseNext);
Eigen::Quaternionf rotNext(math::Helpers::Orientation(globalPoseNext));
Eigen::Quaternionf rotPrev(math::Helpers::Orientation(globalPosePrev));
@@ -687,7 +778,8 @@ namespace armarx
math::Helpers::Orientation(globalPose) = rotNew.toRotationMatrix();
- return Timestamped<FramedPosePtr> {time, new FramedPose(globalPose, armarx::GlobalFrame, "")};
+ return Timestamped<FramedPosePtr>{time,
+ new FramedPose(globalPose, armarx::GlobalFrame, "")};
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index d786dd0d67efc4b5e8fe07e02bef4f4b370588da..446748dff36613366221c6fb59b3c8fc5513bd4a 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -24,7 +24,9 @@
#pragma once
+#include <mutex>
#include <optional>
+#include <shared_mutex>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
@@ -38,24 +40,18 @@
#include "SharedRobotServants.h"
-#include <shared_mutex>
-#include <mutex>
-
-
namespace armarx
{
/**
* \class RobotStatePropertyDefinition
* \brief
*/
- class RobotStatePropertyDefinitions :
- public ComponentPropertyDefinitions
+ class RobotStatePropertyDefinitions : public ComponentPropertyDefinitions
{
public:
RobotStatePropertyDefinitions(std::string prefix);
};
-
/**
* \defgroup Component-RobotStateComponent RobotStateComponent
* \ingroup RobotAPI-Components
@@ -82,7 +78,6 @@ namespace armarx
virtual public RobotStateComponentInterface
{
public:
-
std::string getDefaultName() const override;
@@ -97,11 +92,15 @@ namespace armarx
* \return Clone of the internal synchronized robot fixed to the configuration from the time of calling this function.
*/
// [[deprecated]]
- SharedRobotInterfacePrx getRobotSnapshot(const std::string& deprecated, const Ice::Current&) override;
+ SharedRobotInterfacePrx getRobotSnapshot(const std::string& deprecated,
+ const Ice::Current&) override;
- SharedRobotInterfacePrx getRobotSnapshotAtTimestamp(double time, const Ice::Current& current) override;
- NameValueMap getJointConfigAtTimestamp(double timestamp, const Ice::Current&) const override;
- RobotStateConfig getRobotStateAtTimestamp(double timestamp, const Ice::Current&) const override;
+ SharedRobotInterfacePrx getRobotSnapshotAtTimestamp(double time,
+ const Ice::Current& current) override;
+ NameValueMap getJointConfigAtTimestamp(double timestamp,
+ const Ice::Current&) const override;
+ RobotStateConfig getRobotStateAtTimestamp(double timestamp,
+ const Ice::Current&) const override;
/// \return the robot xml filename as specified in the configuration
std::string getRobotFilename(const Ice::Current&) const override;
@@ -122,14 +121,14 @@ namespace armarx
RobotInfoNodePtr getRobotInfo(const Ice::Current&) const override;
// GlobalRobotPoseLocalizationListener
- void reportGlobalRobotPose(const TransformStamped& globalRobotPose, const Ice::Current& = Ice::Current()) override;
+ void reportGlobalRobotPose(const TransformStamped& globalRobotPose,
+ const Ice::Current& = Ice::Current()) override;
// Own interface.
void setRobotStateObserver(RobotStateObserverPtr observer);
protected:
-
// Component interface.
/**
@@ -152,31 +151,54 @@ namespace armarx
// Inherited from KinematicUnitInterface
/// Does nothing.
- void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportControlModeChanged(const NameControlModeMap& jointModes,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/// Stores the reported joint angles in the joint history and publishes the new joint angles.
- void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointAngles(const NameValueMap& jointAngles,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/// Sends the joint velocities to the robot state observer.
- void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointVelocities(const NameValueMap& jointVelocities,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/// Does nothing.
- void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointTorques(const NameValueMap& jointTorques,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/// Does nothing.
- void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
+ void reportJointAccelerations(const NameValueMap& jointAccelerations,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c) override;
/// Does nothing.
- void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointCurrents(const NameValueMap& jointCurrents,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/// Does nothing.
- void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/// Does nothing.
- void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointStatuses(const NameStatusMap& jointStatuses,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
void simulatorWasReset(const Ice::Current& = Ice::emptyCurrent) override;
- private:
+ private:
void readRobotInfo(const std::string& robotFile);
RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
void insertPose(IceUtil::Time timestamp, const Eigen::Matrix4f& globalPose);
-
template <class ValueT>
struct Timestamped
{
@@ -193,7 +215,6 @@ namespace armarx
private:
-
/// Local robot model.
VirtualRobot::RobotPtr _synchronized;
/// Local shared robot.
@@ -221,7 +242,6 @@ namespace armarx
float robotModelScaling;
RobotInfoNodePtr robotInfo;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
index cfc854b4c19309b39c56591fda88ca4b54300086..7d960606eda7f39ced7bc3c1c8e88dffda811414 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.cpp
@@ -23,18 +23,19 @@
*/
#include "SharedRobotServants.h"
-#include <ArmarXCore/core/logging/Logging.h>
-
#include <Eigen/Geometry>
+#include <Ice/ObjectAdapter.h>
+
#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
-#include <VirtualRobot/RobotConfig.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <Ice/ObjectAdapter.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
using namespace VirtualRobot;
@@ -43,6 +44,7 @@ using namespace Ice;
#undef VERBOSE
+
//#define VERBOSE
@@ -57,32 +59,33 @@ namespace armarx
SharedObjectBase::SharedObjectBase()
{
- this-> _referenceCount = 0;
+ this->_referenceCount = 0;
#ifdef VERBOSE
ARMARX_LOG_S << "construct " << this << flush;
#endif
}
-
- void SharedObjectBase::ref(const Current& current)
+ void
+ SharedObjectBase::ref(const Current& current)
{
std::unique_lock lock(this->_counterMutex);
_referenceCount++;
#ifdef VERBOSE
- ARMARX_LOG_S << "ref: " << _referenceCount << " " << this << flush;
+ ARMARX_LOG_S << "ref: " << _referenceCount << " " << this << flush;
#endif
}
- void SharedObjectBase::unref(const Current& current)
+ void
+ SharedObjectBase::unref(const Current& current)
{
std::unique_lock lock(this->_counterMutex);
#ifdef VERBOSE
- ARMARX_LOG_S << "unref: " << _referenceCount << " " << this << flush;
+ ARMARX_LOG_S << "unref: " << _referenceCount << " " << this << flush;
#endif
- _referenceCount --;
+ _referenceCount--;
if (_referenceCount == 0)
{
@@ -90,13 +93,15 @@ namespace armarx
}
}
- void SharedObjectBase::destroy(const Current& current)
+ void
+ SharedObjectBase::destroy(const Current& current)
{
try
{
current.adapter->remove(current.id);
#ifdef VERBOSE
- ARMARX_ERROR_S << "[SharedObject] destroy " << " " << this << flush;
+ ARMARX_ERROR_S << "[SharedObject] destroy "
+ << " " << this << flush;
#endif
}
catch (const NotRegisteredException& e)
@@ -110,49 +115,52 @@ namespace armarx
// SharedRobotNodeServant
///////////////////////////////
- SharedRobotNodeServant::SharedRobotNodeServant(RobotNodePtr node) :
- _node(node)
+ SharedRobotNodeServant::SharedRobotNodeServant(RobotNodePtr node) : _node(node)
{
#ifdef VERBOSE
- ARMARX_LOG_S << "[SharedRobotNodeServant] construct " << " " << this << flush;
+ ARMARX_LOG_S << "[SharedRobotNodeServant] construct "
+ << " " << this << flush;
#endif
}
-
SharedRobotNodeServant::~SharedRobotNodeServant()
{
#ifdef VERBOSE
- ARMARX_FATAL_S << "[SharedRobotNodeServant] destruct " << " " << this << flush;
+ ARMARX_FATAL_S << "[SharedRobotNodeServant] destruct "
+ << " " << this << flush;
#endif
}
- float SharedRobotNodeServant::getJointValue(const Current& current) const
+ float
+ SharedRobotNodeServant::getJointValue(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
return _node->getJointValue();
}
- std::string SharedRobotNodeServant::getName(const Current& current) const
+ std::string
+ SharedRobotNodeServant::getName(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
return _node->getName();
}
- PoseBasePtr SharedRobotNodeServant::getLocalTransformation(const Current& current) const
+ PoseBasePtr
+ SharedRobotNodeServant::getLocalTransformation(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
return new Pose(_node->getLocalTransformation());
}
- FramedPoseBasePtr SharedRobotNodeServant::getGlobalPose(const Current& current) const
+ FramedPoseBasePtr
+ SharedRobotNodeServant::getGlobalPose(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
- return new FramedPose(_node->getGlobalPose(),
- GlobalFrame,
- "");
+ return new FramedPose(_node->getGlobalPose(), GlobalFrame, "");
}
- FramedPoseBasePtr SharedRobotNodeServant::getPoseInRootFrame(const Current& current) const
+ FramedPoseBasePtr
+ SharedRobotNodeServant::getPoseInRootFrame(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
return new FramedPose(_node->getPoseInRootFrame(),
@@ -160,8 +168,8 @@ namespace armarx
_node->getRobot()->getName());
}
-
- JointType SharedRobotNodeServant::getType(const Current& current) const
+ JointType
+ SharedRobotNodeServant::getType(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
@@ -179,7 +187,8 @@ namespace armarx
}
}
- Vector3BasePtr SharedRobotNodeServant::getJointTranslationDirection(const Current& current) const
+ Vector3BasePtr
+ SharedRobotNodeServant::getJointTranslationDirection(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
RobotNodePrismatic* prismatic = dynamic_cast<RobotNodePrismatic*>(_node.get());
@@ -194,7 +203,8 @@ namespace armarx
}
}
- Vector3BasePtr SharedRobotNodeServant::getJointRotationAxis(const Current& current) const
+ Vector3BasePtr
+ SharedRobotNodeServant::getJointRotationAxis(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
RobotNodeRevolute* revolute = dynamic_cast<RobotNodeRevolute*>(_node.get());
@@ -209,15 +219,18 @@ namespace armarx
}
}
-
- bool SharedRobotNodeServant::hasChild(const std::string& name, bool recursive, const Current& current) const
+ bool
+ SharedRobotNodeServant::hasChild(const std::string& name,
+ bool recursive,
+ const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
//return _node->hasChild(name,recursive);
return false;
}
- std::string SharedRobotNodeServant::getParent(const Current& current) const
+ std::string
+ SharedRobotNodeServant::getParent(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
SceneObjectPtr parent = _node->getParent();
@@ -230,7 +243,8 @@ namespace armarx
return parent->getName();
}
- NameList SharedRobotNodeServant::getChildren(const Current& current) const
+ NameList
+ SharedRobotNodeServant::getChildren(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
std::vector<SceneObjectPtr> children = _node->getChildren();
@@ -242,10 +256,13 @@ namespace armarx
return names;
}
- NameList SharedRobotNodeServant::getAllParents(const std::string& name, const Ice::Current& current) const
+ NameList
+ SharedRobotNodeServant::getAllParents(const std::string& name,
+ const Ice::Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
- std::vector<RobotNodePtr> parents = _node->getAllParents(_node->getRobot()->getRobotNodeSet(name));
+ std::vector<RobotNodePtr> parents =
+ _node->getAllParents(_node->getRobot()->getRobotNodeSet(name));
NameList names;
for (RobotNodePtr const& node : parents)
{
@@ -254,31 +271,36 @@ namespace armarx
return names;
}
- float SharedRobotNodeServant::getJointValueOffest(const Current& current) const
+ float
+ SharedRobotNodeServant::getJointValueOffest(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
return _node->getJointValueOffset();
}
- float SharedRobotNodeServant::getJointLimitHigh(const Current& current) const
+ float
+ SharedRobotNodeServant::getJointLimitHigh(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
return _node->getJointLimitHigh();
}
- float SharedRobotNodeServant::getJointLimitLow(const Current& current) const
+ float
+ SharedRobotNodeServant::getJointLimitLow(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
return _node->getJointLimitLow();
}
- Vector3BasePtr SharedRobotNodeServant::getCoM(const Current& current) const
+ Vector3BasePtr
+ SharedRobotNodeServant::getCoM(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
return new Vector3(_node->getCoMLocal());
}
- FloatSeq SharedRobotNodeServant::getInertia(const Current& current) const
+ FloatSeq
+ SharedRobotNodeServant::getInertia(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
FloatSeq result;
@@ -292,21 +314,23 @@ namespace armarx
return result;
}
- float SharedRobotNodeServant::getMass(const Current& current) const
+ float
+ SharedRobotNodeServant::getMass(const Current& current) const
{
ReadLockPtr lock = _node->getRobot()->getReadLock();
return _node->getMass();
}
-
///////////////////////////////
// SharedRobotServant
///////////////////////////////
- SharedRobotServant::SharedRobotServant(RobotPtr robot, RobotStateComponentInterfacePrx robotStateComponent, RobotStateListenerInterfacePrx robotStateListenerPrx)
- : _robot(robot),
- robotStateComponent(robotStateComponent),
- robotStateListenerPrx(robotStateListenerPrx)
+ SharedRobotServant::SharedRobotServant(RobotPtr robot,
+ RobotStateComponentInterfacePrx robotStateComponent,
+ RobotStateListenerInterfacePrx robotStateListenerPrx) :
+ _robot(robot),
+ robotStateComponent(robotStateComponent),
+ robotStateListenerPrx(robotStateListenerPrx)
{
#ifdef VERBOSE
ARMARX_WARNING_S << "construct " << this << flush;
@@ -326,16 +350,20 @@ namespace armarx
{
value.second->unref();
}
- catch (...) {}
+ catch (...)
+ {
+ }
}
}
- void SharedRobotServant::setRobotStateComponent(RobotStateComponentInterfacePrx robotStateComponent)
+ void
+ SharedRobotServant::setRobotStateComponent(RobotStateComponentInterfacePrx robotStateComponent)
{
this->robotStateComponent = robotStateComponent;
}
- SharedRobotNodeInterfacePrx SharedRobotServant::getRobotNode(const std::string& name, const Current& current)
+ SharedRobotNodeInterfacePrx
+ SharedRobotServant::getRobotNode(const std::string& name, const Current& current)
{
// ARMARX_LOG_S << "Looking for node: " << name << flush;
assert(_robot);
@@ -352,8 +380,8 @@ namespace armarx
throw UserException("RobotNode \"" + name + "\" not defined.");
}
- SharedRobotNodeInterfacePtr servant = new SharedRobotNodeServant(
- _robot->getRobotNode(name));
+ SharedRobotNodeInterfacePtr servant =
+ new SharedRobotNodeServant(_robot->getRobotNode(name));
//servant->ref();
prx = SharedRobotNodeInterfacePrx::uncheckedCast(current.adapter->addWithUUID(servant));
prx->ref();
@@ -364,21 +392,23 @@ namespace armarx
return this->_cachedNodes[name];
}
- SharedRobotNodeInterfacePrx SharedRobotServant::getRootNode(const Current& current)
+ SharedRobotNodeInterfacePrx
+ SharedRobotServant::getRootNode(const Current& current)
{
assert(_robot);
std::unique_lock cloneLock(m);
- std::string name = _robot->getRootNode()/*,current*/->getName();
+ std::string name = _robot->getRootNode() /*,current*/->getName();
return this->getRobotNode(name, current);
}
-
- bool SharedRobotServant::hasRobotNode(const std::string& name, const Current& current)
+ bool
+ SharedRobotServant::hasRobotNode(const std::string& name, const Current& current)
{
return _robot->hasRobotNode(name);
}
- NameList SharedRobotServant::getRobotNodes(const Current& current)
+ NameList
+ SharedRobotServant::getRobotNodes(const Current& current)
{
std::vector<RobotNodePtr> robotNodes = _robot->getRobotNodes();
NameList names;
@@ -389,7 +419,8 @@ namespace armarx
return names;
}
- RobotNodeSetInfoPtr SharedRobotServant::getRobotNodeSet(const std::string& name, const Current& current)
+ RobotNodeSetInfoPtr
+ SharedRobotServant::getRobotNodeSet(const std::string& name, const Current& current)
{
RobotNodeSetPtr robotNodeSet = _robot->getRobotNodeSet(name);
@@ -412,10 +443,10 @@ namespace armarx
info->rootName = robotNodeSet->getKinematicRoot()->getName();
return info;
-
}
- NameList SharedRobotServant::getRobotNodeSets(const Current& current)
+ NameList
+ SharedRobotServant::getRobotNodeSets(const Current& current)
{
std::vector<RobotNodeSetPtr> robotNodeSets = _robot->getRobotNodeSets();
NameList names;
@@ -427,34 +458,40 @@ namespace armarx
return names;
}
- bool SharedRobotServant::hasRobotNodeSet(const std::string& name, const Current& current)
+ bool
+ SharedRobotServant::hasRobotNodeSet(const std::string& name, const Current& current)
{
return _robot->hasRobotNodeSet(name);
}
- std::string SharedRobotServant::getName(const Current& current)
+ std::string
+ SharedRobotServant::getName(const Current& current)
{
//ARMARX_VERBOSE_S << "SharedRobotServant::getname:" << _robot->getName();
return _robot->getName();
}
- std::string SharedRobotServant::getType(const Current& current)
+ std::string
+ SharedRobotServant::getType(const Current& current)
{
return _robot->getType();
}
- void SharedRobotServant::setTimestamp(const IceUtil::Time& updateTime)
+ void
+ SharedRobotServant::setTimestamp(const IceUtil::Time& updateTime)
{
updateTimestamp = updateTime;
}
- PoseBasePtr SharedRobotServant::getGlobalPose(const Current& current)
+ PoseBasePtr
+ SharedRobotServant::getGlobalPose(const Current& current)
{
ReadLockPtr lock = _robot->getReadLock();
return new Pose(_robot->getGlobalPose());
}
- NameValueMap SharedRobotServant::getConfig(const Current& current)
+ NameValueMap
+ SharedRobotServant::getConfig(const Current& current)
{
if (!_robot)
{
@@ -463,50 +500,56 @@ namespace armarx
}
ReadLockPtr lock = _robot->getReadLock();
- std::map < std::string, float > values = _robot->getConfig()->getRobotNodeJointValueMap();
+ std::map<std::string, float> values = _robot->getConfig()->getRobotNodeJointValueMap();
NameValueMap result(values.begin(), values.end());
return result;
}
- NameValueMap SharedRobotServant::getConfigAndPose(PoseBasePtr& globalPose, const Current& current)
+ NameValueMap
+ SharedRobotServant::getConfigAndPose(PoseBasePtr& globalPose, const Current& current)
{
globalPose = getGlobalPose(current);
return getConfig(current);
}
- TimestampBasePtr SharedRobotServant::getTimestamp(const Current&) const
+ TimestampBasePtr
+ SharedRobotServant::getTimestamp(const Current&) const
{
return new TimestampVariant(updateTimestamp);
}
- RobotStateComponentInterfacePrx SharedRobotServant::getRobotStateComponent(const Current&) const
+ RobotStateComponentInterfacePrx
+ SharedRobotServant::getRobotStateComponent(const Current&) const
{
return robotStateComponent;
}
- void SharedRobotServant::setGlobalPose(const armarx::PoseBasePtr& pose, const Current&)
+ void
+ SharedRobotServant::setGlobalPose(const armarx::PoseBasePtr& pose, const Current&)
{
WriteLockPtr lock = _robot->getWriteLock();
Eigen::Matrix4f newPose = PosePtr::dynamicCast(pose)->toEigen();
if (robotStateListenerPrx)
{
Eigen::Matrix4f oldPose = _robot->getGlobalPose();
- robotStateListenerPrx->reportGlobalRobotRootPose(new FramedPose(newPose, GlobalFrame, ""), TimeUtil::GetTime().toMicroSeconds(), !oldPose.isApprox(newPose));
+ robotStateListenerPrx->reportGlobalRobotRootPose(
+ new FramedPose(newPose, GlobalFrame, ""),
+ TimeUtil::GetTime().toMicroSeconds(),
+ !oldPose.isApprox(newPose));
}
_robot->setGlobalPose(newPose, true);
}
- float SharedRobotServant::getScaling(const Current&)
+ float
+ SharedRobotServant::getScaling(const Current&)
{
return _robot->getScaling();
}
- RobotPtr SharedRobotServant::getRobot() const
+ RobotPtr
+ SharedRobotServant::getRobot() const
{
return this->_robot;
}
-}
-
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h
index 5dff6c3891075d67a6a3e0ff9fa1f3a9a5d615f3..5db221ac610f9c33ff263838549c01ce32595a35 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.h
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.h
@@ -23,14 +23,15 @@
*/
#pragma once
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/interface/core/RobotState.h>
+#include <mutex>
#include <VirtualRobot/VirtualRobot.h>
-#include <mutex>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx
{
@@ -41,14 +42,14 @@ namespace armarx
*
*/
- class SharedObjectBase :
- virtual public SharedObjectInterface
+ class SharedObjectBase : virtual public SharedObjectInterface
{
public:
void ref(const Ice::Current& c = Ice::emptyCurrent) override;
void unref(const Ice::Current& c = Ice::emptyCurrent) override;
void destroy(const Ice::Current& c = Ice::emptyCurrent) override;
SharedObjectBase();
+
private:
unsigned int _referenceCount;
std::mutex _counterMutex;
@@ -61,29 +62,36 @@ namespace armarx
* TCPControlUnit and HeadIKUnit classes address this class by the SharedRobotNodeInterface and SharedRobotNodeInterfacePrx generated by
* Ice.
*/
- class SharedRobotNodeServant :
- virtual public SharedRobotNodeInterface,
- public SharedObjectBase
+ class SharedRobotNodeServant : virtual public SharedRobotNodeInterface, public SharedObjectBase
{
public:
- SharedRobotNodeServant(VirtualRobot::RobotNodePtr node /*,const Ice::Current & current = Ice::emptyCurrent*/);
+ SharedRobotNodeServant(
+ VirtualRobot::RobotNodePtr node /*,const Ice::Current & current = Ice::emptyCurrent*/);
~SharedRobotNodeServant() override;
float getJointValue(const Ice::Current& current = Ice::emptyCurrent) const override;
std::string getName(const Ice::Current& current = Ice::emptyCurrent) const override;
- PoseBasePtr getLocalTransformation(const Ice::Current& current = Ice::emptyCurrent) const override;
- FramedPoseBasePtr getGlobalPose(const Ice::Current& current = Ice::emptyCurrent) const override;
- FramedPoseBasePtr getPoseInRootFrame(const Ice::Current& current = Ice::emptyCurrent) const override;
+ PoseBasePtr
+ getLocalTransformation(const Ice::Current& current = Ice::emptyCurrent) const override;
+ FramedPoseBasePtr
+ getGlobalPose(const Ice::Current& current = Ice::emptyCurrent) const override;
+ FramedPoseBasePtr
+ getPoseInRootFrame(const Ice::Current& current = Ice::emptyCurrent) const override;
JointType getType(const Ice::Current& current = Ice::emptyCurrent) const override;
- Vector3BasePtr getJointTranslationDirection(const Ice::Current& current = Ice::emptyCurrent) const override;
- Vector3BasePtr getJointRotationAxis(const Ice::Current& current = Ice::emptyCurrent) const override;
-
- bool hasChild(const std::string& name, bool recursive, const Ice::Current& current = Ice::emptyCurrent) const override;
+ Vector3BasePtr getJointTranslationDirection(
+ const Ice::Current& current = Ice::emptyCurrent) const override;
+ Vector3BasePtr
+ getJointRotationAxis(const Ice::Current& current = Ice::emptyCurrent) const override;
+
+ bool hasChild(const std::string& name,
+ bool recursive,
+ const Ice::Current& current = Ice::emptyCurrent) const override;
std::string getParent(const Ice::Current& current = Ice::emptyCurrent) const override;
NameList getChildren(const Ice::Current& current = Ice::emptyCurrent) const override;
- NameList getAllParents(const std::string& name, const Ice::Current& current = Ice::emptyCurrent) const override;
+ NameList getAllParents(const std::string& name,
+ const Ice::Current& current = Ice::emptyCurrent) const override;
float getJointValueOffest(const Ice::Current& current = Ice::emptyCurrent) const override;
float getJointLimitHigh(const Ice::Current& current = Ice::emptyCurrent) const override;
@@ -94,7 +102,6 @@ namespace armarx
float getMass(const Ice::Current& current = Ice::emptyCurrent) const override;
-
protected:
VirtualRobot::RobotNodePtr _node;
};
@@ -107,30 +114,39 @@ namespace armarx
* classes generated by Ice.
*/
- class SharedRobotServant :
- public virtual SharedRobotInterface,
- public SharedObjectBase
+ class SharedRobotServant : public virtual SharedRobotInterface, public SharedObjectBase
{
public:
- SharedRobotServant(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStateComponent, RobotStateListenerInterfacePrx robotStateListenerPrx);
+ SharedRobotServant(VirtualRobot::RobotPtr robot,
+ RobotStateComponentInterfacePrx robotStateComponent,
+ RobotStateListenerInterfacePrx robotStateListenerPrx);
~SharedRobotServant() override;
void setRobotStateComponent(RobotStateComponentInterfacePrx robotStateComponent);
- SharedRobotNodeInterfacePrx getRobotNode(const std::string& name, const Ice::Current& current = Ice::emptyCurrent) override;
- SharedRobotNodeInterfacePrx getRootNode(const Ice::Current& current = Ice::emptyCurrent) override;
- bool hasRobotNode(const std::string& name, const Ice::Current& current = Ice::emptyCurrent) override;
+ SharedRobotNodeInterfacePrx
+ getRobotNode(const std::string& name,
+ const Ice::Current& current = Ice::emptyCurrent) override;
+ SharedRobotNodeInterfacePrx
+ getRootNode(const Ice::Current& current = Ice::emptyCurrent) override;
+ bool hasRobotNode(const std::string& name,
+ const Ice::Current& current = Ice::emptyCurrent) override;
NameList getRobotNodes(const Ice::Current& current = Ice::emptyCurrent) override;
- RobotNodeSetInfoPtr getRobotNodeSet(const std::string& name, const Ice::Current& current = Ice::emptyCurrent) override;
+ RobotNodeSetInfoPtr
+ getRobotNodeSet(const std::string& name,
+ const Ice::Current& current = Ice::emptyCurrent) override;
NameList getRobotNodeSets(const Ice::Current& current = Ice::emptyCurrent) override;
- bool hasRobotNodeSet(const std::string& name, const Ice::Current& current = Ice::emptyCurrent) override;
+ bool hasRobotNodeSet(const std::string& name,
+ const Ice::Current& current = Ice::emptyCurrent) override;
std::string getName(const Ice::Current& current = Ice::emptyCurrent) override;
std::string getType(const Ice::Current& current = Ice::emptyCurrent) override;
PoseBasePtr getGlobalPose(const Ice::Current& current = Ice::emptyCurrent) override;
NameValueMap getConfig(const Ice::Current& current = Ice::emptyCurrent) override;
- NameValueMap getConfigAndPose(PoseBasePtr& globalPose, const Ice::Current& current = Ice::emptyCurrent) override;
- void setGlobalPose(const PoseBasePtr& pose, const Ice::Current& current = Ice::emptyCurrent) override;
+ NameValueMap getConfigAndPose(PoseBasePtr& globalPose,
+ const Ice::Current& current = Ice::emptyCurrent) override;
+ void setGlobalPose(const PoseBasePtr& pose,
+ const Ice::Current& current = Ice::emptyCurrent) override;
float getScaling(const Ice::Current& = Ice::emptyCurrent) override;
@@ -139,6 +155,7 @@ namespace armarx
void setTimestamp(const IceUtil::Time& updateTime);
TimestampBasePtr getTimestamp(const Ice::Current& = Ice::emptyCurrent) const override;
RobotStateComponentInterfacePrx getRobotStateComponent(const Ice::Current&) const override;
+
protected:
VirtualRobot::RobotPtr _robot;
std::recursive_mutex m;
@@ -146,9 +163,7 @@ namespace armarx
IceUtil::Time updateTimestamp;
RobotStateComponentInterfacePrx robotStateComponent;
RobotStateListenerInterfacePrx robotStateListenerPrx;
-
};
using SharedRobotServantPtr = IceInternal::Handle<SharedRobotServant>;
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp b/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp
index 28ed7f7c6e06d0502ec040e10cb93c02d5fb52e1..b360e46888bd07e29a08f1235acbe94da0ac2316 100644
--- a/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp
+++ b/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp
@@ -23,20 +23,25 @@
#define BOOST_TEST_MODULE RobotAPI::FramedPose::Test
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
+
+#include <ArmarXCore/core/test/IceTestHelper.h>
#include <ArmarXCore/util/json/JSONObject.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
+
#include <RobotAPI/components/RobotState/RobotStateComponent.h>
-#include <ArmarXCore/core/test/IceTestHelper.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
using namespace armarx;
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
using namespace armarx;
+
class RobotStateComponentTestEnvironment
{
public:
- RobotStateComponentTestEnvironment(const std::string& testName, int registryPort = 11220, bool addObjects = true)
+ RobotStateComponentTestEnvironment(const std::string& testName,
+ int registryPort = 11220,
+ bool addObjects = true)
{
Ice::PropertiesPtr properties = Ice::createProperties();
armarx::Application::LoadDefaultConfig(properties);
@@ -45,7 +50,8 @@ public:
// If you need to set properties (optional):
properties->setProperty("ArmarX.RobotStateComponent.AgentName", "Armar3");
properties->setProperty("ArmarX.RobotStateComponent.RobotNodeSetName", "Robot");
- properties->setProperty("ArmarX.RobotStateComponent.RobotFileName", "RobotAPI/robots/Armar3/ArmarIII.xml");
+ properties->setProperty("ArmarX.RobotStateComponent.RobotFileName",
+ "RobotAPI/robots/Armar3/ArmarIII.xml");
// The IceTestHelper starts all required Ice processes
_iceTestHelper = new IceTestHelper(registryPort, registryPort + 1);
@@ -56,18 +62,24 @@ public:
if (addObjects)
{
// This is how you create components.
- _rsc = _manager->createComponentAndRun<RobotStateComponent, RobotStateComponentInterfacePrx>("ArmarX", "RobotStateComponent");
+ _rsc =
+ _manager
+ ->createComponentAndRun<RobotStateComponent, RobotStateComponentInterfacePrx>(
+ "ArmarX", "RobotStateComponent");
}
}
+
~RobotStateComponentTestEnvironment()
{
_manager->shutdown();
}
+
// In your tests, you can access your component through this proxy
RobotStateComponentInterfacePrx _rsc;
TestArmarXManagerPtr _manager;
IceTestHelperPtr _iceTestHelper;
};
+
using RobotStateComponentTestEnvironmentPtr = std::shared_ptr<RobotStateComponentTestEnvironment>;
BOOST_AUTO_TEST_CASE(RobotStateComponentHistoryTest)
@@ -104,10 +116,12 @@ BOOST_AUTO_TEST_CASE(RobotStateComponentHistoryTest)
config = env._rsc->getJointConfigAtTimestamp(t2);
BOOST_CHECK_EQUAL(config.at("Elbow L"), 1);
- config = env._rsc->getJointConfigAtTimestamp(t2 + 1); // future timestamp -> latest values should be returned
+ config = env._rsc->getJointConfigAtTimestamp(
+ t2 + 1); // future timestamp -> latest values should be returned
BOOST_CHECK_EQUAL(config.at("Elbow L"), 1);
- config = env._rsc->getJointConfigAtTimestamp((t1 + t2) * 0.5); // interpolated values in the middle
+ config =
+ env._rsc->getJointConfigAtTimestamp((t1 + t2) * 0.5); // interpolated values in the middle
BOOST_CHECK_CLOSE(config.at("Elbow L"), 0.5f, 0.01);
config = env._rsc->getJointConfigAtTimestamp(t1 + (t2 - t1) * 0.7); // interpolated values
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
index 1311e68e132cdbafc805351064d643494f48f87d..1609976cd60ed2abe6b53b050ca054c40f871806 100644
--- a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
@@ -26,7 +26,6 @@
#include <ArmarXCore/core/time/Metronome.h>
-
namespace armarx
{
RobotToArVizPropertyDefinitions::RobotToArVizPropertyDefinitions(std::string prefix) :
@@ -34,39 +33,46 @@ namespace armarx
{
}
- armarx::PropertyDefinitionsPtr RobotToArViz::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ RobotToArViz::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs(new RobotToArVizPropertyDefinitions(getConfigIdentifier()));
+ armarx::PropertyDefinitionsPtr defs(
+ new RobotToArVizPropertyDefinitions(getConfigIdentifier()));
// defs->optional(updateFrequency, "updateFrequency", "Target number of updates per second.");
- defs->defineOptionalProperty("updateFrequency", updateFrequencyHz, "Target number of updates per second.");
- defs->optional(gui.useCollisionModel, "UseCollisionModel", "Use the collision model for visualization");
-
- defs->optional(gui.showRobotNodeFrames, "ShowRobotNodeFrames",
+ defs->defineOptionalProperty(
+ "updateFrequency", updateFrequencyHz, "Target number of updates per second.");
+ defs->optional(gui.useCollisionModel,
+ "UseCollisionModel",
+ "Use the collision model for visualization");
+
+ defs->optional(gui.showRobotNodeFrames,
+ "ShowRobotNodeFrames",
"If true, show frames of robot nodes (can be changed in RemoteGui).");
return defs;
}
-
- std::string RobotToArViz::getDefaultName() const
+ std::string
+ RobotToArViz::getDefaultName() const
{
return "RobotToArViz";
}
-
- void RobotToArViz::onInitComponent()
+ void
+ RobotToArViz::onInitComponent()
{
getProperty(updateFrequencyHz, "updateFrequency");
}
-
- void RobotToArViz::onConnectComponent()
+ void
+ RobotToArViz::onConnectComponent()
{
// Load robot.
if (!RobotState::hasRobot(robotName))
{
- this->robot = RobotState::addRobot(robotName, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ this->robot = RobotState::addRobot(robotName,
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
}
// Initialize robot visu element.
@@ -83,26 +89,24 @@ namespace armarx
createRemoteGuiTab();
RemoteGui_startRunningTask();
- task = new SimpleRunningTask<>([this]()
- {
- this->updateRobotRun();
- });
+ task = new SimpleRunningTask<>([this]() { this->updateRobotRun(); });
task->start();
}
-
- void RobotToArViz::onDisconnectComponent()
+ void
+ RobotToArViz::onDisconnectComponent()
{
task->stop();
task = nullptr;
}
-
- void RobotToArViz::onExitComponent()
+ void
+ RobotToArViz::onExitComponent()
{
}
- void RobotToArViz::createRemoteGuiTab()
+ void
+ RobotToArViz::createRemoteGuiTab()
{
using namespace RemoteGui::Client;
@@ -110,7 +114,8 @@ namespace armarx
int row = 0;
{
tab.showRobotNodeFrames.setValue(gui.showRobotNodeFrames);
- root.add(Label("Show Robot Node Frames"), {row, 0}).add(tab.showRobotNodeFrames, {row, 1});
+ root.add(Label("Show Robot Node Frames"), {row, 0})
+ .add(tab.showRobotNodeFrames, {row, 1});
row += 1;
tab.useCollisionModel.setValue(gui.useCollisionModel);
@@ -120,7 +125,8 @@ namespace armarx
RemoteGui_createTab(getName(), root, &tab);
}
- void RobotToArViz::RemoteGui_update()
+ void
+ RobotToArViz::RemoteGui_update()
{
if (tab.showRobotNodeFrames.hasValueChanged())
{
@@ -134,8 +140,8 @@ namespace armarx
}
}
-
- void RobotToArViz::updateRobotRun()
+ void
+ RobotToArViz::updateRobotRun()
{
Metronome metronome(Frequency::Hertz(updateFrequencyHz));
while (task and not task->isStopped())
@@ -146,8 +152,8 @@ namespace armarx
}
}
-
- void RobotToArViz::updateRobotOnce()
+ void
+ RobotToArViz::updateRobotOnce()
{
RobotState::synchronizeLocalClone(robotName);
@@ -160,7 +166,7 @@ namespace armarx
std::vector<viz::Layer> layers;
robotViz.joints(robot->getConfig()->getRobotNodeJointValueMap())
- .pose(robot->getGlobalPose());
+ .pose(robot->getGlobalPose());
if (gui.useCollisionModel)
{
@@ -188,8 +194,8 @@ namespace armarx
arviz.commit(layers);
}
-
- bool RobotToArViz::trySetFilePathFromDataDir(viz::Robot& robotViz, const std::string& absolutePath)
+ bool
+ RobotToArViz::trySetFilePathFromDataDir(viz::Robot& robotViz, const std::string& absolutePath)
{
// Set file location. Try to use ArmarX data directory.
std::filesystem::path filepath = absolutePath;
@@ -217,4 +223,4 @@ namespace armarx
}
return false;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.h b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
index 5eea92090fd3d4826f87b99cd9676c8f6200ba8c..794315aaadfa5ce0e33f9e561a30e279104e3c7e 100644
--- a/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
@@ -24,28 +24,25 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-
#include <ArmarXCore/interface/observers/ObserverInterface.h>
+
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-
namespace armarx
{
/**
* @class RobotToArVizPropertyDefinitions
* @brief Property definitions of `RobotToArViz`.
*/
- class RobotToArVizPropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
+ class RobotToArVizPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
RobotToArVizPropertyDefinitions(std::string prefix);
};
-
-
/**
* @defgroup Component-RobotToArViz RobotToArViz
* @ingroup RobotAPI-Components
@@ -67,13 +64,11 @@ namespace armarx
using RobotState = RobotStateComponentPluginUser;
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
protected:
-
/// @see armarx::ManagedIceObject::onInitComponent()
void onInitComponent() override;
@@ -95,14 +90,13 @@ namespace armarx
private:
-
void updateRobotRun();
void updateRobotOnce();
- static bool trySetFilePathFromDataDir(viz::Robot& robotViz, const std::string& absolutePath);
+ static bool trySetFilePathFromDataDir(viz::Robot& robotViz,
+ const std::string& absolutePath);
private:
-
float updateFrequencyHz = 100;
SimpleRunningTask<>::pointer_type task;
@@ -110,8 +104,7 @@ namespace armarx
VirtualRobot::RobotPtr robot;
- viz::Robot robotViz { "" };
-
+ viz::Robot robotViz{""};
// Remote Gui
@@ -121,6 +114,7 @@ namespace armarx
RemoteGui::Client::CheckBox useCollisionModel;
// Todo: add spin box for scale
};
+
Tab tab;
struct Gui
@@ -128,8 +122,8 @@ namespace armarx
bool showRobotNodeFrames = false;
bool useCollisionModel = false;
};
+
std::mutex guiMutex;
Gui gui;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotToArViz/main.cpp b/source/RobotAPI/components/RobotToArViz/main.cpp
index f0d3c514ddc51065741b36cd42347b8ee3db848e..9b9567bfe64a4b0639e408e69b63143d316fcb59 100644
--- a/source/RobotAPI/components/RobotToArViz/main.cpp
+++ b/source/RobotAPI/components/RobotToArViz/main.cpp
@@ -20,14 +20,14 @@
* GNU General Public License
*/
-#include <RobotAPI/components/RobotToArViz/RobotToArViz.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/components/RobotToArViz/RobotToArViz.h>
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::RobotToArViz > (argc, argv, "RobotToArViz");
+ return armarx::runSimpleComponentApp<armarx::RobotToArViz>(argc, argv, "RobotToArViz");
}
diff --git a/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp b/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp
index 8a7082d680abfa3573e6d5f60417161c36ca7ea8..eb985b211b4ae91b60e78da32ca902b7d51007da 100644
--- a/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp
+++ b/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/RobotToArViz/RobotToArViz.h>
#include <iostream>
+#include <RobotAPI/components/RobotToArViz/RobotToArViz.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::RobotToArViz instance;
diff --git a/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.cpp b/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.cpp
index 48ee7e41be1f4485ca27eb992a8e4046bbd4b375..ac42f3f028685a2117938bd702b9e092666cea39 100644
--- a/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.cpp
+++ b/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.cpp
@@ -21,52 +21,49 @@
*/
#include "StatechartExecutorExample.h"
-#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
-#include <ArmarXGui/libraries/RemoteGui/Storage.h>
#include <ArmarXCore/core/time/CycleUtil.h>
-#include <ArmarXCore/observers/variant/Variant.h>
+#include <ArmarXCore/interface/statechart/RemoteStateOffererIce.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
#include <ArmarXCore/observers/variant/StringValueMap.h>
-
+#include <ArmarXCore/observers/variant/Variant.h>
+#include <ArmarXCore/statechart/State.h>
+#include <ArmarXCore/statechart/StateBase.h>
#include <ArmarXCore/statechart/StateParameter.h>
+#include <ArmarXCore/statechart/StateUtil.h>
+#include <ArmarXCore/statechart/StateUtilFunctions.h>
+#include <ArmarXCore/statechart/Statechart.h>
+#include <ArmarXGui/libraries/RemoteGui/Storage.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <ArmarXCore/interface/statechart/RemoteStateOffererIce.h>
-#include <ArmarXCore/statechart/StateBase.h>
-#include <ArmarXCore/statechart/StateUtilFunctions.h>
-#include <ArmarXCore/statechart/StateUtil.h>
-#include <ArmarXCore/statechart/State.h>
-
-#include <ArmarXCore/statechart/Statechart.h>
-
namespace armarx
{
DEFINEEVENT(EvAbort)
- StatechartExecutorExamplePropertyDefinitions::StatechartExecutorExamplePropertyDefinitions(std::string prefix) :
+ StatechartExecutorExamplePropertyDefinitions::StatechartExecutorExamplePropertyDefinitions(
+ std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
-
}
-
- std::string StatechartExecutorExample::getDefaultName() const
+ std::string
+ StatechartExecutorExample::getDefaultName() const
{
return "StatechartExecutorExample";
}
-
- void StatechartExecutorExample::onInitComponent()
+ void
+ StatechartExecutorExample::onInitComponent()
{
usingProxy("SimpleStatechartExecutor");
usingProxy("RemoteGuiProvider");
}
-
- void StatechartExecutorExample::onConnectComponent()
+ void
+ StatechartExecutorExample::onConnectComponent()
{
getProxy(_statechartExecutor, "SimpleStatechartExecutor");
@@ -75,31 +72,32 @@ namespace armarx
setupRemoteGuiWidget();
_remoteGuiTab = RemoteGui::TabProxy(_remoteGuiPrx, _tabName);
- _remoteGuiTask = new RunningTask<StatechartExecutorExample>(this, &StatechartExecutorExample::runRemoteGui);
+ _remoteGuiTask = new RunningTask<StatechartExecutorExample>(
+ this, &StatechartExecutorExample::runRemoteGui);
_remoteGuiTask->start();
}
-
- void StatechartExecutorExample::onDisconnectComponent()
+ void
+ StatechartExecutorExample::onDisconnectComponent()
{
-
}
-
- void StatechartExecutorExample::onExitComponent()
+ void
+ StatechartExecutorExample::onExitComponent()
{
-
}
-
- armarx::PropertyDefinitionsPtr StatechartExecutorExample::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ StatechartExecutorExample::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new StatechartExecutorExamplePropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new StatechartExecutorExamplePropertyDefinitions(getConfigIdentifier()));
}
using namespace RemoteGui;
- void StatechartExecutorExample::setupRemoteGuiWidget()
+
+ void
+ StatechartExecutorExample::setupRemoteGuiWidget()
{
auto grid = makeSimpleGridLayout("Layout").cols(3);
@@ -134,7 +132,8 @@ namespace armarx
_remoteGuiPrx->createTab(_tabName, vlayout);
}
- void StatechartExecutorExample::runRemoteGui()
+ void
+ StatechartExecutorExample::runRemoteGui()
{
int cycleDurationMs = 20;
CycleUtil c(cycleDurationMs);
@@ -204,27 +203,26 @@ namespace armarx
if (_remoteGuiTab.getButton("Test_args").clicked())
{
-
}
if (_remoteGuiTab.getButton("Test_wrongProxy").clicked())
{
- StringVariantContainerBaseMap output = _statechartExecutor->getSetOutputParameters();
+ StringVariantContainerBaseMap output =
+ _statechartExecutor->getSetOutputParameters();
if (!output.empty())
{
- int outInt = SingleVariantPtr::dynamicCast(output["OutputInt"])->get()->get<int>();
+ int outInt =
+ SingleVariantPtr::dynamicCast(output["OutputInt"])->get()->get<int>();
ARMARX_INFO << VAROUT(outInt);
}
-
}
if (_remoteGuiTab.getButtonClicked("Run"))
{
-
}
_remoteGuiTab.sendUpdates();
c.waitForCycleDuration();
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.h b/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.h
index 6d66459008ccd6561bdee4eac63d20a88dcf2d00..1a48863c3cda224de15be286bc102ffac148850c 100644
--- a/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.h
+++ b/source/RobotAPI/components/StatechartExecutorExample/StatechartExecutorExample.h
@@ -24,13 +24,11 @@
#include <ArmarXCore/core/Component.h>
-
#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
-#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-
#include <ArmarXCore/interface/components/SimpleStatechartExecutorInterface.h>
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
namespace armarx
{
@@ -38,15 +36,12 @@ namespace armarx
* @class StatechartExecutorExamplePropertyDefinitions
* @brief
*/
- class StatechartExecutorExamplePropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
+ class StatechartExecutorExamplePropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
StatechartExecutorExamplePropertyDefinitions(std::string prefix);
};
-
-
/**
* @defgroup Component-StatechartExecutorExample StatechartExecutorExample
* @ingroup ArmarXGui-Components
@@ -58,17 +53,14 @@ namespace armarx
*
* Detailed description of class StatechartExecutorExample.
*/
- class StatechartExecutorExample :
- virtual public armarx::Component
+ class StatechartExecutorExample : virtual public armarx::Component
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
virtual std::string getDefaultName() const override;
protected:
-
/// @see armarx::ManagedIceObject::onInitComponent()
virtual void onInitComponent() override;
@@ -86,7 +78,6 @@ namespace armarx
private:
-
SimpleStatechartExecutorInterfacePrx _statechartExecutor;
void setupRemoteGuiWidget();
@@ -95,6 +86,5 @@ namespace armarx
std::string _tabName;
void runRemoteGui();
RunningTask<StatechartExecutorExample>::pointer_type _remoteGuiTask;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp b/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp
index ee47310699ea73207a266a6db7319dd2fa09a70d..bbf8f4d0d5b2aa51f763711ea3b6f59b27c1da75 100644
--- a/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp
+++ b/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.cpp
@@ -24,35 +24,35 @@
#include <ArmarXCore/observers/variant/Variant.h>
-
namespace armarx
{
TopicTimingClientPropertyDefinitions::TopicTimingClientPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>("TimingTestTopicName", "TimingTestTopic", "-");
- defineOptionalProperty<int>("SimulateWorkForMS", 20, "Sleeps until it returns from the topic call");
-
- defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
+ defineOptionalProperty<int>(
+ "SimulateWorkForMS", 20, "Sleeps until it returns from the topic call");
+ defineOptionalProperty<std::string>(
+ "DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
}
-
- std::string TopicTimingClient::getDefaultName() const
+ std::string
+ TopicTimingClient::getDefaultName() const
{
return "TopicTimingClient";
}
-
- void TopicTimingClient::onInitComponent()
+ void
+ TopicTimingClient::onInitComponent()
{
usingTopicFromProperty("TimingTestTopicName");
offeringTopicFromProperty("DebugObserverName");
}
-
- void TopicTimingClient::onConnectComponent()
+ void
+ TopicTimingClient::onConnectComponent()
{
// Get topics and proxies here. Pass the *InterfacePrx type as template argument.
debugObserver = getTopicFromProperty<DebugObserverInterfacePrx>("DebugObserverName");
@@ -67,30 +67,26 @@ namespace armarx
simulateWorkForMS = getProperty<int>("SimulateWorkForMS").getValue();
}
-
- void TopicTimingClient::onDisconnectComponent()
+ void
+ TopicTimingClient::onDisconnectComponent()
{
-
}
-
- void TopicTimingClient::onExitComponent()
+ void
+ TopicTimingClient::onExitComponent()
{
-
}
-
-
-
- armarx::PropertyDefinitionsPtr TopicTimingClient::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ TopicTimingClient::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new TopicTimingClientPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new TopicTimingClientPropertyDefinitions(getConfigIdentifier()));
}
-}
-
+} // namespace armarx
-void armarx::TopicTimingClient::reportSmall(const topic_timing::SmallData& data, const Ice::Current&)
+void
+armarx::TopicTimingClient::reportSmall(const topic_timing::SmallData& data, const Ice::Current&)
{
IceUtil::Time time_now = IceUtil::Time::now();
@@ -125,8 +121,9 @@ void armarx::TopicTimingClient::reportSmall(const topic_timing::SmallData& data,
debugObserver->setDebugChannel("TopicTimingClientSmall-" + getName(), channel);
}
-void armarx::TopicTimingClient::reportBig(const topic_timing::BigData& data, const Ice::Current&)
+void
+armarx::TopicTimingClient::reportBig(const topic_timing::BigData& data, const Ice::Current&)
{
IceUtil::Time sentTime = IceUtil::Time::microSeconds(data.sentTimestamp);
- (void) sentTime;
+ (void)sentTime;
}
diff --git a/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.h b/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.h
index fbe94d4eade590937683a29ef88cad2a01608adb..c3cef25b9b88dbbb9a8f760a50462ca7153caccc 100644
--- a/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.h
+++ b/source/RobotAPI/components/TopicTimingTest/TopicTimingClient.h
@@ -23,15 +23,14 @@
#pragma once
-#include <ArmarXCore/core/Component.h>
-
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <RobotAPI/interface/core/TopicTimingTest.h>
+#include <mutex>
#include <boost/circular_buffer.hpp>
-#include <mutex>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <RobotAPI/interface/core/TopicTimingTest.h>
namespace armarx
{
@@ -39,15 +38,12 @@ namespace armarx
* @class TopicTimingClientPropertyDefinitions
* @brief
*/
- class TopicTimingClientPropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
+ class TopicTimingClientPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
TopicTimingClientPropertyDefinitions(std::string prefix);
};
-
-
/**
* @defgroup Component-TopicTimingClient TopicTimingClient
* @ingroup RobotAPI-Components
@@ -59,12 +55,11 @@ namespace armarx
*
* Detailed description of class TopicTimingClient.
*/
- class TopicTimingClient
- : virtual public armarx::Component
- , virtual public armarx::topic_timing::Topic
+ class TopicTimingClient :
+ virtual public armarx::Component,
+ virtual public armarx::topic_timing::Topic
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
virtual std::string getDefaultName() const override;
@@ -73,7 +68,6 @@ namespace armarx
void reportBig(const topic_timing::BigData& data, const Ice::Current&) override;
protected:
-
/// @see armarx::ManagedIceObject::onInitComponent()
virtual void onInitComponent() override;
@@ -99,4 +93,4 @@ namespace armarx
boost::circular_buffer<IceUtil::Time> updateTimes;
int simulateWorkForMS = 20;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/TopicTimingTest/TopicTimingClientMain.cpp b/source/RobotAPI/components/TopicTimingTest/TopicTimingClientMain.cpp
index 63f78571d10286175a171ab7c5c4c244e520ff81..c0e289c49ceb67d2f5c599c4c149fbdeb1b8061e 100644
--- a/source/RobotAPI/components/TopicTimingTest/TopicTimingClientMain.cpp
+++ b/source/RobotAPI/components/TopicTimingTest/TopicTimingClientMain.cpp
@@ -1,10 +1,12 @@
-#include <RobotAPI/components/TopicTimingTest/TopicTimingClient.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotAPI/components/TopicTimingTest/TopicTimingClient.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::TopicTimingClient > (argc, argv, "TopicTimingClient");
+ return armarx::runSimpleComponentApp<armarx::TopicTimingClient>(
+ argc, argv, "TopicTimingClient");
}
diff --git a/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.cpp b/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.cpp
index 48449f8b481a8f2e198a52cb3efa1baa88f3ef4c..2b079b5275b434c6f68f48255de94f7db5fb375d 100644
--- a/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.cpp
+++ b/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.cpp
@@ -23,10 +23,8 @@
#include "TopicTimingServer.h"
#include <ArmarXCore/core/time/CycleUtil.h>
-
#include <ArmarXCore/observers/variant/Variant.h>
-
namespace armarx
{
TopicTimingServerPropertyDefinitions::TopicTimingServerPropertyDefinitions(std::string prefix) :
@@ -35,18 +33,18 @@ namespace armarx
defineOptionalProperty<std::string>("TimingTestTopicName", "TimingTestTopic", "-");
defineOptionalProperty<int>("UpdatePeriodInMS", 10, "-");
- defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
-
+ defineOptionalProperty<std::string>(
+ "DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
}
-
- std::string TopicTimingServer::getDefaultName() const
+ std::string
+ TopicTimingServer::getDefaultName() const
{
return "TopicTimingServer";
}
-
- void TopicTimingServer::onInitComponent()
+ void
+ TopicTimingServer::onInitComponent()
{
offeringTopicFromProperty("TimingTestTopicName");
@@ -55,8 +53,8 @@ namespace armarx
updatePeriodInMS = getProperty<int>("UpdatePeriodInMS").getValue();
}
-
- void TopicTimingServer::onConnectComponent()
+ void
+ TopicTimingServer::onConnectComponent()
{
topic = getTopicFromProperty<armarx::topic_timing::TopicPrx>("TimingTestTopicName");
@@ -68,29 +66,27 @@ namespace armarx
task->start();
}
-
- void TopicTimingServer::onDisconnectComponent()
+ void
+ TopicTimingServer::onDisconnectComponent()
{
task->stop();
task = nullptr;
}
-
- void TopicTimingServer::onExitComponent()
+ void
+ TopicTimingServer::onExitComponent()
{
-
}
-
-
-
- armarx::PropertyDefinitionsPtr TopicTimingServer::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ TopicTimingServer::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new TopicTimingServerPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new TopicTimingServerPropertyDefinitions(getConfigIdentifier()));
}
- void TopicTimingServer::run()
+ void
+ TopicTimingServer::run()
{
CycleUtil c(updatePeriodInMS);
while (!task->isStopped())
@@ -117,4 +113,4 @@ namespace armarx
c.waitForCycleDuration();
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.h b/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.h
index d961b8a6cedc801ae9c76e7bb6b0da892688f477..d84961bca79c103696c639b448e9fd24911da533 100644
--- a/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.h
+++ b/source/RobotAPI/components/TopicTimingTest/TopicTimingServer.h
@@ -24,11 +24,10 @@
#include <ArmarXCore/core/Component.h>
-
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <RobotAPI/interface/core/TopicTimingTest.h>
+#include <RobotAPI/interface/core/TopicTimingTest.h>
namespace armarx
{
@@ -36,15 +35,12 @@ namespace armarx
* @class TopicTimingServerPropertyDefinitions
* @brief
*/
- class TopicTimingServerPropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
+ class TopicTimingServerPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
TopicTimingServerPropertyDefinitions(std::string prefix);
};
-
-
/**
* @defgroup Component-TopicTimingServer TopicTimingServer
* @ingroup RobotAPI-Components
@@ -56,17 +52,14 @@ namespace armarx
*
* Detailed description of class TopicTimingServer.
*/
- class TopicTimingServer :
- virtual public armarx::Component
+ class TopicTimingServer : virtual public armarx::Component
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
virtual std::string getDefaultName() const override;
protected:
-
/// @see armarx::ManagedIceObject::onInitComponent()
virtual void onInitComponent() override;
@@ -96,4 +89,4 @@ namespace armarx
IceUtil::Time lastSmallTime = IceUtil::Time::microSeconds(0);
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/TopicTimingTest/TopicTimingServerMain.cpp b/source/RobotAPI/components/TopicTimingTest/TopicTimingServerMain.cpp
index 1737834bfaf9cadd1322542eddf28b909fa5382f..4927b15dd9e50d6162e79c725137951c70c1985f 100644
--- a/source/RobotAPI/components/TopicTimingTest/TopicTimingServerMain.cpp
+++ b/source/RobotAPI/components/TopicTimingTest/TopicTimingServerMain.cpp
@@ -1,10 +1,12 @@
-#include <RobotAPI/components/TopicTimingTest/TopicTimingServer.h>
-
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/logging/Logging.h>
-int main(int argc, char* argv[])
+#include <RobotAPI/components/TopicTimingTest/TopicTimingServer.h>
+
+int
+main(int argc, char* argv[])
{
- return armarx::runSimpleComponentApp < armarx::TopicTimingServer > (argc, argv, "TopicTimingServer");
+ return armarx::runSimpleComponentApp<armarx::TopicTimingServer>(
+ argc, argv, "TopicTimingServer");
}
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
index a44d697794ceaf47d46266b6e6b515dec9acf84d..fe184e84a8ce430b43c76cbec530b9f1a8c702ae 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.cpp
@@ -25,17 +25,18 @@
#include "ViewSelection.h"
+#include <thread>
+
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/math/ColorUtils.h>
-
-#include <thread>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
namespace armarx
{
- void ViewSelection::onInitComponent()
+ void
+ ViewSelection::onInitComponent()
{
usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
usingProxy(getProperty<std::string>("HeadIKUnitName").getValue());
@@ -58,21 +59,25 @@ namespace armarx
{
visibilityMaskGraph = new CIntensityGraph(graphFileName);
TNodeList* nodes = visibilityMaskGraph->getNodes();
- const float maxOverallHeadTiltAngle = getProperty<float>("MaxOverallHeadTiltAngle").getValue();
+ const float maxOverallHeadTiltAngle =
+ getProperty<float>("MaxOverallHeadTiltAngle").getValue();
const float borderAreaAngle = 10.0f;
const float centralAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
for (size_t i = 0; i < nodes->size(); i++)
{
- CIntensityNode* node = (CIntensityNode*) nodes->at(i);
+ CIntensityNode* node = (CIntensityNode*)nodes->at(i);
- if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle - borderAreaAngle)
+ if (fabs(node->getPosition().fPhi - centralAngle) <
+ maxOverallHeadTiltAngle - borderAreaAngle)
{
node->setIntensity(1.0f);
}
else if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle)
{
- node->setIntensity(1.0f - (fabs(node->getPosition().fPhi - centralAngle) - (maxOverallHeadTiltAngle - borderAreaAngle)) / borderAreaAngle);
+ node->setIntensity(1.0f - (fabs(node->getPosition().fPhi - centralAngle) -
+ (maxOverallHeadTiltAngle - borderAreaAngle)) /
+ borderAreaAngle);
}
else
{
@@ -87,10 +92,11 @@ namespace armarx
return;
}
- sleepingTimeBetweenViewDirectionChanges = getProperty<int>("SleepingTimeBetweenViewDirectionChanges").getValue();
+ sleepingTimeBetweenViewDirectionChanges =
+ getProperty<int>("SleepingTimeBetweenViewDirectionChanges").getValue();
doAutomaticViewSelection = getProperty<bool>("ActiveAtStartup").getValue();
- timeOfLastViewChange = armarx::TimeUtil::GetTime();
+ timeOfLastViewChange = armarx::TimeUtil::GetTime();
// this is robot model specific: offset from the used head coordinate system to the actual
// head center where the eyes are assumed to be located. Here it is correct for ARMAR-III
@@ -100,12 +106,14 @@ namespace armarx
processorTask->setDelayWarningTolerance(sleepingTimeBetweenViewDirectionChanges + 100);
}
-
- void ViewSelection::onConnectComponent()
+ void
+ ViewSelection::onConnectComponent()
{
- robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
+ getProperty<std::string>("RobotStateComponentName").getValue());
- headIKUnitProxy = getProxy<HeadIKUnitInterfacePrx>(getProperty<std::string>("HeadIKUnitName").getValue());
+ headIKUnitProxy =
+ getProxy<HeadIKUnitInterfacePrx>(getProperty<std::string>("HeadIKUnitName").getValue());
headIKUnitProxy->request();
viewSelectionObserver = getTopic<ViewSelectionObserverPrx>(getName() + "Observer");
@@ -114,8 +122,8 @@ namespace armarx
processorTask->start();
}
-
- void ViewSelection::onDisconnectComponent()
+ void
+ ViewSelection::onDisconnectComponent()
{
processorTask->stop();
@@ -135,18 +143,18 @@ namespace armarx
}
}
-
- void ViewSelection::onExitComponent()
+ void
+ ViewSelection::onExitComponent()
{
delete visibilityMaskGraph;
}
-
- void ViewSelection::getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps)
+ void
+ ViewSelection::getActiveSaliencyMaps(std::vector<std::string>& activeSaliencyMaps)
{
std::unique_lock lock(syncMutex);
- IceUtil::Time currentTime = armarx::TimeUtil::GetTime();
+ IceUtil::Time currentTime = armarx::TimeUtil::GetTime();
for (const auto& p : saliencyMaps)
{
if (p.second->validUntil)
@@ -157,14 +165,17 @@ namespace armarx
activeSaliencyMaps.push_back(p.second->name);
}
}
- else if ((currentTime - TimestampVariantPtr::dynamicCast(p.second->timeAdded)->toTime()) < IceUtil::Time::seconds(5))
+ else if ((currentTime -
+ TimestampVariantPtr::dynamicCast(p.second->timeAdded)->toTime()) <
+ IceUtil::Time::seconds(5))
{
activeSaliencyMaps.push_back(p.second->name);
}
}
}
- ViewTargetBasePtr ViewSelection::nextAutomaticViewTarget()
+ ViewTargetBasePtr
+ ViewSelection::nextAutomaticViewTarget()
{
std::vector<std::string> activeSaliencyMaps;
getActiveSaliencyMaps(activeSaliencyMaps);
@@ -196,7 +207,7 @@ namespace armarx
saliency /= activeSaliencyMaps.size();
- CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
+ CIntensityNode* visibilityNode = (CIntensityNode*)visibilityMaskGraphNodes->at(i);
saliency *= visibilityNode->getIntensity();
if (saliency > maxSaliency)
@@ -212,12 +223,14 @@ namespace armarx
ARMARX_DEBUG << "Highest saliency: " << maxSaliency;
// select a new view if saliency is bigger than threshold (which converges towards 0 over time)
- int timeSinceLastViewChange = (armarx::TimeUtil::GetTime() - timeOfLastViewChange).toMilliSeconds();
+ int timeSinceLastViewChange =
+ (armarx::TimeUtil::GetTime() - timeOfLastViewChange).toMilliSeconds();
float saliencyThreshold = 0;
if (timeSinceLastViewChange > 0)
{
- saliencyThreshold = 2.0f * sleepingTimeBetweenViewDirectionChanges / timeSinceLastViewChange;
+ saliencyThreshold =
+ 2.0f * sleepingTimeBetweenViewDirectionChanges / timeSinceLastViewChange;
}
if (maxSaliency > saliencyThreshold)
@@ -226,7 +239,8 @@ namespace armarx
MathTools::convert(visibilityMaskGraphNodes->at(maxIndex)->getPosition(), target);
const float distanceFactor = 1500;
target = distanceFactor * target + offsetToHeadCenter;
- FramedPositionPtr viewTargetPositionPtr = new FramedPosition(target, headFrameName, robotPrx->getName());
+ FramedPositionPtr viewTargetPositionPtr =
+ new FramedPosition(target, headFrameName, robotPrx->getName());
ViewTargetBasePtr viewTarget = new ViewTargetBase();
viewTarget->priority = armarx::DEFAULT_VIEWTARGET_PRIORITY; // * maxSaliency;
@@ -241,15 +255,13 @@ namespace armarx
}
return viewTarget;
-
}
return nullptr;
}
-
-
- void ViewSelection::process()
+ void
+ ViewSelection::process()
{
/*
while(getState() < eManagedIceObjectExiting)
@@ -308,17 +320,34 @@ namespace armarx
{
SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
// FramedPositionPtr viewTargetPositionPtr = FramedPositionPtr::dynamicCast(viewTarget->target);
- FramedPositionPtr viewTargetPositionPtr = new FramedPosition(FramedPositionPtr::dynamicCast(viewTarget->target)->toEigen(), viewTarget->target->frame, robotPrx->getName());
+ FramedPositionPtr viewTargetPositionPtr =
+ new FramedPosition(FramedPositionPtr::dynamicCast(viewTarget->target)->toEigen(),
+ viewTarget->target->frame,
+ robotPrx->getName());
if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
{
float arrowLength = 1500.0f;
- Vector3Ptr startPos = new Vector3(FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())->toEigen());
- FramedDirectionPtr currentDirection = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
- drawer->setArrowDebugLayerVisu("CurrentHeadViewDirection", startPos, currentDirection->toGlobal(robotPrx), DrawColor {0, 0.5, 0.5, 0.1}, arrowLength, 5);
- Eigen::Vector3f plannedDirectionEigen = viewTargetPositionPtr->toGlobalEigen(robotPrx) - startPos->toEigen();
+ Vector3Ptr startPos = new Vector3(
+ FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())
+ ->toEigen());
+ FramedDirectionPtr currentDirection =
+ new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
+ drawer->setArrowDebugLayerVisu("CurrentHeadViewDirection",
+ startPos,
+ currentDirection->toGlobal(robotPrx),
+ DrawColor{0, 0.5, 0.5, 0.1},
+ arrowLength,
+ 5);
+ Eigen::Vector3f plannedDirectionEigen =
+ viewTargetPositionPtr->toGlobalEigen(robotPrx) - startPos->toEigen();
Vector3Ptr plannedDirection = new Vector3(plannedDirectionEigen);
- drawer->setArrowDebugLayerVisu("PlannedHeadViewDirection", startPos, plannedDirection, DrawColor {0.5, 0.5, 0, 0.1}, arrowLength, 5);
+ drawer->setArrowDebugLayerVisu("PlannedHeadViewDirection",
+ startPos,
+ plannedDirection,
+ DrawColor{0.5, 0.5, 0, 0.1},
+ arrowLength,
+ 5);
}
ARMARX_INFO << "Looking at target " << viewTargetPositionPtr->output();
@@ -342,9 +371,8 @@ namespace armarx
}
}
-
-
- void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
+ void
+ ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
{
std::unique_lock lock(manualViewTargetsMutex);
@@ -363,7 +391,8 @@ namespace armarx
condition.notify_all();
}
- void ViewSelection::clearManualViewTargets(const Ice::Current& c)
+ void
+ ViewSelection::clearManualViewTargets(const Ice::Current& c)
{
std::unique_lock lock(manualViewTargetsMutex);
@@ -376,13 +405,15 @@ namespace armarx
// manualViewTargets.swap(temp);
}
- ViewTargetList ViewSelection::getManualViewTargets(const Ice::Current& c)
+ ViewTargetList
+ ViewSelection::getManualViewTargets(const Ice::Current& c)
{
std::unique_lock lock(manualViewTargetsMutex);
ViewTargetList result;
- std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets> temp(manualViewTargets);
+ std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets>
+ temp(manualViewTargets);
while (!temp.empty())
{
@@ -394,9 +425,8 @@ namespace armarx
return result;
}
-
-
- void armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current&)
+ void
+ armarx::ViewSelection::updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current&)
{
std::unique_lock lock(syncMutex);
@@ -407,11 +437,10 @@ namespace armarx
saliencyMaps[map->name] = map;
condition.notify_all();
-
}
-
- void ViewSelection::drawSaliencySphere(const ::Ice::StringSeq& names, const Ice::Current& c)
+ void
+ ViewSelection::drawSaliencySphere(const ::Ice::StringSeq& names, const Ice::Current& c)
{
ARMARX_LOG << "visualizing saliency map";
@@ -419,7 +448,9 @@ namespace armarx
SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
- Eigen::Matrix4f cameraToGlobal = FramedPosePtr::dynamicCast(robotPrx->getRobotNode(headFrameName)->getGlobalPose())->toEigen();
+ Eigen::Matrix4f cameraToGlobal =
+ FramedPosePtr::dynamicCast(robotPrx->getRobotNode(headFrameName)->getGlobalPose())
+ ->toEigen();
std::vector<std::string> activeSaliencyMaps;
@@ -456,7 +487,7 @@ namespace armarx
saliency /= activeSaliencyMaps.size();
- CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
+ CIntensityNode* visibilityNode = (CIntensityNode*)visibilityMaskGraphNodes->at(i);
saliency *= visibilityNode->getIntensity();
Eigen::Vector3d out;
@@ -484,15 +515,16 @@ namespace armarx
}
cloud.pointSize = 10;
drawer->set24BitColoredPointCloudVisu("saliencyMap", "SaliencyCloud", cloud);
-
}
- void ViewSelection::clearSaliencySphere(const Ice::Current& c)
+ void
+ ViewSelection::clearSaliencySphere(const Ice::Current& c)
{
drawer->clearLayer("saliencyMap");
}
- void ViewSelection::removeSaliencyMap(const std::string& name, const Ice::Current& c)
+ void
+ ViewSelection::removeSaliencyMap(const std::string& name, const Ice::Current& c)
{
std::unique_lock lock(syncMutex);
@@ -504,7 +536,8 @@ namespace armarx
condition.notify_all();
}
- Ice::StringSeq ViewSelection::getSaliencyMapNames(const Ice::Current& c)
+ Ice::StringSeq
+ ViewSelection::getSaliencyMapNames(const Ice::Current& c)
{
std::vector<std::string> names;
@@ -518,9 +551,9 @@ namespace armarx
return names;
}
-
- PropertyDefinitionsPtr ViewSelection::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ ViewSelection::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new ViewSelectionPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/ViewSelection/ViewSelection.h b/source/RobotAPI/components/ViewSelection/ViewSelection.h
index 98beeedd5e6d9a176d9b76e83b4b9659d99ef8d5..69dd1535fdb999b5ebfb6c5c53ee781617f4f56d 100644
--- a/source/RobotAPI/components/ViewSelection/ViewSelection.h
+++ b/source/RobotAPI/components/ViewSelection/ViewSelection.h
@@ -26,70 +26,99 @@
#pragma once
+#include <condition_variable>
+#include <mutex>
+#include <queue>
+
+#include <Eigen/Geometry>
+
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/interface/core/RobotState.h>
-
-#include <RobotAPI/interface/components/ViewSelectionInterface.h>
-
-#include <RobotAPI/components/units/HeadIKUnit.h>
-
-#include <RobotAPI/components/EarlyVisionGraph/IntensityGraph.h>
#include <RobotAPI/components/EarlyVisionGraph/GraphPyramidLookupTable.h>
+#include <RobotAPI/components/EarlyVisionGraph/IntensityGraph.h>
#include <RobotAPI/components/EarlyVisionGraph/MathTools.h>
-
-
-#include <Eigen/Geometry>
-
-
-#include <condition_variable>
-#include <mutex>
-#include <queue>
+#include <RobotAPI/components/units/HeadIKUnit.h>
+#include <RobotAPI/interface/components/ViewSelectionInterface.h>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx
{
struct CompareViewTargets
{
- bool operator()(ViewTargetBasePtr const& t1, ViewTargetBasePtr const& t2)
+ bool
+ operator()(ViewTargetBasePtr const& t1, ViewTargetBasePtr const& t2)
{
if (t1->priority == t2->priority)
{
return t1->timeAdded->timestamp < t2->timeAdded->timestamp;
-
}
return t1->priority < t2->priority;
}
};
-
/**
* @class ViewSelectionPropertyDefinitions
* @brief
*/
- class ViewSelectionPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class ViewSelectionPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- ViewSelectionPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ ViewSelectionPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
- defineOptionalProperty<std::string>("HeadIKUnitName", "HeadIKUnit", "Name of the head IK unit component that should be used");
- defineOptionalProperty<std::string>("HeadIKKinematicChainName", "IKVirtualGaze", "Name of the kinematic chain for the head IK");
- defineOptionalProperty<std::string>("HeadFrameName", "Head Base", "Name of the frame of the head base in the robot model");
- defineOptionalProperty<std::string>("CameraFrameName", "VirtualCentralGaze", "Name of the frame of the head base in the robot model");
- defineOptionalProperty<int>("SleepingTimeBetweenViewDirectionChanges", 2500, "Time between two view changes, to keep the head looking into one direction for a while (in ms)");
- defineOptionalProperty<bool>("ActiveAtStartup", true, "Decide whether the automatic view selection will be activated (can be changed via the proxy during runtime)");
- defineOptionalProperty<bool>("VisualizeViewDirection", true, "Draw view ray on DebugLayer.");
- defineOptionalProperty<float>("MaxOverallHeadTiltAngle", 55.0f, "Maximal angle the head and eyes can look down (in degrees)");
- defineOptionalProperty<float>("CentralHeadTiltAngle", 110.0f, "Defines the height direction that will be considered 'central' in the reachable area of the head (in degrees). Default is looking 20 degrees downwards");
- defineOptionalProperty<float>("ProbabilityToLookForALostObject", 0.03f, "Probability that one of the objects that have been seen but could later not been localized again will be included in the view selection");
- defineOptionalProperty<float>("VisuSaliencyThreshold", 0.0f, "If greater than zero the saliency map is drawn into the debug drawer on each iteration. The value is used as minimum saliency threshold for a point to be shown in debug visu");
+ defineOptionalProperty<std::string>(
+ "RobotStateComponentName",
+ "RobotStateComponent",
+ "Name of the robot state component that should be used");
+ defineOptionalProperty<std::string>(
+ "HeadIKUnitName",
+ "HeadIKUnit",
+ "Name of the head IK unit component that should be used");
+ defineOptionalProperty<std::string>("HeadIKKinematicChainName",
+ "IKVirtualGaze",
+ "Name of the kinematic chain for the head IK");
+ defineOptionalProperty<std::string>(
+ "HeadFrameName",
+ "Head Base",
+ "Name of the frame of the head base in the robot model");
+ defineOptionalProperty<std::string>(
+ "CameraFrameName",
+ "VirtualCentralGaze",
+ "Name of the frame of the head base in the robot model");
+ defineOptionalProperty<int>("SleepingTimeBetweenViewDirectionChanges",
+ 2500,
+ "Time between two view changes, to keep the head looking "
+ "into one direction for a while (in ms)");
+ defineOptionalProperty<bool>("ActiveAtStartup",
+ true,
+ "Decide whether the automatic view selection will be "
+ "activated (can be changed via the proxy during runtime)");
+ defineOptionalProperty<bool>(
+ "VisualizeViewDirection", true, "Draw view ray on DebugLayer.");
+ defineOptionalProperty<float>(
+ "MaxOverallHeadTiltAngle",
+ 55.0f,
+ "Maximal angle the head and eyes can look down (in degrees)");
+ defineOptionalProperty<float>(
+ "CentralHeadTiltAngle",
+ 110.0f,
+ "Defines the height direction that will be considered 'central' in the reachable "
+ "area of the head (in degrees). Default is looking 20 degrees downwards");
+ defineOptionalProperty<float>(
+ "ProbabilityToLookForALostObject",
+ 0.03f,
+ "Probability that one of the objects that have been seen but could later not been "
+ "localized again will be included in the view selection");
+ defineOptionalProperty<float>(
+ "VisuSaliencyThreshold",
+ 0.0f,
+ "If greater than zero the saliency map is drawn into the debug drawer on each "
+ "iteration. The value is used as minimum saliency threshold for a point to be "
+ "shown in debug visu");
}
};
@@ -106,15 +135,14 @@ namespace armarx
* @ingroup Component-ViewSelection
* @brief The ViewSelection class
*/
- class ViewSelection :
- virtual public Component,
- virtual public ViewSelectionInterface
+ class ViewSelection : virtual public Component, virtual public ViewSelectionInterface
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "ViewSelection";
}
@@ -145,13 +173,15 @@ namespace armarx
*/
PropertyDefinitionsPtr createPropertyDefinitions() override;
- void addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c = Ice::emptyCurrent) override;
+ void addManualViewTarget(const FramedPositionBasePtr& target,
+ const Ice::Current& c = Ice::emptyCurrent) override;
void clearManualViewTargets(const Ice::Current& c = Ice::emptyCurrent) override;
ViewTargetList getManualViewTargets(const Ice::Current& c = Ice::emptyCurrent) override;
- void activateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
+ void
+ activateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
std::unique_lock lock(manualViewTargetsMutex);
@@ -161,7 +191,8 @@ namespace armarx
viewSelectionObserver->onActivateAutomaticViewSelection();
}
- void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
+ void
+ deactivateAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
std::unique_lock lock(manualViewTargetsMutex);
@@ -171,26 +202,29 @@ namespace armarx
viewSelectionObserver->onDeactivateAutomaticViewSelection();
}
- bool isEnabledAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
+ bool
+ isEnabledAutomaticViewSelection(const Ice::Current& c = Ice::emptyCurrent) override
{
std::unique_lock lock(manualViewTargetsMutex);
return doAutomaticViewSelection;
}
- void updateSaliencyMap(const SaliencyMapBasePtr& map, const Ice::Current& c = Ice::emptyCurrent) override;
+ void updateSaliencyMap(const SaliencyMapBasePtr& map,
+ const Ice::Current& c = Ice::emptyCurrent) override;
- void removeSaliencyMap(const std::string& name, const Ice::Current& c = Ice::emptyCurrent) override;
+ void removeSaliencyMap(const std::string& name,
+ const Ice::Current& c = Ice::emptyCurrent) override;
::Ice::StringSeq getSaliencyMapNames(const Ice::Current& c = Ice::emptyCurrent) override;
- void drawSaliencySphere(const ::Ice::StringSeq& names, const Ice::Current& c = Ice::emptyCurrent) override;
+ void drawSaliencySphere(const ::Ice::StringSeq& names,
+ const Ice::Current& c = Ice::emptyCurrent) override;
void clearSaliencySphere(const Ice::Current& c = Ice::emptyCurrent) override;
private:
-
void process();
ViewTargetBasePtr nextAutomaticViewTarget();
@@ -217,7 +251,8 @@ namespace armarx
bool drawViewDirection;
std::mutex manualViewTargetsMutex;
- std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets> manualViewTargets;
+ std::priority_queue<ViewTargetBasePtr, std::vector<ViewTargetBasePtr>, CompareViewTargets>
+ manualViewTargets;
bool doAutomaticViewSelection;
@@ -230,7 +265,5 @@ namespace armarx
std::map<std::string, SaliencyMapBasePtr> saliencyMaps;
float visuSaliencyThreshold;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.cpp b/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.cpp
index 0ebd0eac1dad18f7c53374c4a4fcc5ba173bbf14..14c6006fb989687a25034866b64c6165b0d810c3 100644
--- a/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.cpp
+++ b/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.cpp
@@ -22,75 +22,73 @@
#include "MemoryNameSystem.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include <SimoxUtility/algorithm/string.h>
-#include <RobotAPI/libraries/armem/core/error.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/armem/core/error.h>
namespace armarx::armem
{
- armarx::PropertyDefinitionsPtr MemoryNameSystem::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ MemoryNameSystem::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
return defs;
}
-
- std::string MemoryNameSystem::getDefaultName() const
+ std::string
+ MemoryNameSystem::getDefaultName() const
{
return "MemoryNameSystem";
}
-
-
- void MemoryNameSystem::onInitComponent()
+ void
+ MemoryNameSystem::onInitComponent()
{
}
-
- void MemoryNameSystem::onConnectComponent()
+ void
+ MemoryNameSystem::onConnectComponent()
{
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
-
- void MemoryNameSystem::onDisconnectComponent()
+ void
+ MemoryNameSystem::onDisconnectComponent()
{
}
-
- void MemoryNameSystem::onExitComponent()
+ void
+ MemoryNameSystem::onExitComponent()
{
}
-
- mns::dto::RegisterServerResult MemoryNameSystem::registerServer(
- const mns::dto::RegisterServerInput& input, const Ice::Current& c)
+ mns::dto::RegisterServerResult
+ MemoryNameSystem::registerServer(const mns::dto::RegisterServerInput& input,
+ const Ice::Current& c)
{
mns::dto::RegisterServerResult result = PluginUser::registerServer(input, c);
tab.rebuild = true;
return result;
}
-
- mns::dto::RemoveServerResult MemoryNameSystem::removeServer(
- const mns::dto::RemoveServerInput& input, const Ice::Current& c)
+ mns::dto::RemoveServerResult
+ MemoryNameSystem::removeServer(const mns::dto::RemoveServerInput& input, const Ice::Current& c)
{
mns::dto::RemoveServerResult result = PluginUser::removeServer(input, c);
tab.rebuild = true;
return result;
}
-
-
// REMOTE GUI
- void MemoryNameSystem::createRemoteGuiTab()
+ void
+ MemoryNameSystem::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
@@ -101,8 +99,8 @@ namespace armarx::armem
RemoteGui_createTab(getName(), root, &tab);
}
-
- void MemoryNameSystem::RemoteGui_update()
+ void
+ MemoryNameSystem::RemoteGui_update()
{
if (tab.rebuild.exchange(false))
{
@@ -110,4 +108,4 @@ namespace armarx::armem
}
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h b/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h
index f0279f4a1103a6b6f5a88f4b31ab5692cc5bda74..de827ddf9a2f9d887ff97519bb106a666498892b 100644
--- a/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h
+++ b/source/RobotAPI/components/armem/MemoryNameSystem/MemoryNameSystem.h
@@ -22,13 +22,12 @@
#pragma once
-#include <RobotAPI/libraries/armem/mns/plugins/PluginUser.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <ArmarXCore/core/Component.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <ArmarXCore/core/Component.h>
-
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/mns/plugins/PluginUser.h>
namespace armarx::armem
{
@@ -44,32 +43,31 @@ namespace armarx::armem
* Detailed description of class MemoryNameSystem.
*/
class MemoryNameSystem :
- virtual public armarx::Component
- , virtual public armem::mns::plugins::PluginUser
- , virtual public LightweightRemoteGuiComponentPluginUser
+ virtual public armarx::Component,
+ virtual public armem::mns::plugins::PluginUser,
+ virtual public LightweightRemoteGuiComponentPluginUser
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
// mns::MemoryNameSystemInterface interface
public:
- mns::dto::RegisterServerResult registerServer(const mns::dto::RegisterServerInput& input, const Ice::Current&) override;
- mns::dto::RemoveServerResult removeServer(const mns::dto::RemoveServerInput& input, const Ice::Current&) override;
+ mns::dto::RegisterServerResult registerServer(const mns::dto::RegisterServerInput& input,
+ const Ice::Current&) override;
+ mns::dto::RemoveServerResult removeServer(const mns::dto::RemoveServerInput& input,
+ const Ice::Current&) override;
// Others are inherited from ComponentPluginUser
// LightweightRemoteGuiComponentPluginUser interface
public:
-
void createRemoteGuiTab();
void RemoteGui_update() override;
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -87,18 +85,17 @@ namespace armarx::armem
private:
-
struct Properties
{
};
- Properties properties;
+ Properties properties;
struct RemoteGuiTab : RemoteGui::Client::Tab
{
std::atomic_bool rebuild = false;
};
- RemoteGuiTab tab;
+ RemoteGuiTab tab;
};
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/components/armem/MemoryNameSystem/test/MemoryNameSystemTest.cpp b/source/RobotAPI/components/armem/MemoryNameSystem/test/MemoryNameSystemTest.cpp
index 3eecee385f44a1a05596c617a938f00048fcc1fd..20be150594d03d2ee56ac2cbc2ecc98d1acd46c9 100644
--- a/source/RobotAPI/components/armem/MemoryNameSystem/test/MemoryNameSystemTest.cpp
+++ b/source/RobotAPI/components/armem/MemoryNameSystem/test/MemoryNameSystemTest.cpp
@@ -24,9 +24,10 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
#include "../MemoryNameSystem.h"
+#include <RobotAPI/Test.h>
+
#include <iostream>
BOOST_AUTO_TEST_CASE(testExample)
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
index fabf44525736293fdbf5f2f84a2bdbef951e2954..9d3b5c9a067591bcf307375f12c7d5f99ddab76a 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -21,24 +21,33 @@
*/
#include "LegacyRobotStateMemoryAdapter.h"
+
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem
{
- armarx::PropertyDefinitionsPtr LegacyRobotStateMemoryAdapter::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ LegacyRobotStateMemoryAdapter::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
std::string prefix = "mem.";
- defs->optional(properties.frequency, prefix + "updateFrequency", "The frequency in Hz to check for updates and send them to the memory.");
- defs->optional(properties.memoryNameSystemName, prefix + "memoryNameSystemName", "The name of the MemoryNameSystem.");
- defs->optional(properties.robotStateMemoryName, prefix + "memoryName", "The name of the RobotStateMemory.");
+ defs->optional(properties.frequency,
+ prefix + "updateFrequency",
+ "The frequency in Hz to check for updates and send them to the memory.");
+ defs->optional(properties.memoryNameSystemName,
+ prefix + "memoryNameSystemName",
+ "The name of the MemoryNameSystem.");
+ defs->optional(properties.robotStateMemoryName,
+ prefix + "memoryName",
+ "The name of the RobotStateMemory.");
prefix = "listener.";
defs->topic<KinematicUnitListener>("RealRobotState", prefix + "KinematicUnitName");
@@ -46,19 +55,21 @@ namespace armarx::armem
return defs;
}
-
- std::string LegacyRobotStateMemoryAdapter::getDefaultName() const
+ std::string
+ LegacyRobotStateMemoryAdapter::getDefaultName() const
{
return "LegacyRobotStateMemoryAdapter";
}
- void LegacyRobotStateMemoryAdapter::checkUpdateAndSendToMemory()
+ void
+ LegacyRobotStateMemoryAdapter::checkUpdateAndSendToMemory()
{
std::lock_guard l(updateMutex);
if (updateChanged)
{
// If the update is not changed it has probably been committed already.
- ARMARX_INFO << deactivateSpam() << "Try to send data to robotStateMemory but data has not changed.";
+ ARMARX_INFO << deactivateSpam()
+ << "Try to send data to robotStateMemory but data has not changed.";
return;
}
@@ -87,21 +98,20 @@ namespace armarx::armem
// is this corect??
prop.platform.acceleration = Eigen::Vector3f::Zero();
- prop.platform.relativePosition = Eigen::Vector3f(update.platformPose.x, // this should be globasl AFAIK
- update.platformPose.y,
- update.platformPose.rotationAroundZ);
+ prop.platform.relativePosition =
+ Eigen::Vector3f(update.platformPose.x, // this should be globasl AFAIK
+ update.platformPose.y,
+ update.platformPose.rotationAroundZ);
prop.platform.velocity = Eigen::Vector3f(std::get<0>(update.platformVelocity),
std::get<1>(update.platformVelocity),
std::get<2>(update.platformVelocity));
armem::EntityUpdate entityUpdate;
entityUpdate.entityID = propEntityID;
- entityUpdate.referencedTime = Time(Duration::MicroSeconds(_timestampUpdateFirstModifiedInUs)); // we take the oldest timestamp
+ entityUpdate.referencedTime = Time(Duration::MicroSeconds(
+ _timestampUpdateFirstModifiedInUs)); // we take the oldest timestamp
- entityUpdate.instancesData =
- {
- prop.toAron()
- };
+ entityUpdate.instancesData = {prop.toAron()};
ARMARX_DEBUG << "Committing " << entityUpdate;
armem::EntityUpdateResult updateResult = memoryWriter.commit(entityUpdate);
@@ -117,22 +127,23 @@ namespace armarx::armem
transform.header.agent = "Armar3";
transform.header.parentFrame = OdometryFrame;
transform.header.frame = armarx::armem::robot_state::constants::robotRootNodeName;
- transform.header.timestamp = armem::Time(armem::Duration::MicroSeconds(_timestampUpdateFirstModifiedInUs));
+ transform.header.timestamp =
+ armem::Time(armem::Duration::MicroSeconds(_timestampUpdateFirstModifiedInUs));
Eigen::Isometry3f tf = Eigen::Isometry3f::Identity();
tf.translation().x() = (update.platformPose.x);
tf.translation().y() = (update.platformPose.y);
- tf.linear() = Eigen::AngleAxisf((update.platformPose.rotationAroundZ), Eigen::Vector3f::UnitZ()).toRotationMatrix();
+ tf.linear() =
+ Eigen::AngleAxisf((update.platformPose.rotationAroundZ), Eigen::Vector3f::UnitZ())
+ .toRotationMatrix();
transform.transform = tf.matrix();
armem::EntityUpdate locUpdate;
locUpdate.entityID = locEntityID;
- locUpdate.referencedTime = Time(Duration::MicroSeconds(_timestampUpdateFirstModifiedInUs));
- locUpdate.instancesData =
- {
- transform.toAron()
- };
+ locUpdate.referencedTime =
+ Time(Duration::MicroSeconds(_timestampUpdateFirstModifiedInUs));
+ locUpdate.instancesData = {transform.toAron()};
ARMARX_DEBUG << "Committing " << entityUpdate;
armem::EntityUpdateResult updateResult = memoryWriter.commit(locUpdate);
@@ -148,7 +159,8 @@ namespace armarx::armem
updateChanged = true;
}
- void LegacyRobotStateMemoryAdapter::updateTimestamps(long ts)
+ void
+ LegacyRobotStateMemoryAdapter::updateTimestamps(long ts)
{
if (updateChanged)
{
@@ -157,12 +169,19 @@ namespace armarx::armem
updateChanged = false;
}
- void LegacyRobotStateMemoryAdapter::reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&)
+ void
+ LegacyRobotStateMemoryAdapter::reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&)
{
-
}
- void LegacyRobotStateMemoryAdapter::reportJointAngles(const NameValueMap& m, Ice::Long ts, bool, const Ice::Current&)
+ void
+ LegacyRobotStateMemoryAdapter::reportJointAngles(const NameValueMap& m,
+ Ice::Long ts,
+ bool,
+ const Ice::Current&)
{
ARMARX_DEBUG << "Got an update for joint angles";
std::lock_guard l(updateMutex);
@@ -170,7 +189,11 @@ namespace armarx::armem
updateTimestamps(ts);
}
- void LegacyRobotStateMemoryAdapter::reportJointVelocities(const NameValueMap& m, Ice::Long ts, bool, const Ice::Current&)
+ void
+ LegacyRobotStateMemoryAdapter::reportJointVelocities(const NameValueMap& m,
+ Ice::Long ts,
+ bool,
+ const Ice::Current&)
{
ARMARX_DEBUG << "Got an update for joint vels";
std::lock_guard l(updateMutex);
@@ -178,7 +201,11 @@ namespace armarx::armem
updateTimestamps(ts);
}
- void LegacyRobotStateMemoryAdapter::reportJointTorques(const NameValueMap& m, Ice::Long ts, bool, const Ice::Current&)
+ void
+ LegacyRobotStateMemoryAdapter::reportJointTorques(const NameValueMap& m,
+ Ice::Long ts,
+ bool,
+ const Ice::Current&)
{
ARMARX_DEBUG << "Got an update for joint torques";
std::lock_guard l(updateMutex);
@@ -186,7 +213,11 @@ namespace armarx::armem
updateTimestamps(ts);
}
- void LegacyRobotStateMemoryAdapter::reportJointAccelerations(const NameValueMap& m, Ice::Long ts, bool, const Ice::Current&)
+ void
+ LegacyRobotStateMemoryAdapter::reportJointAccelerations(const NameValueMap& m,
+ Ice::Long ts,
+ bool,
+ const Ice::Current&)
{
ARMARX_DEBUG << "Got an update for joint accels";
std::lock_guard l(updateMutex);
@@ -194,7 +225,11 @@ namespace armarx::armem
updateTimestamps(ts);
}
- void LegacyRobotStateMemoryAdapter::reportJointCurrents(const NameValueMap& m, Ice::Long ts, bool, const Ice::Current&)
+ void
+ LegacyRobotStateMemoryAdapter::reportJointCurrents(const NameValueMap& m,
+ Ice::Long ts,
+ bool,
+ const Ice::Current&)
{
ARMARX_DEBUG << "Got an update for joint currents";
std::lock_guard l(updateMutex);
@@ -202,14 +237,20 @@ namespace armarx::armem
updateTimestamps(ts);
}
- void LegacyRobotStateMemoryAdapter::reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&)
+ void
+ LegacyRobotStateMemoryAdapter::reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&)
{
-
}
- void LegacyRobotStateMemoryAdapter::reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&)
+ void
+ LegacyRobotStateMemoryAdapter::reportJointStatuses(const NameStatusMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&)
{
-
}
// void LegacyRobotStateMemoryAdapter::reportPlatformPose(const PlatformPose& p, const Ice::Current &)
@@ -220,7 +261,10 @@ namespace armarx::armem
// updateTimestamps(p.timestampInMicroSeconds);
// }
- void LegacyRobotStateMemoryAdapter::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&)
+ void
+ LegacyRobotStateMemoryAdapter::reportGlobalRobotPose(
+ const ::armarx::TransformStamped& transformStamped,
+ const ::Ice::Current&)
{
ARMARX_DEBUG << "Got an update for platform pose";
@@ -240,9 +284,12 @@ namespace armarx::armem
updateTimestamps(p.timestampInMicroSeconds);
}
}
-
-
- void LegacyRobotStateMemoryAdapter::reportPlatformVelocity(Ice::Float f1, Ice::Float f2, Ice::Float f3, const Ice::Current &)
+
+ void
+ LegacyRobotStateMemoryAdapter::reportPlatformVelocity(Ice::Float f1,
+ Ice::Float f2,
+ Ice::Float f3,
+ const Ice::Current&)
{
ARMARX_DEBUG << "Got an update for platform vels";
auto now = IceUtil::Time::now().toMicroSeconds();
@@ -251,7 +298,12 @@ namespace armarx::armem
update.platformVelocity = {f1, f2, f3};
updateTimestamps(now);
}
- void LegacyRobotStateMemoryAdapter::reportPlatformOdometryPose(Ice::Float f1, Ice::Float f2, Ice::Float f3, const Ice::Current &)
+
+ void
+ LegacyRobotStateMemoryAdapter::reportPlatformOdometryPose(Ice::Float f1,
+ Ice::Float f2,
+ Ice::Float f3,
+ const Ice::Current&)
{
ARMARX_DEBUG << "Got an update for platform odom pose";
auto now = IceUtil::Time::now().toMicroSeconds();
@@ -260,10 +312,9 @@ namespace armarx::armem
update.platformOdometryPose = {f1, f2, f3};
updateTimestamps(now);
}
-
-
- void LegacyRobotStateMemoryAdapter::commitArmar3RobotDescription()
+ void
+ LegacyRobotStateMemoryAdapter::commitArmar3RobotDescription()
{
ARMARX_IMPORTANT << "Commiting Armar3 to descriptions";
armem::arondto::RobotDescription desc;
@@ -277,11 +328,12 @@ namespace armarx::armem
entityUpdate.confidence = 1.0;
entityUpdate.entityID.memoryName = armarx::armem::robot_state::constants::memoryName;
- entityUpdate.entityID.coreSegmentName = armarx::armem::robot_state::constants::descriptionCoreSegment;
+ entityUpdate.entityID.coreSegmentName =
+ armarx::armem::robot_state::constants::descriptionCoreSegment;
entityUpdate.entityID.providerSegmentName = "Armar3";
entityUpdate.entityID.entityName = "Armar3";
- entityUpdate.instancesData = { desc.toAron() };
+ entityUpdate.instancesData = {desc.toAron()};
entityUpdate.referencedTime = armem::Time::Now();
auto res = memoryWriter.commit(c);
if (!res.allSuccess())
@@ -290,8 +342,8 @@ namespace armarx::armem
}
}
-
- void LegacyRobotStateMemoryAdapter::onInitComponent()
+ void
+ LegacyRobotStateMemoryAdapter::onInitComponent()
{
usingProxy(properties.memoryNameSystemName);
@@ -301,13 +353,15 @@ namespace armarx::armem
const int fInMS = (1000 / properties.frequency);
// create running task and run method checkUpdateAndSendToMemory in a loop
- runningTask = new PeriodicTask<LegacyRobotStateMemoryAdapter>(this, &LegacyRobotStateMemoryAdapter::checkUpdateAndSendToMemory, fInMS);
+ runningTask = new PeriodicTask<LegacyRobotStateMemoryAdapter>(
+ this, &LegacyRobotStateMemoryAdapter::checkUpdateAndSendToMemory, fInMS);
}
-
- void LegacyRobotStateMemoryAdapter::onConnectComponent()
+ void
+ LegacyRobotStateMemoryAdapter::onConnectComponent()
{
- auto mns = client::MemoryNameSystem(getProxy<mns::MemoryNameSystemInterfacePrx>(properties.memoryNameSystemName), this);
+ auto mns = client::MemoryNameSystem(
+ getProxy<mns::MemoryNameSystemInterfacePrx>(properties.memoryNameSystemName), this);
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Waiting for memory '" << properties.robotStateMemoryName << "' ...";
@@ -328,15 +382,15 @@ namespace armarx::armem
runningTask->start();
}
-
- void LegacyRobotStateMemoryAdapter::onDisconnectComponent()
+ void
+ LegacyRobotStateMemoryAdapter::onDisconnectComponent()
{
runningTask->stop();
}
-
- void LegacyRobotStateMemoryAdapter::onExitComponent()
- {
+ void
+ LegacyRobotStateMemoryAdapter::onExitComponent()
+ {
runningTask->stop();
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
index 71ab81f1a3582fafeed767ae7dcfc00f20cf0cad..d5a84a63bd9878767f91d8ea2499945c824cd0a6 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.h
@@ -27,15 +27,16 @@
// Base Classes
#include <ArmarXCore/core/Component.h>
+
#include <RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.h>
// ArmarX
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
+
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
#include <RobotAPI/libraries/core/FramedPose.h>
@@ -46,27 +47,42 @@ namespace armarx::armem
virtual public robot_state::LegacyRobotStateMemoryAdapterInterface
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
- void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) override;
+ void reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override;
void reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
- void reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
+ void
+ reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
- void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
- void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
- void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
- void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override;
+ void reportJointAccelerations(const NameValueMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override;
+ void
+ reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override;
+ void reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long,
+ bool,
+ const Ice::Current&) override;
+ void
+ reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override;
// void reportPlatformPose(const PlatformPose &, const Ice::Current &) override;
- void reportPlatformVelocity(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
- void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override;
+ void
+ reportPlatformVelocity(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;
+ void reportPlatformOdometryPose(Ice::Float,
+ Ice::Float,
+ Ice::Float,
+ const Ice::Current&) override;
- void reportGlobalRobotPose(const ::armarx::TransformStamped&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
+ void reportGlobalRobotPose(const ::armarx::TransformStamped&,
+ const ::Ice::Current& = ::Ice::emptyCurrent) override;
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -114,17 +130,26 @@ namespace armarx::armem
std::string robotStateMemoryName = "RobotState";
int frequency = 100;
};
+
Properties properties;
// RunningTask
armarx::PeriodicTask<LegacyRobotStateMemoryAdapter>::pointer_type runningTask;
// Memory
- MemoryID propEntityID = MemoryID(armarx::armem::robot_state::constants::memoryName, armarx::armem::robot_state::constants::proprioceptionCoreSegment, "Armar3", "Armar3");
- MemoryID locEntityID = MemoryID(armarx::armem::robot_state::constants::memoryName, armarx::armem::robot_state::constants::localizationCoreSegment, "Armar3", (OdometryFrame + "," + armarx::armem::robot_state::constants::robotRootNodeName));
+ MemoryID propEntityID =
+ MemoryID(armarx::armem::robot_state::constants::memoryName,
+ armarx::armem::robot_state::constants::proprioceptionCoreSegment,
+ "Armar3",
+ "Armar3");
+ MemoryID locEntityID = MemoryID(
+ armarx::armem::robot_state::constants::memoryName,
+ armarx::armem::robot_state::constants::localizationCoreSegment,
+ "Armar3",
+ (OdometryFrame + "," + armarx::armem::robot_state::constants::robotRootNodeName));
armem::client::Writer memoryWriter;
mutable std::mutex updateMutex;
};
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
index b7be203d48161b80490ea59125737fae0a33cb17..8116b8562e0e6680d5a3fde0dfe9270206416121 100644
--- a/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
+++ b/source/RobotAPI/components/armem/client/ExampleMemoryClient/ExampleMemoryClient.h
@@ -23,18 +23,17 @@
#pragma once
-#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx
{
@@ -57,7 +56,6 @@ namespace armarx
virtual public armarx::LightweightRemoteGuiComponentPluginUser
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -69,7 +67,6 @@ namespace armarx
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -81,9 +78,9 @@ namespace armarx
private:
-
// Callback for updates on `example_entity`.
- void processExampleEntityUpdate(const armem::MemoryID& id, const std::vector<armem::MemoryID>& snapshotIDs);
+ void processExampleEntityUpdate(const armem::MemoryID& id,
+ const std::vector<armem::MemoryID>& snapshotIDs);
// Examples
void waitForMemory();
@@ -108,12 +105,12 @@ namespace armarx
private:
-
struct Properties
{
std::string usedMemoryName = "Example";
float commitFrequency = 10;
};
+
Properties p;
armem::client::Reader memoryReader;
@@ -142,6 +139,7 @@ namespace armarx
std::optional<armem::wm::Memory> queryResult;
RemoteGui::Client::GroupBox queryResultGroup;
};
+
RemoteGuiTab tab;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
index 7c5291ade05e93034da21238af4c0cafbccea30c..696784e612fe5fc1b21974676b48ff9976164a9c 100644
--- a/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
+++ b/source/RobotAPI/components/armem/client/GraspProviderExample/GraspProviderExample.h
@@ -1,40 +1,35 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
-#include <RobotAPI/libraries/core/Pose.h>
+
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h>
-
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/core/Pose.h>
#pragma once
-
namespace armarx
{
- class GraspProviderExamplePropertyDefinitions :
- public armarx::ComponentPropertyDefinitions
+ class GraspProviderExamplePropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
GraspProviderExamplePropertyDefinitions(std::string prefix);
};
-
class GraspProviderExample :
virtual public armarx::Component,
virtual public armarx::armem::ClientPluginUser
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
GraspProviderExample();
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
void onConnectComponent() override;
@@ -48,7 +43,6 @@ namespace armarx
private:
-
Eigen::Matrix4f const identityMatrix = Eigen::Matrix4f::Identity();
Eigen::Vector3f const zeroVector = Eigen::Vector3f();
@@ -60,4 +54,4 @@ namespace armarx
armarx::armem::GraspCandidateWriter writer;
armarx::armem::GraspCandidateReader reader;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
index 318ab3d38183f7cf93337082b5b0386c5451f609..0c3c1115b80b5e38a6e6a5458bfa28b94b8fd355 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.cpp
@@ -43,7 +43,6 @@
#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
-
namespace armarx
{
@@ -58,7 +57,6 @@ namespace armarx
return defs;
}
-
ObjectInstanceToIndex::ObjectInstanceToIndex()
{
addPlugin(objectClientPlugin);
@@ -70,7 +68,6 @@ namespace armarx
return "ObjectInstanceToIndex";
}
-
void
ObjectInstanceToIndex::onInitComponent()
{
@@ -83,7 +80,6 @@ namespace armarx
armem::objects::instaceSegmentID, this, &This::processObjectInstance);
}
-
void
ObjectInstanceToIndex::onConnectComponent()
{
@@ -102,19 +98,16 @@ namespace armarx
}
}
-
void
ObjectInstanceToIndex::onDisconnectComponent()
{
}
-
void
ObjectInstanceToIndex::onExitComponent()
{
}
-
void
ObjectInstanceToIndex::createRemoteGuiTab()
{
@@ -124,13 +117,11 @@ namespace armarx
RemoteGui_createTab(getName(), root, &tab);
}
-
void
ObjectInstanceToIndex::RemoteGui_update()
{
}
-
void
ObjectInstanceToIndex::processRobotState(const armem::MemoryID& id,
const std::vector<armem::MemoryID>& snapshotIDs)
@@ -209,7 +200,6 @@ namespace armarx
#endif
}
-
void
ObjectInstanceToIndex::processObjectInstance(const armem::MemoryID& id,
const std::vector<armem::MemoryID>& snapshotIDs)
@@ -224,11 +214,10 @@ namespace armarx
armem::index::spatialSegmentID.withProviderSegmentName(getName()),
.indexNamedProviderSegmentID =
armem::index::namedSegmentID.withProviderSegmentName(getName()),
- .params = armem::objects::ObjectInstanceToIndex::Parameters{
- .maxFrequency = armarx::Frequency::Hertz(properties.object.maxFrequencyHz)
- },
- .state = {}
- };
+ .params =
+ armem::objects::ObjectInstanceToIndex::Parameters{
+ .maxFrequency = armarx::Frequency::Hertz(properties.object.maxFrequencyHz)},
+ .state = {}};
}
ARMARX_CHECK(object.has_value());
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h
index ca3344af472d7fc3ee8b4b2e02a9134d5a05c892..f8609aab15d49d67e1617074d668a42de16701a6 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/ObjectInstanceToIndex.h
@@ -36,7 +36,6 @@
#include "impl/ObjectInstanceToIndex.h"
-
namespace armarx
{
@@ -93,8 +92,10 @@ namespace armarx
{
float maxFrequencyHz = 10;
};
+
Object object;
};
+
Properties properties;
armem::client::Writer indexSpatialMemoryWriter;
@@ -112,6 +113,7 @@ namespace armarx
struct RemoteGuiTab : RemoteGui::Client::Tab
{
};
+
RemoteGuiTab tab;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp
index 34901591b175f4b5e09dcef339868bbe488d5b50..537c31c501d6fcff9123aaa643632e48735ad539 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.cpp
@@ -123,7 +123,6 @@ namespace armarx::armem::objects
indexSpatialMemoryWriter.commit(commit);
}
-
std::vector<const objpose::ObjectPose*>
ObjectInstanceToIndex::filterObjectPoses(const objpose::ObjectPoseSeq& objectPoses,
const std::vector<MemoryID>& updatedSnapshotIDs)
diff --git a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h
index dd8f29399757375351a9c855e77cd1e009a00f6f..6f0ceed645c69018cffb4eab5fd3b1c9ce52c3ce 100644
--- a/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h
+++ b/source/RobotAPI/components/armem/client/ObjectInstanceToIndex/impl/ObjectInstanceToIndex.h
@@ -30,7 +30,6 @@
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
namespace armarx::armem::objects
{
@@ -52,17 +51,18 @@ namespace armarx::armem::objects
armem::MemoryID indexSpatialProviderSegmentID;
armem::MemoryID indexNamedProviderSegmentID;
-
struct Parameters
{
armarx::Frequency maxFrequency = armarx::Frequency::HertzDouble(60);
};
+
Parameters params;
struct State
{
std::map<armarx::ObjectID, armarx::DateTime> latestUpdateDateTimes;
};
+
State state;
};
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h
index 198257f6b033cfc0433ca5b0caad18e47ee77e45..c5f45e6459b3e9d88e946ffd735820ed018ccbdf 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/Component.h
@@ -30,18 +30,15 @@
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
// For some reason, the generated main requires Impl to be complete ...
#include "Impl.h"
-
namespace armarx::robot_state_prediction_client_example
{
class Impl;
-
/**
* @defgroup Component-ExampleClient ExampleClient
* @ingroup RobotAPI-Components
@@ -55,10 +52,10 @@ namespace armarx::robot_state_prediction_client_example
* Connects to the example memory, and commits and queries example data.
*/
class Component :
- virtual public armarx::Component
- , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- , virtual public armarx::ArVizComponentPluginUser
- , virtual public armarx::armem::ClientPluginUser
+ virtual public armarx::Component,
+ virtual public armarx::LightweightRemoteGuiComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::armem::ClientPluginUser
{
public:
using Impl = robot_state_prediction_client_example::Impl;
@@ -78,7 +75,6 @@ namespace armarx::robot_state_prediction_client_example
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -88,13 +84,12 @@ namespace armarx::robot_state_prediction_client_example
private:
-
std::unique_ptr<Impl> pimpl = nullptr;
struct RemoteGuiTab : RemoteGui::Client::Tab
{
};
- RemoteGuiTab tab;
+ RemoteGuiTab tab;
};
-}
+} // namespace armarx::robot_state_prediction_client_example
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
index 94fbe1b178a3d538601976da47f28215d8ec30d0..b4390b1a05f51182e4264397bae56b03218fa1b4 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.cpp
@@ -38,7 +38,6 @@
#include "simox_alg.hpp"
-
namespace armarx::armem::robot_state
{
@@ -46,21 +45,18 @@ namespace armarx::armem::robot_state
{
}
-
std::vector<armem::MemoryID>
RobotStatePredictionClient::queryLocalizationEntityIDs()
{
return _queryEntityIDs(armem::robot_state::constants::localizationCoreSegment);
}
-
std::vector<MemoryID>
RobotStatePredictionClient::queryProprioceptionEntityIDs()
{
return _queryEntityIDs(armem::robot_state::constants::proprioceptionCoreSegment);
}
-
std::vector<MemoryID>
RobotStatePredictionClient::_queryEntityIDs(const std::string& coreSegmentName)
{
@@ -85,7 +81,6 @@ namespace armarx::armem::robot_state
}
}
-
std::vector<armem::PredictionRequest>
RobotStatePredictionClient::makePredictionRequests(
const std::vector<armem::MemoryID>& entityIDs,
@@ -104,7 +99,6 @@ namespace armarx::armem::robot_state
return requests;
}
-
std::vector<PredictionResult>
RobotStatePredictionClient::predict(const std::vector<MemoryID>& entityIDs,
Time predictedTime,
@@ -115,7 +109,6 @@ namespace armarx::armem::robot_state
return predict(requests);
}
-
std::vector<PredictionResult>
RobotStatePredictionClient::predict(const std::vector<PredictionRequest>& requests)
{
@@ -124,7 +117,6 @@ namespace armarx::armem::robot_state
return results;
}
-
std::optional<Eigen::Affine3f>
RobotStatePredictionClient::lookupGlobalPose(
const std::vector<armem::PredictionResult>& localizationPredictionResults,
@@ -220,7 +212,6 @@ namespace armarx::armem::robot_state
}
}
-
std::optional<Eigen::Affine3f>
RobotStatePredictionClient::predictGlobalPose(const std::vector<MemoryID>& entityIDs,
Time predictedTime,
@@ -231,7 +222,6 @@ namespace armarx::armem::robot_state
return pose;
}
-
std::optional<std::map<std::string, float>>
RobotStatePredictionClient::predictJointPositions(const std::vector<MemoryID>& entityIDs,
Time predictedTime,
@@ -242,7 +232,6 @@ namespace armarx::armem::robot_state
return lookupJointPositions(results, robotName);
}
-
RobotStatePredictionClient::WholeBodyPrediction
RobotStatePredictionClient::predictWholeBody(const std::vector<armem::MemoryID>& locEntityIDs,
const std::vector<armem::MemoryID>& propEntityIDs,
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h
index 6ef31275e1efbf7ea9f5fd43ce2fb467d54b2bd7..321f69cec71c09cd84fa04bf689ba3ee2ead825a 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClient.h
@@ -32,7 +32,6 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
-
namespace armarx::armem::robot_state
{
@@ -41,12 +40,12 @@ namespace armarx::armem::robot_state
public:
RobotStatePredictionClient();
-
struct WholeBodyPrediction
{
std::optional<Eigen::Affine3f> globalPose;
std::optional<std::map<std::string, float>> jointPositions;
};
+
WholeBodyPrediction
predictWholeBody(const std::vector<armem::MemoryID>& localizationEntityIDs,
const std::vector<armem::MemoryID>& proprioceptionEntityIDs,
@@ -100,6 +99,7 @@ namespace armarx::armem::robot_state
armem::client::Reader reader;
std::optional<armem::robot_state::VirtualRobotReader> robotReader;
};
+
Remote remote;
};
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h
index 3023fef630da40fc660dc828bd66c14c5060982d..c7beed04cf328bbdce225d8782024892aae3c46e 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/RobotStatePredictionClientExample.h
@@ -24,8 +24,8 @@
#include "Component.h"
-
namespace armarx
{
- using RobotStatePredictionClientExample = armarx::robot_state_prediction_client_example::Component;
+ using RobotStatePredictionClientExample =
+ armarx::robot_state_prediction_client_example::Component;
}
diff --git a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp
index 4a56a6ca012eb3b3180a757005f9a5d5633b1f82..85199992ae567657a51808fb1dd36c5be589158d 100644
--- a/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp
+++ b/source/RobotAPI/components/armem/client/RobotStatePredictionClientExample/simox_alg.hpp
@@ -36,7 +36,6 @@ namespace simox::alg
return conc;
}
-
template <class KeyT, class ValueT>
std::map<KeyT, ValueT>
map_from_key_value_pairs(const std::vector<KeyT>& lhs, const std::vector<ValueT>& rhs)
@@ -51,7 +50,6 @@ namespace simox::alg
return map;
}
-
template <class KeyT, class ValueT>
std::vector<ValueT>
multi_at(const std::map<KeyT, ValueT>& map,
diff --git a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
index df79259c85fb27ef40299fabf3af03e978c7f2e5..053045f47c1933cc7bb1d6950f5ddde25306a6c1 100644
--- a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
+++ b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.cpp
@@ -25,12 +25,11 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/time/Frequency.h>
-#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/Metronome.h>
namespace armarx::simple_virtual_robot
{
@@ -42,19 +41,27 @@ namespace armarx::simple_virtual_robot
armarx::PropertyDefinitionsPtr
SimpleVirtualRobot::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
// defs->topic(debugObserver);
- defs->optional(properties.oneShot, "p.oneShot", "If true, commit once after connecting, then stop.");
- defs->optional(properties.updateFrequency, "p.updateFrequency", "Memory update frequency (write).");
-
- defs->optional(properties.robot.name, "p.robot.name",
- "Optional override for the robot name. If not set, the default name from the robot model is used.");
- defs->optional(properties.robot.package, "p.robot.package", "Package of the Simox robot XML.");
+ defs->optional(
+ properties.oneShot, "p.oneShot", "If true, commit once after connecting, then stop.");
+ defs->optional(
+ properties.updateFrequency, "p.updateFrequency", "Memory update frequency (write).");
+
+ defs->optional(properties.robot.name,
+ "p.robot.name",
+ "Optional override for the robot name. If not set, the default name from "
+ "the robot model is used.");
+ defs->optional(
+ properties.robot.package, "p.robot.package", "Package of the Simox robot XML.");
defs->optional(properties.robot.path, "p.robot.path", "Local path of the Simox robot XML.");
- defs->optional(properties.robot.jointValues, "p.robot.jointValues", "Specify a certain joint configuration.");
+ defs->optional(properties.robot.jointValues,
+ "p.robot.jointValues",
+ "Specify a certain joint configuration.");
defs->optional(properties.robot.globalPositionX, "p.robot.globalPositionX", "");
defs->optional(properties.robot.globalPositionY, "p.robot.globalPositionY", "");
@@ -77,10 +84,7 @@ namespace armarx::simple_virtual_robot
void
SimpleVirtualRobot::onConnectComponent()
{
- task = new armarx::SimpleRunningTask<>([this]()
- {
- this->run();
- });
+ task = new armarx::SimpleRunningTask<>([this]() { this->run(); });
task->start();
}
@@ -95,8 +99,6 @@ namespace armarx::simple_virtual_robot
{
}
-
-
VirtualRobot::RobotPtr
SimpleVirtualRobot::loadRobot(const Properties::Robot& p) const
{
@@ -110,12 +112,12 @@ namespace armarx::simple_virtual_robot
PackagePath path(p.package, p.path);
ARMARX_INFO << "Load robot from " << path << ".";
- VirtualRobot::RobotPtr robot =
- VirtualRobot::RobotIO::loadRobot(path.toSystemPath(), VirtualRobot::RobotIO::eStructure);
+ VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(
+ path.toSystemPath(), VirtualRobot::RobotIO::eStructure);
- ARMARX_CHECK_NOT_NULL(robot) << "Failed to load robot `" << path << "`.";
+ ARMARX_CHECK_NOT_NULL(robot) << "Failed to load robot `" << path << "`.";
- if(not p.jointValues.empty())
+ if (not p.jointValues.empty())
{
ARMARX_DEBUG << "Parsing: " << p.jointValues;
@@ -129,7 +131,10 @@ namespace armarx::simple_virtual_robot
robot->setJointValues(jointValues);
}
- const Eigen::Isometry3f global_T_robot = Eigen::Translation3f{properties.robot.globalPositionX, properties.robot.globalPositionY, 0} * Eigen::AngleAxisf{properties.robot.globalPositionYaw, Eigen::Vector3f::UnitZ()};
+ const Eigen::Isometry3f global_T_robot =
+ Eigen::Translation3f{
+ properties.robot.globalPositionX, properties.robot.globalPositionY, 0} *
+ Eigen::AngleAxisf{properties.robot.globalPositionYaw, Eigen::Vector3f::UnitZ()};
robot->setGlobalPose(global_T_robot.matrix());
if (not p.name.empty())
@@ -140,7 +145,6 @@ namespace armarx::simple_virtual_robot
return robot;
}
-
void
SimpleVirtualRobot::run()
{
diff --git a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
index d960e0ff3955b8159c63ed9c6e1eff465e26a3a2..967e400ead4b2b7e583e79480d722f2740109bb9 100644
--- a/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
+++ b/source/RobotAPI/components/armem/client/SimpleVirtualRobot/SimpleVirtualRobot.h
@@ -8,11 +8,10 @@
// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
+#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h>
-
namespace armarx::simple_virtual_robot
{
@@ -47,9 +46,7 @@ namespace armarx::simple_virtual_robot
void run();
-
private:
-
struct Properties
{
bool oneShot = true;
@@ -67,6 +64,7 @@ namespace armarx::simple_virtual_robot
float globalPositionY = 0;
float globalPositionYaw = 0;
};
+
Robot robot;
};
@@ -82,9 +80,7 @@ namespace armarx::simple_virtual_robot
armarx::SimpleRunningTask<>::pointer_type task;
armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotWriter>*
- virtualRobotWriterPlugin = nullptr;
-
-
+ virtualRobotWriterPlugin = nullptr;
};
-} // namespace armarx::simple_virtual_robot
+} // namespace armarx::simple_virtual_robot
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
index f3d1b870cecb1100c521bf9017120f0b25a738fa..5decaf9b04504670555a9d99b988e1f8dedd032f 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.cpp
@@ -2,67 +2,64 @@
#include "VirtualRobotReaderExampleClient.h"
-#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/Metronome.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-
namespace armarx::robot_state
{
VirtualRobotReaderExampleClient::VirtualRobotReaderExampleClient() = default;
- armarx::PropertyDefinitionsPtr VirtualRobotReaderExampleClient::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ VirtualRobotReaderExampleClient::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs =
new ComponentPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
- defs->optional(properties.robotName, "p.robotName",
- "The name of the robot to use.");
- defs->optional(properties.updateFrequency, "p.updateFrequency [Hz]",
+ defs->optional(properties.robotName, "p.robotName", "The name of the robot to use.");
+ defs->optional(properties.updateFrequency,
+ "p.updateFrequency [Hz]",
"The frequency of the running loop.");
return defs;
}
-
- std::string VirtualRobotReaderExampleClient::getDefaultName() const
+ std::string
+ VirtualRobotReaderExampleClient::getDefaultName() const
{
return "VirtualRobotReaderExampleClient";
}
-
- void VirtualRobotReaderExampleClient::onInitComponent()
+ void
+ VirtualRobotReaderExampleClient::onInitComponent()
{
}
-
- void VirtualRobotReaderExampleClient::onConnectComponent()
+ void
+ VirtualRobotReaderExampleClient::onConnectComponent()
{
ARMARX_IMPORTANT << "Running virtual robot synchronization example.";
- task = new SimpleRunningTask<>([this]()
- {
- this->run();
- });
+ task = new SimpleRunningTask<>([this]() { this->run(); });
task->start();
}
-
- void VirtualRobotReaderExampleClient::onDisconnectComponent()
+ void
+ VirtualRobotReaderExampleClient::onDisconnectComponent()
{
task->stop();
}
-
- void VirtualRobotReaderExampleClient::onExitComponent()
+ void
+ VirtualRobotReaderExampleClient::onExitComponent()
{
}
-
- void VirtualRobotReaderExampleClient::run()
+ void
+ VirtualRobotReaderExampleClient::run()
{
Metronome metronome(Frequency::Hertz(properties.updateFrequency));
while (task and not task->isStopped())
@@ -98,10 +95,10 @@ namespace armarx::robot_state
// Do something with the robot (your code follows here, there are just a examples) ...
Eigen::Matrix4f globalPose = robot->getGlobalPose();
- (void) globalPose;
+ (void)globalPose;
std::vector<std::string> nodeNames = robot->getRobotNodeNames();
- (void) nodeNames;
+ (void)nodeNames;
// End.
}
@@ -110,4 +107,4 @@ namespace armarx::robot_state
}
}
-} // namespace armarx::robot_state
+} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
index 328f847e8a36655d24ba68ee09b13fd189cb0d02..20fd1dec54ef626826d082f49cf99d8bbea08ce4 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
+++ b/source/RobotAPI/components/armem/client/VirtualRobotReaderExampleClient/VirtualRobotReaderExampleClient.h
@@ -22,15 +22,14 @@
#pragma once
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
+#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
-
namespace armarx::robot_state
{
@@ -57,7 +56,6 @@ namespace armarx::robot_state
std::string getDefaultName() const override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -67,17 +65,16 @@ namespace armarx::robot_state
private:
-
void run();
private:
-
struct Properties
{
std::string robotName{"Armar6"};
float updateFrequency{10.F};
};
+
Properties properties;
// The running task which is started in onConnectComponent().
@@ -95,7 +92,6 @@ namespace armarx::robot_state
// For publishing timing information.
armarx::DebugObserverInterfacePrx debugObserver;
-
};
-} // namespace armarx::robot_state
+} // namespace armarx::robot_state
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
index 675d2dae9e79c28d5a2fe2d8ce9630963d878dc0..2abd2486c310ebab4f874318e22fd025c6b137f1 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
@@ -105,13 +105,12 @@ namespace armarx::virtual_robot_writer_example
VirtualRobot::RobotPtr
VirtualRobotWriterExample::loadRobot() const
{
- auto robot =
- VirtualRobot::RobotIO::loadRobot(PackagePath(p.robot.package, p.robot.path).toSystemPath(),
- VirtualRobot::RobotIO::eStructure);
+ auto robot = VirtualRobot::RobotIO::loadRobot(
+ PackagePath(p.robot.package, p.robot.path).toSystemPath(),
+ VirtualRobot::RobotIO::eStructure);
return robot;
}
-
void
VirtualRobotWriterExample::run()
{
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.h b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.h
index 9a13bc9500442584fba3f6015ec2922bac53914f..f1d00dee85ef2a1824570fff59eb697f67ee345c 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.h
+++ b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.h
@@ -3,8 +3,8 @@
#include <VirtualRobot/VirtualRobot.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
@@ -12,10 +12,10 @@
#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h"
-#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx::virtual_robot_writer_example
@@ -61,7 +61,8 @@ namespace armarx::virtual_robot_writer_example
armarx::PeriodicTask<VirtualRobotWriterExample>::pointer_type task;
- armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotWriter>* virtualRobotWriterPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotWriter>*
+ virtualRobotWriterPlugin = nullptr;
struct Properties
{
@@ -75,4 +76,4 @@ namespace armarx::virtual_robot_writer_example
} p;
};
-} // namespace armarx::virtual_robot_writer_example
+} // namespace armarx::virtual_robot_writer_example
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index 981489f8621918a733208a0dc7c6c20d274ecfe9..5cdfe078f450e8871640c9f8a1d724679da7eb1d 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -22,61 +22,64 @@
#include "ExampleMemory.h"
+#include <SimoxUtility/algorithm/string.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
-#include <SimoxUtility/algorithm/string.h>
-
+#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/Prediction.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
-
-
namespace armarx
{
- armarx::PropertyDefinitionsPtr ExampleMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ ExampleMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
defs->topic(debugObserver);
setMemoryName("Example");
p.core._defaultSegmentsStr = simox::alg::join(p.core.defaultCoreSegments, ", ");
- defs->optional(p.core._defaultSegmentsStr, "core.DefaultSegments",
+ defs->optional(p.core._defaultSegmentsStr,
+ "core.DefaultSegments",
"Core segments to add on start up (just as example).");
- defs->optional(p.core.addOnUsage, "core.AddOnUsage",
- "If enabled, core segments are added when required by a new provider segment."
- "This will usually be off for most memory servers.");
-
- defs->optional(p.enableRemoteGui, "p.enableRemoteGui",
+ defs->optional(
+ p.core.addOnUsage,
+ "core.AddOnUsage",
+ "If enabled, core segments are added when required by a new provider segment."
+ "This will usually be off for most memory servers.");
+
+ defs->optional(p.enableRemoteGui,
+ "p.enableRemoteGui",
"If true, the memory cotent is shown in the remote gui."
"Can be very slow for high-frequency updates!");
return defs;
}
-
- std::string ExampleMemory::getDefaultName() const
+ std::string
+ ExampleMemory::getDefaultName() const
{
return "ExampleMemory";
}
-
- void ExampleMemory::onInitComponent()
+ void
+ ExampleMemory::onInitComponent()
{
// Usually, the memory server will specify a number of core segments with a specific aron type.
workingMemory().addCoreSegment("ExampleData", armem::example::ExampleData::ToAronType());
workingMemory().addCoreSegment("LinkedData", armem::example::LinkedData::ToAronType());
// We support the "Latest" prediction engine for the entire memory.
- workingMemory().addPredictor(
- armem::PredictionEngine{.engineID = "Latest"},
- [this](const armem::PredictionRequest& request)
- { return this->predictLatest(request); });
+ workingMemory().addPredictor(armem::PredictionEngine{.engineID = "Latest"},
+ [this](const armem::PredictionRequest& request)
+ { return this->predictLatest(request); });
// For illustration purposes, we add more segments (without types).
bool trim = true;
@@ -90,7 +93,8 @@ namespace armarx
}
}
- void ExampleMemory::onConnectComponent()
+ void
+ ExampleMemory::onConnectComponent()
{
if (p.enableRemoteGui)
{
@@ -99,28 +103,30 @@ namespace armarx
}
}
- void ExampleMemory::onDisconnectComponent()
+ void
+ ExampleMemory::onDisconnectComponent()
{
}
- void ExampleMemory::onExitComponent()
+ void
+ ExampleMemory::onExitComponent()
{
}
-
-
// WRITING
- armem::data::AddSegmentsResult ExampleMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
+ armem::data::AddSegmentsResult
+ ExampleMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
{
// This function is overloaded to trigger the remote gui rebuild.
- armem::data::AddSegmentsResult result = ReadWritePluginUser::addSegments(input, p.core.addOnUsage);
+ armem::data::AddSegmentsResult result =
+ ReadWritePluginUser::addSegments(input, p.core.addOnUsage);
tab.rebuild = true;
return result;
}
-
- armem::data::CommitResult ExampleMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
+ armem::data::CommitResult
+ ExampleMemory::commit(const armem::data::Commit& commit, const Ice::Current&)
{
// This function is overloaded to trigger the remote gui rebuild.
armem::data::CommitResult result = ReadWritePluginUser::commit(commit);
@@ -128,42 +134,35 @@ namespace armarx
return result;
}
-
// READING
// Inherited from Plugin
-
// ACTIONS
armem::actions::GetActionsOutputSeq
- ExampleMemory::getActions(
- const armem::actions::GetActionsInputSeq& input)
+ ExampleMemory::getActions(const armem::actions::GetActionsInputSeq& input)
{
using namespace armem::actions;
Action greeting{"hi", "Say hello to " + input[0].id.entityName};
Action failure{"fail", "Fail dramatically"};
Action nothing{"null", "Do nothing, but deeply nested"};
- SubMenu one{"one", "One", { nothing } };
- SubMenu two{"two", "Two", { one } };
- SubMenu three{"three", "Three", { two } };
- SubMenu four{"four", "Four", { three } };
-
- Menu menu {
- greeting,
- failure,
- four,
- SubMenu{"mut", "Mutate", {
- Action{"copy", "Copy latest instance"}
- }}
- };
-
- return {{ menu.toIce() }};
+ SubMenu one{"one", "One", {nothing}};
+ SubMenu two{"two", "Two", {one}};
+ SubMenu three{"three", "Three", {two}};
+ SubMenu four{"four", "Four", {three}};
+
+ Menu menu{greeting,
+ failure,
+ four,
+ SubMenu{"mut", "Mutate", {Action{"copy", "Copy latest instance"}}}};
+
+ return {{menu.toIce()}};
}
- armem::actions::ExecuteActionOutputSeq ExampleMemory::executeActions(
- const armem::actions::ExecuteActionInputSeq& input)
+ armem::actions::ExecuteActionOutputSeq
+ ExampleMemory::executeActions(const armem::actions::ExecuteActionInputSeq& input)
{
using namespace armem::actions;
@@ -193,12 +192,13 @@ namespace armarx
if (instance != nullptr)
{
armem::EntityUpdate update;
- armem::MemoryID newID = memoryID.getCoreSegmentID()
- .withProviderSegmentName(memoryID.providerSegmentName)
- .withEntityName(memoryID.entityName);
+ armem::MemoryID newID =
+ memoryID.getCoreSegmentID()
+ .withProviderSegmentName(memoryID.providerSegmentName)
+ .withEntityName(memoryID.entityName);
update.entityID = newID;
update.referencedTime = armem::Time::Now();
- update.instancesData = { instance->data() };
+ update.instancesData = {instance->data()};
armem::Commit newCommit;
newCommit.add(update);
@@ -261,7 +261,8 @@ namespace armarx
// REMOTE GUI
- void ExampleMemory::createRemoteGuiTab()
+ void
+ ExampleMemory::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
@@ -274,8 +275,8 @@ namespace armarx
RemoteGui_createTab(getName(), root, &tab);
}
-
- void ExampleMemory::RemoteGui_update()
+ void
+ ExampleMemory::RemoteGui_update()
{
if (tab.rebuild.exchange(false))
{
@@ -283,4 +284,4 @@ namespace armarx
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
index b41f1262755f8ddad59eb9dc3c03309766183116..2166224ecec916d4e17df6285fb0473b59fc99fc 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
@@ -24,14 +24,13 @@
#include <ArmarXCore/core/Component.h>
-
#include <ArmarXCore/interface/observers/ObserverInterface.h>
+
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-
namespace armarx
{
/**
@@ -46,20 +45,21 @@ namespace armarx
* Detailed description of class ExampleMemory.
*/
class ExampleMemory :
- virtual public armarx::Component
- , virtual public armem::server::ReadWritePluginUser
- , virtual public LightweightRemoteGuiComponentPluginUser
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser,
+ virtual public LightweightRemoteGuiComponentPluginUser
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
// WritingInterface interface
public:
- armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current& = Ice::emptyCurrent) override;
+ armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input,
+ const Ice::Current&) override;
+ armem::data::CommitResult commit(const armem::data::Commit& commit,
+ const Ice::Current& = Ice::emptyCurrent) override;
// LightweightRemoteGuiComponentPluginUser interface
@@ -69,12 +69,13 @@ namespace armarx
// ActionsInterface interface
public:
- armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& input) override;
- armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& input) override;
+ armem::actions::GetActionsOutputSeq
+ getActions(const armem::actions::GetActionsInputSeq& input) override;
+ armem::actions::ExecuteActionOutputSeq
+ executeActions(const armem::actions::ExecuteActionInputSeq& input) override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -84,7 +85,6 @@ namespace armarx
private:
-
armem::PredictionResult predictLatest(const armem::PredictionRequest& request);
armarx::DebugObserverInterfacePrx debugObserver;
@@ -93,7 +93,8 @@ namespace armarx
{
struct CoreSegments
{
- std::vector<std::string> defaultCoreSegments = { "ExampleModality", "ExampleConcept" };
+ std::vector<std::string> defaultCoreSegments = {"ExampleModality",
+ "ExampleConcept"};
std::string _defaultSegmentsStr;
bool addOnUsage = false;
};
@@ -102,8 +103,8 @@ namespace armarx
bool enableRemoteGui = false;
};
- Properties p;
+ Properties p;
struct RemoteGuiTab : RemoteGui::Client::Tab
{
@@ -111,7 +112,7 @@ namespace armarx
RemoteGui::Client::GroupBox memoryGroup;
};
- RemoteGuiTab tab;
+ RemoteGuiTab tab;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
index bffeec398b2f5b1b913670f766a5e08d9a623563..81988955ceca8f5a887ecc01b57a2c619bd5aecd 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/test/ExampleMemoryTest.cpp
@@ -24,19 +24,19 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
#include "../ExampleMemory.h"
-#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
-#include <RobotAPI/libraries/armem/core.h>
+#include <RobotAPI/Test.h>
#include <iostream>
+#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
+#include <RobotAPI/libraries/armem/core.h>
+
using armarx::armem::example::ExampleData;
namespace armem = armarx::armem;
-
BOOST_AUTO_TEST_CASE(test_ExampleData_type)
{
armarx::aron::type::ObjectPtr type = ExampleData::ToAronType();
diff --git a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
index 12d1848e5c22ed828e73fda2328f0d4eabca0064..9b4b42914c626a8a2a042b37a762a733b866fdcf 100644
--- a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.cpp
@@ -22,60 +22,62 @@
#include "GeneralPurposeMemory.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include <SimoxUtility/algorithm/string.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-
namespace armarx
{
GeneralPurposeMemory::GeneralPurposeMemory()
{
}
- armarx::PropertyDefinitionsPtr GeneralPurposeMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ GeneralPurposeMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
return defs;
}
-
- std::string GeneralPurposeMemory::getDefaultName() const
+ std::string
+ GeneralPurposeMemory::getDefaultName() const
{
return "GeneralPurposeMemory";
}
-
- void GeneralPurposeMemory::onInitComponent()
+ void
+ GeneralPurposeMemory::onInitComponent()
{
setMemoryName(getName());
}
-
- void GeneralPurposeMemory::onConnectComponent()
+ void
+ GeneralPurposeMemory::onConnectComponent()
{
}
-
- void GeneralPurposeMemory::onDisconnectComponent()
+ void
+ GeneralPurposeMemory::onDisconnectComponent()
{
}
-
- void GeneralPurposeMemory::onExitComponent()
+ void
+ GeneralPurposeMemory::onExitComponent()
{
}
-
-
- armem::data::AddSegmentsResult GeneralPurposeMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
+ armem::data::AddSegmentsResult
+ GeneralPurposeMemory::addSegments(const armem::data::AddSegmentsInput& input,
+ const Ice::Current&)
{
// Allowing adding core segments.
- armem::data::AddSegmentsResult result = ReadWritePluginUser::addSegments(input, addCoreSegmentOnUsage);
+ armem::data::AddSegmentsResult result =
+ ReadWritePluginUser::addSegments(input, addCoreSegmentOnUsage);
return result;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.h b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.h
index 0ee828883381d8e8272b2d49a1fdfa81179e69ed..1fecd6679227ea08561372d80f485ecacd0221f0 100644
--- a/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.h
+++ b/source/RobotAPI/components/armem/server/GeneralPurposeMemory/GeneralPurposeMemory.h
@@ -26,11 +26,10 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-
namespace armarx
{
/**
@@ -45,11 +44,10 @@ namespace armarx
* Detailed description of class GeneralPurposeMemory.
*/
class GeneralPurposeMemory :
- virtual public armarx::Component
- , virtual public armem::server::ReadWritePluginUser
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser
{
public:
-
GeneralPurposeMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
@@ -57,13 +55,11 @@ namespace armarx
public:
-
- armem::data::AddSegmentsResult
- addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
+ armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input,
+ const Ice::Current&) override;
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -81,8 +77,6 @@ namespace armarx
private:
-
bool addCoreSegmentOnUsage = true;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index 7649e1f4ef73da60db7e7e14c3d95c1de004b9ff..b00236a6eb0c3bfe4b7583d261eceb83280bca13 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -2,6 +2,7 @@
#include <memory>
+
#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/Component.h>
@@ -143,6 +144,5 @@ namespace armarx::armem::server::grasp
RunningTask<GraspMemory>::pointer_type taskVisuKnownGrasps;
std::map<std::string, viz::Layer> graspVisuPerObject;
-
};
} // namespace armarx::armem::server::grasp
diff --git a/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.cpp b/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.cpp
index dffd3d2cb5ae44c0f95830ef780c0494253d8830..b25677875eba8f48bc4681d58054e154120bf014 100644
--- a/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.cpp
+++ b/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.cpp
@@ -30,7 +30,6 @@
#include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
#include <RobotAPI/libraries/armem_index/memory_ids.h>
-
namespace armarx
{
@@ -42,20 +41,20 @@ namespace armarx
setMemoryName(armem::index::memoryID.memoryName);
- defs->optional(properties.maxHistorySize, "p.maxHistorySize", "The maximum size of entity histories.")
- .setMin(1);
+ defs->optional(properties.maxHistorySize,
+ "p.maxHistorySize",
+ "The maximum size of entity histories.")
+ .setMin(1);
return defs;
}
-
std::string
IndexMemory::getDefaultName() const
{
return "IndexMemory";
}
-
void
IndexMemory::onInitComponent()
{
@@ -71,7 +70,6 @@ namespace armarx
.setMaxHistorySize(maxHistorySize);
}
-
void
IndexMemory::onConnectComponent()
{
@@ -81,19 +79,16 @@ namespace armarx
}
}
-
void
IndexMemory::onDisconnectComponent()
{
}
-
void
IndexMemory::onExitComponent()
{
}
-
void
IndexMemory::createRemoteGuiTab()
{
@@ -106,7 +101,6 @@ namespace armarx
RemoteGui_createTab(getName(), root, &tab);
}
-
void
IndexMemory::RemoteGui_update()
{
diff --git a/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.h b/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.h
index b272fcafc11e7d398b95175a26d1fcbf5fdf75d6..7a4b97f4f98a43f02d7a9e4c8862697b6a165b33 100644
--- a/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.h
+++ b/source/RobotAPI/components/armem/server/IndexMemory/IndexMemory.h
@@ -29,7 +29,6 @@
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-
namespace armarx
{
/**
@@ -72,12 +71,13 @@ namespace armarx
{
int maxHistorySize = 1024;
};
- Properties properties;
+ Properties properties;
struct RemoteGuiTab : RemoteGui::Client::Tab
{
};
+
RemoteGuiTab tab;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.cpp b/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.cpp
index 63d5bdbd644a75960a9c3824d870faf69f1eb476..dfbead355d108213cdf3a9e02a00971f464354c3 100644
--- a/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.cpp
+++ b/source/RobotAPI/components/armem/server/LaserScansMemory/LaserScansMemory.cpp
@@ -31,7 +31,6 @@
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
-
namespace armarx::armem::server::laser_scans
{
LaserScansMemory::LaserScansMemory()
@@ -56,14 +55,12 @@ namespace armarx::armem::server::laser_scans
return defs;
}
-
std::string
LaserScansMemory::getDefaultName() const
{
return "LaserScansMemory";
}
-
void
LaserScansMemory::onInitComponent()
{
@@ -73,26 +70,22 @@ namespace armarx::armem::server::laser_scans
commonVisu.init(&coreSegment, &virtualRobotReaderPlugin->get());
}
-
void
LaserScansMemory::onConnectComponent()
{
commonVisu.connect(getArvizClient(), debugObserver->getDebugObserver());
}
-
void
LaserScansMemory::onDisconnectComponent()
{
}
-
void
LaserScansMemory::onExitComponent()
{
}
-
armem::data::AddSegmentsResult
LaserScansMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
{
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
index 3958c25bf9f2152b9661ea6c6754b232b979312f..2bf83ac1f803f70cf5a3d3ac140102559c854723 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
+++ b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.cpp
@@ -22,12 +22,13 @@
#include "MotionMemory.h"
-
namespace armarx
{
- armarx::PropertyDefinitionsPtr MotionMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ MotionMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
setMemoryName("Motion");
@@ -37,41 +38,38 @@ namespace armarx
return defs;
}
-
- MotionMemory::MotionMemory() :
- mdbMotions(iceAdapter()),
- motionPrimitive(iceAdapter())
+ MotionMemory::MotionMemory() : mdbMotions(iceAdapter()), motionPrimitive(iceAdapter())
{
}
-
- std::string MotionMemory::getDefaultName() const
+ std::string
+ MotionMemory::getDefaultName() const
{
return "MotionMemory";
}
-
- void MotionMemory::onInitComponent()
+ void
+ MotionMemory::onInitComponent()
{
mdbMotions.init();
motionPrimitive.init();
}
-
- void MotionMemory::onConnectComponent()
+ void
+ MotionMemory::onConnectComponent()
{
mdbMotions.onConnect();
motionPrimitive.onConnect();
}
-
- void MotionMemory::onDisconnectComponent()
+ void
+ MotionMemory::onDisconnectComponent()
{
}
-
- void MotionMemory::onExitComponent()
+ void
+ MotionMemory::onExitComponent()
{
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
index 0a9a054f740d49291e8010aca8f2c1528928847d..09a75b7fbe7e140ed8a840454b3d7935e66387eb 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
+++ b/source/RobotAPI/components/armem/server/MotionMemory/MotionMemory.h
@@ -22,14 +22,11 @@
#pragma once
-#include <RobotAPI/libraries/armem_motions/server/MotionSegment.h>
-
-#include <RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h>
-
-#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
-
#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+#include <RobotAPI/libraries/armem_motions/server/MotionSegment.h>
+#include <RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h>
namespace armarx
{
@@ -45,11 +42,10 @@ namespace armarx
* Detailed description of class ExampleMemory.
*/
class MotionMemory :
- virtual public armarx::Component
- , virtual public armem::server::ReadWritePluginUser
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser
{
public:
-
MotionMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
@@ -57,7 +53,6 @@ namespace armarx
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -67,10 +62,8 @@ namespace armarx
private:
-
armem::server::motions::mdb::segment::MDBMotionSegment mdbMotions;
armem::server::motions::mps::segment::MPSegment motionPrimitive;
// TODO: mdt Segment
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/MotionMemory/test/MotionMemoryTest.cpp b/source/RobotAPI/components/armem/server/MotionMemory/test/MotionMemoryTest.cpp
index 5eda6d831d1768ec5f8975eddcf88ed07e3fc60e..d9e1c5b73f0a0f91fe09575c5a203aff8392bdf4 100644
--- a/source/RobotAPI/components/armem/server/MotionMemory/test/MotionMemoryTest.cpp
+++ b/source/RobotAPI/components/armem/server/MotionMemory/test/MotionMemoryTest.cpp
@@ -24,17 +24,16 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
#include "../MotionMemory.h"
-#include <RobotAPI/libraries/armem/core.h>
+#include <RobotAPI/Test.h>
#include <iostream>
-namespace armem = armarx::armem;
+#include <RobotAPI/libraries/armem/core.h>
+namespace armem = armarx::armem;
BOOST_AUTO_TEST_CASE(test_ExampleData_type)
{
-
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index deeec0ebb3405b9ac61bdbf9dbc76549f3fa4564..9886d8896c0d9111a45b02a3ac238935d02a5c7d 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -21,6 +21,7 @@
*/
#include "ObjectMemory.h"
+
#include <VirtualRobot/VirtualRobot.h>
#include "ArmarXCore/core/time/Clock.h"
@@ -32,17 +33,18 @@
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/Prediction.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
-
namespace armarx::armem::server::obj
{
- armarx::PropertyDefinitionsPtr ObjectMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ ObjectMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
+ armarx::PropertyDefinitionsPtr defs(
+ new ComponentPropertyDefinitions(getConfigIdentifier()));
const std::string prefix = "mem.";
workingMemory().name() = objects::memoryID.memoryName;
@@ -58,47 +60,52 @@ namespace armarx::armem::server::obj
defs->topic(debugObserver);
// Subscribe
- defs->topic<objpose::ObjectPoseTopic>(); // "ObjectPoseTopic", "ObjectPoseTopicName", "Name of the Object Pose Topic.");
+ defs->topic<
+ objpose::
+ ObjectPoseTopic>(); // "ObjectPoseTopic", "ObjectPoseTopicName", "Name of the Object Pose Topic.");
// Use
// defs->component(kinematicUnitObserver); // Optional dependency.
- defs->defineOptionalProperty<std::string>("cmp.KinematicUnitObserverName", "KinematicUnitObserver",
- "Name of the kinematic unit observer.");
- defs->optional(predictionTimeWindow, "prediction.TimeWindow",
+ defs->defineOptionalProperty<std::string>("cmp.KinematicUnitObserverName",
+ "KinematicUnitObserver",
+ "Name of the kinematic unit observer.");
+ defs->optional(predictionTimeWindow,
+ "prediction.TimeWindow",
"Duration of time window into the past to use for predictions"
" when requested via the PredictingMemoryInterface (in seconds).");
defs->optional(p.markerMemoryName, prefix + ".marker.Name", "Marker Memory Name");
- defs->optional(p.maxMarkerHistorySize, prefix + ".marker.maxHistorySize", "Maximum marker memory history size");
+ defs->optional(p.maxMarkerHistorySize,
+ prefix + ".marker.maxHistorySize",
+ "Maximum marker memory history size");
return defs;
}
-
ObjectMemory::ObjectMemory() :
instance::SegmentAdapter(iceAdapter()),
familiar_object_instance::SegmentAdapter(iceAdapter()),
classSegment(iceAdapter()),
- attachmentSegment(iceAdapter()), virtualRobotReaderPlugin(nullptr)
+ attachmentSegment(iceAdapter()),
+ virtualRobotReaderPlugin(nullptr)
{
addPlugin(virtualRobotReaderPlugin);
}
-
ObjectMemory::~ObjectMemory()
{
}
-
- std::string ObjectMemory::getDefaultName() const
+ std::string
+ ObjectMemory::getDefaultName() const
{
return "ObjectMemory";
}
-
- void ObjectMemory::onInitComponent()
+ void
+ ObjectMemory::onInitComponent()
{
- const auto initSegmentWithCatch = [&](const std::string & segmentName, const auto&& fn)
+ const auto initSegmentWithCatch = [&](const std::string& segmentName, const auto&& fn)
{
try
{
@@ -106,120 +113,115 @@ namespace armarx::armem::server::obj
}
catch (const LocalException& e)
{
- ARMARX_ERROR << "Failed to init `" << segmentName << "` segment. Reason: \n" << e.what();
+ ARMARX_ERROR << "Failed to init `" << segmentName << "` segment. Reason: \n"
+ << e.what();
}
catch (const std::exception& e)
{
- ARMARX_ERROR << "Failed to init `" << segmentName << "` segment. Reason: \n" << e.what();
+ ARMARX_ERROR << "Failed to init `" << segmentName << "` segment. Reason: \n"
+ << e.what();
}
catch (...)
{
- ARMARX_ERROR << "Failed to init `" << segmentName << "` segment for unknown reason.";
+ ARMARX_ERROR << "Failed to init `" << segmentName
+ << "` segment for unknown reason.";
}
};
// Class segment needs to be initialized before instance segment,
// as the instance segment may refer to and search through the class segment.
- initSegmentWithCatch("class", [&]()
- {
- classSegment.init();
- });
+ initSegmentWithCatch("class", [&]() { classSegment.init(); });
instance::SegmentAdapter::init();
familiar_object_instance::SegmentAdapter::init();
- initSegmentWithCatch("attachment", [&]()
- {
- attachmentSegment.init();
- });
+ initSegmentWithCatch("attachment", [&]() { attachmentSegment.init(); });
- initSegmentWithCatch(p.markerMemoryName, [&]()
- {
- workingMemory()
- .addCoreSegment(p.markerMemoryName, arondto::Marker::ToAronType())
- .setMaxHistorySize(p.maxMarkerHistorySize);
- });
+ initSegmentWithCatch(p.markerMemoryName,
+ [&]()
+ {
+ workingMemory()
+ .addCoreSegment(p.markerMemoryName,
+ arondto::Marker::ToAronType())
+ .setMaxHistorySize(p.maxMarkerHistorySize);
+ });
}
-
- void ObjectMemory::onConnectComponent()
+ void
+ ObjectMemory::onConnectComponent()
{
ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
-
- getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
+
+ getProxyFromProperty(
+ kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
// Create first to use the original values.
createRemoteGuiTab();
// ARMARX_CHECK(virtualRobotReaderPlugin->isAvailable());
- instance::SegmentAdapter::connect(
- &virtualRobotReaderPlugin->get(),
- kinematicUnitObserver,
- ArVizComponentPluginUser::getArvizClient(),
- debugObserver
- );
+ instance::SegmentAdapter::connect(&virtualRobotReaderPlugin->get(),
+ kinematicUnitObserver,
+ ArVizComponentPluginUser::getArvizClient(),
+ debugObserver);
familiar_object_instance::SegmentAdapter::connect(
std::experimental::make_observer(&virtualRobotReaderPlugin->get()),
ArVizComponentPluginUser::getArvizClient(),
- debugObserver
- );
+ debugObserver);
- classSegment.connect(
- ArVizComponentPluginUser::getArvizClient()
- );
+ classSegment.connect(ArVizComponentPluginUser::getArvizClient());
attachmentSegment.connect();
RemoteGui_startRunningTask();
}
-
- void ObjectMemory::onDisconnectComponent()
+ void
+ ObjectMemory::onDisconnectComponent()
{
}
-
- void ObjectMemory::onExitComponent()
+ void
+ ObjectMemory::onExitComponent()
{
}
- armem::actions::GetActionsOutputSeq ObjectMemory::getActions(
- const armem::actions::GetActionsInputSeq& inputs)
+ armem::actions::GetActionsOutputSeq
+ ObjectMemory::getActions(const armem::actions::GetActionsInputSeq& inputs)
{
using namespace armem::actions;
GetActionsOutputSeq outputs;
for (const auto& input : inputs)
{
auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
- if (armem::contains(armem::MemoryID("Object", "Class"), memoryID)
- and memoryID.hasEntityName() and not memoryID.hasGap())
+ if (armem::contains(armem::MemoryID("Object", "Class"), memoryID) and
+ memoryID.hasEntityName() and not memoryID.hasGap())
{
- Menu menu {
+ Menu menu{
Action{"vis", "Visualize object"},
};
// For strange C++ reasons, this works, but emplace_back doesn't.
- outputs.push_back({ menu.toIce() });
+ outputs.push_back({menu.toIce()});
}
}
return outputs;
}
- armem::actions::ExecuteActionOutputSeq ObjectMemory::executeActions(
- const armem::actions::ExecuteActionInputSeq& inputs)
+ armem::actions::ExecuteActionOutputSeq
+ ObjectMemory::executeActions(const armem::actions::ExecuteActionInputSeq& inputs)
{
using namespace armem::actions;
ExecuteActionOutputSeq outputs;
-
+
for (const auto& input : inputs)
{
auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
if (input.actionPath == ActionPath{"vis"})
{
- if (armem::contains(armem::MemoryID("Object", "Class"), memoryID)
- and memoryID.hasEntityName() and not memoryID.hasGap())
+ if (armem::contains(armem::MemoryID("Object", "Class"), memoryID) and
+ memoryID.hasEntityName() and not memoryID.hasGap())
{
classSegment.visualizeClass(memoryID);
outputs.emplace_back(true, "");
@@ -251,9 +253,9 @@ namespace armarx::armem::server::obj
auto boRequest = armarx::fromIce<armem::PredictionRequest>(request);
armem::PredictionResult result;
result.snapshotID = boRequest.snapshotID;
- if (armem::contains(workingMemory().id().withCoreSegmentName("Instance"), boRequest.snapshotID)
- and not boRequest.snapshotID.hasGap()
- and boRequest.snapshotID.hasTimestamp())
+ if (armem::contains(workingMemory().id().withCoreSegmentName("Instance"),
+ boRequest.snapshotID) and
+ not boRequest.snapshotID.hasGap() and boRequest.snapshotID.hasTimestamp())
{
objpose::ObjectPosePredictionRequest objPoseRequest;
toIce(objPoseRequest.timeWindow, Duration::SecondsDouble(predictionTimeWindow));
@@ -316,7 +318,8 @@ namespace armarx::armem::server::obj
return results;
}
- void ObjectMemory::createRemoteGuiTab()
+ void
+ ObjectMemory::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
@@ -325,21 +328,13 @@ namespace armarx::armem::server::obj
tab->instance.setup(*this);
tab->clazz.setup(classSegment);
- HBoxLayout segments =
- {
- tab->instance.group,
- tab->clazz.group
- };
- VBoxLayout root =
- {
- segments,
- VSpacer()
- };
+ HBoxLayout segments = {tab->instance.group, tab->clazz.group};
+ VBoxLayout root = {segments, VSpacer()};
RemoteGui_createTab(Component::getName(), root, tab.get());
}
-
- void ObjectMemory::RemoteGui_update()
+ void
+ ObjectMemory::RemoteGui_update()
{
tab->instance.update(*this);
tab->clazz.update(classSegment);
@@ -350,7 +345,8 @@ namespace armarx::armem::server::obj
}
}
- void ObjectMemory::reloadKnownObjectClasses(const Ice::Current& current)
+ void
+ ObjectMemory::reloadKnownObjectClasses(const Ice::Current& current)
{
ARMARX_INFO << "Reloading object classes ...";
classSegment.reloadObjectClassesByObjectFinder();
@@ -358,4 +354,4 @@ namespace armarx::armem::server::obj
}
-}
+} // namespace armarx::armem::server::obj
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index ec78eb3fa7493a890541e915317ccd1689d9e15d..51dfff412e3fb582b7ddfd85c88c3513d12f6fd8 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -22,28 +22,24 @@
#pragma once
-#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
-#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
-#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
-#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
-#include <RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.h>
-#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
+#include <memory>
-#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-
-#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-#include <memory>
+#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-
namespace armarx::armem::server::obj
{
@@ -61,15 +57,15 @@ namespace armarx::armem::server::obj
class ObjectMemory :
virtual public Component
- , virtual public armarx::armem::server::ObjectMemoryInterface
- , virtual public armarx::armem::server::ReadWritePluginUser
- , virtual public armarx::armem::server::obj::instance::SegmentAdapter
- , virtual public armarx::armem::server::obj::familiar_object_instance::SegmentAdapter
- , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- , virtual public armarx::ArVizComponentPluginUser
+ ,
+ virtual public armarx::armem::server::ObjectMemoryInterface,
+ virtual public armarx::armem::server::ReadWritePluginUser,
+ virtual public armarx::armem::server::obj::instance::SegmentAdapter,
+ virtual public armarx::armem::server::obj::familiar_object_instance::SegmentAdapter,
+ virtual public armarx::LightweightRemoteGuiComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser
{
public:
-
ObjectMemory();
virtual ~ObjectMemory() override;
@@ -79,7 +75,6 @@ namespace armarx::armem::server::obj
public:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -114,7 +109,6 @@ namespace armarx::armem::server::obj
void reloadKnownObjectClasses(const Ice::Current& current) override;
private:
-
DebugObserverInterfacePrx debugObserver;
KinematicUnitObserverInterfacePrx kinematicUnitObserver;
@@ -131,17 +125,21 @@ namespace armarx::armem::server::obj
instance::SegmentAdapter::RemoteGui instance;
clazz::Segment::RemoteGui clazz;
};
+
std::unique_ptr<RemoteGuiTab> tab;
- struct Properties {
+ struct Properties
+ {
int64_t maxMarkerHistorySize = -1;
std::string markerMemoryName = "Marker";
};
+
Properties p;
- armem::client::plugins::ReaderWriterPlugin<robot_state::VirtualRobotReader>* virtualRobotReaderPlugin;
+ armem::client::plugins::ReaderWriterPlugin<robot_state::VirtualRobotReader>*
+ virtualRobotReaderPlugin;
};
-}
+} // namespace armarx::armem::server::obj
#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
index 40329162efb831da65f1dcd1671dc458e5d6d1b0..93e89faa66c08e4b1a6faf5149d179607579580e 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/test/ObjectMemory.cpp
@@ -25,16 +25,15 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/ObjectMemory/ObjectMemory.h>
+#include <iostream>
+
+#include <RobotAPI/components/ObjectMemory/ObjectMemory.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <iostream>
-
using namespace armarx;
-
BOOST_AUTO_TEST_CASE(test_ObjectMemory)
{
}
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp
index 7ea316348abfa01cf08700ea362a6faa57a3afb9..bd259f8ea00a9ed336a4af72b96683d56ce174e4 100644
--- a/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.cpp
@@ -22,56 +22,58 @@
#include "ReasoningMemory.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include <SimoxUtility/algorithm/string.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/error.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
namespace armarx::armem::server
{
- armarx::PropertyDefinitionsPtr ReasoningMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ ReasoningMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
setMemoryName("Reasoning");
return defs;
}
-
- ReasoningMemory::ReasoningMemory() :
- anticipationSegment(iceAdapter())
+ ReasoningMemory::ReasoningMemory() : anticipationSegment(iceAdapter())
{
-
}
- std::string ReasoningMemory::getDefaultName() const
+ std::string
+ ReasoningMemory::getDefaultName() const
{
return "ReasoningMemory";
}
- void ReasoningMemory::onInitComponent()
+ void
+ ReasoningMemory::onInitComponent()
{
-
- {
- wm::CoreSegment& cs = workingMemory().addCoreSegment("Anticipation", armarx::reasoning::arondto::Anticipation::ToAronType());
- cs.setMaxHistorySize(20);
- }
-
+ {
+ wm::CoreSegment& cs = workingMemory().addCoreSegment(
+ "Anticipation", armarx::reasoning::arondto::Anticipation::ToAronType());
+ cs.setMaxHistorySize(20);
+ }
}
- void ReasoningMemory::onConnectComponent()
+ void
+ ReasoningMemory::onConnectComponent()
{
}
- void ReasoningMemory::onDisconnectComponent()
+ void
+ ReasoningMemory::onDisconnectComponent()
{
}
- void ReasoningMemory::onExitComponent()
+ void
+ ReasoningMemory::onExitComponent()
{
}
-}
+} // namespace armarx::armem::server
diff --git a/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h
index 2ad670842bb15e1159326a977d23497af714aaa8..ce42a8dc55c4be8141b114674fbb22defb9bb22d 100644
--- a/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h
+++ b/source/RobotAPI/components/armem/server/ReasoningMemory/ReasoningMemory.h
@@ -30,7 +30,7 @@
namespace armarx::armem::server
{
class ReasoningMemory :
- virtual public armarx::Component,
+ virtual public armarx::Component,
virtual public armem::server::ReadWritePluginUser
{
public:
@@ -40,7 +40,6 @@ namespace armarx::armem::server
std::string getDefaultName() const override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -52,4 +51,4 @@ namespace armarx::armem::server
private:
reasoning::AnticipationSegment anticipationSegment;
};
-}
+} // namespace armarx::armem::server
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 09109c2bb721d90890763d7803526f7acfb2bb81..2249bdf242a197d1bff3be840d846f58a2487568 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -188,7 +188,6 @@ namespace armarx::armem::server::robot_state
startRobotUnitStream();
}
-
}
void
diff --git a/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.cpp b/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.cpp
index d23b8cd13ea3fcb807d969d860805d3552cdaa6e..0d7730f9e339cadd3eaa3a10808b1c873c7e0f2c 100644
--- a/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.cpp
@@ -22,18 +22,19 @@
#include "SubjectMemory.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include <SimoxUtility/algorithm/string.h>
-#include <RobotAPI/libraries/armem/core/error.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/armem/core/error.h>
namespace armarx
{
- armarx::PropertyDefinitionsPtr SubjectMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ SubjectMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
const std::string prefix = "mem.";
defs->optional(memoryName, prefix + "MemoryName", "Name of this memory (server).");
@@ -42,32 +43,36 @@ namespace armarx
}
SubjectMemory::SubjectMemory() :
- mdbMotions(armem::server::ComponentPluginUser::iceMemory, armem::server::ComponentPluginUser::workingMemoryMutex)
+ mdbMotions(armem::server::ComponentPluginUser::iceMemory,
+ armem::server::ComponentPluginUser::workingMemoryMutex)
{
-
}
-
- std::string SubjectMemory::getDefaultName() const
+ std::string
+ SubjectMemory::getDefaultName() const
{
return "MotionMemory";
}
- void SubjectMemory::onInitComponent()
+ void
+ SubjectMemory::onInitComponent()
{
workingMemory.name() = memoryName;
longtermMemory.name() = memoryName;
}
- void SubjectMemory::onConnectComponent()
+ void
+ SubjectMemory::onConnectComponent()
{
}
- void SubjectMemory::onDisconnectComponent()
+ void
+ SubjectMemory::onDisconnectComponent()
{
}
- void SubjectMemory::onExitComponent()
+ void
+ SubjectMemory::onExitComponent()
{
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.h b/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.h
index 8018df78fc2b92de1353089521c8649aded3b377..70dedf50b485004a70c927fb2bc0037c1aff2e04 100644
--- a/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.h
+++ b/source/RobotAPI/components/armem/server/SubjectMemory/SubjectMemory.h
@@ -24,8 +24,8 @@
#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
#include <RobotAPI/libraries/armem_subjects/server/MotionDatabase/Segment.h>
namespace armarx
@@ -42,18 +42,16 @@ namespace armarx
* Detailed description of class ExampleMemory.
*/
class SubjectMemory :
- virtual public armarx::Component
- , virtual public armem::server::ReadWritePluginUser
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser
{
public:
-
SubjectMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -66,6 +64,5 @@ namespace armarx
std::string memoryName = "Subject";
armem::server::subjects::mdb::Segment mdbSubjects;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/SubjectMemory/test/SubjectMemoryTest.cpp b/source/RobotAPI/components/armem/server/SubjectMemory/test/SubjectMemoryTest.cpp
index 5eda6d831d1768ec5f8975eddcf88ed07e3fc60e..b44292dbb47f181b835ae9d918df24e09cf21d0f 100644
--- a/source/RobotAPI/components/armem/server/SubjectMemory/test/SubjectMemoryTest.cpp
+++ b/source/RobotAPI/components/armem/server/SubjectMemory/test/SubjectMemoryTest.cpp
@@ -25,16 +25,15 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../MotionMemory.h"
+
+#include <iostream>
#include <RobotAPI/libraries/armem/core.h>
-#include <iostream>
+#include "../MotionMemory.h"
namespace armem = armarx::armem;
-
BOOST_AUTO_TEST_CASE(test_ExampleData_type)
{
-
}
diff --git a/source/RobotAPI/components/armem/server/SystemStateMemory/SystemStateMemory.cpp b/source/RobotAPI/components/armem/server/SystemStateMemory/SystemStateMemory.cpp
index 76e0a0263a83c0133cae79beab4036ca60b36b5f..9d1e469d7abcf3ee18539f6ad8102558d5e0a2b1 100644
--- a/source/RobotAPI/components/armem/server/SystemStateMemory/SystemStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/SystemStateMemory/SystemStateMemory.cpp
@@ -24,9 +24,11 @@
namespace armarx
{
- armarx::PropertyDefinitionsPtr SystemStateMemory::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ SystemStateMemory::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr defs =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
setMemoryName("SystemState");
@@ -37,35 +39,35 @@ namespace armarx
}
SystemStateMemory::SystemStateMemory() :
- ramUsageCoreSegment(iceAdapter()),
- cpuUsageCoreSegment(iceAdapter())
+ ramUsageCoreSegment(iceAdapter()), cpuUsageCoreSegment(iceAdapter())
{
}
- std::string SystemStateMemory::getDefaultName() const
+ std::string
+ SystemStateMemory::getDefaultName() const
{
return "SystemStateMemory";
}
-
- void SystemStateMemory::onInitComponent()
+ void
+ SystemStateMemory::onInitComponent()
{
ramUsageCoreSegment.init();
cpuUsageCoreSegment.init();
}
-
- void SystemStateMemory::onConnectComponent()
+ void
+ SystemStateMemory::onConnectComponent()
{
}
-
- void SystemStateMemory::onDisconnectComponent()
+ void
+ SystemStateMemory::onDisconnectComponent()
{
}
-
- void SystemStateMemory::onExitComponent()
+ void
+ SystemStateMemory::onExitComponent()
{
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/armem/server/SystemStateMemory/SystemStateMemory.h b/source/RobotAPI/components/armem/server/SystemStateMemory/SystemStateMemory.h
index 5c42703356cb7f8b741ff27625c91648bdf46b1a..b599db2dcd50454419c60c75b15ebd056efc0e2c 100644
--- a/source/RobotAPI/components/armem/server/SystemStateMemory/SystemStateMemory.h
+++ b/source/RobotAPI/components/armem/server/SystemStateMemory/SystemStateMemory.h
@@ -24,26 +24,24 @@
#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
+#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
#include <RobotAPI/libraries/armem_system_state/server/CPUSegment.h>
#include <RobotAPI/libraries/armem_system_state/server/RAMSegment.h>
namespace armarx
{
class SystemStateMemory :
- virtual public armarx::Component
- , virtual public armem::server::ReadWritePluginUser
+ virtual public armarx::Component,
+ virtual public armem::server::ReadWritePluginUser
{
public:
-
SystemStateMemory();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -53,9 +51,9 @@ namespace armarx
private:
-
- armem::server::systemstate::segment::LightweightRamMonitorProviderSegment ramUsageCoreSegment;
- armem::server::systemstate::segment::LightweightCpuMonitorProviderSegment cpuUsageCoreSegment;
-
+ armem::server::systemstate::segment::LightweightRamMonitorProviderSegment
+ ramUsageCoreSegment;
+ armem::server::systemstate::segment::LightweightCpuMonitorProviderSegment
+ cpuUsageCoreSegment;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/ik_demo/ComponentInterface.ice b/source/RobotAPI/components/ik_demo/ComponentInterface.ice
index edde6d507af83320f080fb20c61856a4036747b3..6114541b7a984664878b5636e85b588308ffa755 100644
--- a/source/RobotAPI/components/ik_demo/ComponentInterface.ice
+++ b/source/RobotAPI/components/ik_demo/ComponentInterface.ice
@@ -24,12 +24,19 @@
#pragma once
-module armar6 { module skills { module components { module armar6_ik_demo
+module armar6
{
-
- interface ComponentInterface
+ module skills
{
- // Define your interface here.
- };
+ module components
+ {
+ module armar6_ik_demo
+ {
-};};};};
+ interface ComponentInterface{
+ // Define your interface here.
+ };
+ };
+ };
+ };
+};
diff --git a/source/RobotAPI/components/ik_demo/IkDemo.cpp b/source/RobotAPI/components/ik_demo/IkDemo.cpp
index 4ff605566ffec008e8cbdd811554d070cbc3f36d..4a1643d54412593b26434a5bdafbafb1f1a1176b 100644
--- a/source/RobotAPI/components/ik_demo/IkDemo.cpp
+++ b/source/RobotAPI/components/ik_demo/IkDemo.cpp
@@ -1,15 +1,14 @@
#include "IkDemo.h"
#include <SimoxUtility/algorithm/string.h>
-#include <SimoxUtility/meta/EnumNames.hpp>
#include <SimoxUtility/math/pose/invert.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/XML/RobotIO.h>
+#include <SimoxUtility/meta/EnumNames.hpp>
#include <VirtualRobot/IK/CompositeDiffIK/CompositeDiffIK.h>
#include <VirtualRobot/IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h>
#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
@@ -22,7 +21,6 @@
namespace viz = armarx::viz;
-
namespace armar6::skills::components::armar6_ik_demo
{
@@ -31,10 +29,9 @@ namespace armar6::skills::components::armar6_ik_demo
SimpleDiffIk,
CompositeDiffIk,
};
- const simox::meta::EnumNames<IkMethod> IkMethodNames =
- {
- { IkMethod::SimpleDiffIk, "Simple Diff IK" },
- { IkMethod::CompositeDiffIk, "Composite Diff IK" },
+ const simox::meta::EnumNames<IkMethod> IkMethodNames = {
+ {IkMethod::SimpleDiffIk, "Simple Diff IK"},
+ {IkMethod::CompositeDiffIk, "Composite Diff IK"},
};
struct Manipulator
@@ -43,11 +40,13 @@ namespace armar6::skills::components::armar6_ik_demo
{
gizmo.box.color(simox::Color::cyan(255, 64));
}
+
virtual ~Manipulator()
{
}
- virtual bool handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage)
+ virtual bool
+ handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage)
{
bool updated = false;
if (interaction.layer() == gizmo.layer.data_.name)
@@ -63,7 +62,6 @@ namespace armar6::skills::components::armar6_ik_demo
viz::PoseGizmo gizmo;
};
-
struct TcpManipulator : public Manipulator
{
TcpManipulator()
@@ -73,19 +71,21 @@ namespace armar6::skills::components::armar6_ik_demo
{
options.push_back("Use " + IkMethodNames.to_name(method));
}
- gizmo.box
- .size({75, 100, 200})
- .enable(viz::interaction().selection().transform().hideDuringTransform()
- .contextMenu(options));
+ gizmo.box.size({75, 100, 200})
+ .enable(
+ viz::interaction().selection().transform().hideDuringTransform().contextMenu(
+ options));
};
- void visualize(viz::Client& arviz) override
+ void
+ visualize(viz::Client& arviz) override
{
gizmo.setLayer(arviz.layer(tcp->getName()));
gizmo.update();
}
- bool handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage) override
+ bool
+ handle(viz::InteractionFeedback const& interaction, viz::StagedCommit* stage) override
{
bool updated = Manipulator::handle(interaction, stage);
@@ -93,96 +93,104 @@ namespace armar6::skills::components::armar6_ik_demo
{
switch (interaction.type())
{
- case viz::InteractionFeedbackType::ContextMenuChosen:
- {
- int i = 0;
- for (IkMethod method : IkMethodNames.values())
+ case viz::InteractionFeedbackType::ContextMenuChosen:
{
- if (i == interaction.chosenContextMenuEntry())
+ int i = 0;
+ for (IkMethod method : IkMethodNames.values())
{
- this->method = method;
- updated |= true;
- ARMARX_IMPORTANT << "[" << tcp->getName() << "] Using " << IkMethodNames.to_name(method);
- break;
+ if (i == interaction.chosenContextMenuEntry())
+ {
+ this->method = method;
+ updated |= true;
+ ARMARX_IMPORTANT << "[" << tcp->getName() << "] Using "
+ << IkMethodNames.to_name(method);
+ break;
+ }
+ ++i;
}
- ++i;
}
- } break;
- default:
break;
+ default:
+ break;
}
}
return updated;
}
- void runIk(IkDemo::Robot& robot) override
+ void
+ runIk(IkDemo::Robot& robot) override
{
const Eigen::Matrix4f tcpPoseInRobotFrame =
- simox::math::inverted_pose(robot.robot->getGlobalPose()) * gizmo.getCurrent();
+ simox::math::inverted_pose(robot.robot->getGlobalPose()) * gizmo.getCurrent();
switch (method)
{
- case IkMethod::SimpleDiffIk:
- {
- armarx::SimpleDiffIK ik;
- armarx::SimpleDiffIK::Result result = ik.CalculateDiffIK(tcpPoseInRobotFrame, nodeSet, tcp);
-
- if (result.reached)
+ case IkMethod::SimpleDiffIk:
{
- gizmo.box.color(simox::Color::kit_green(64));
+ armarx::SimpleDiffIK ik;
+ armarx::SimpleDiffIK::Result result =
+ ik.CalculateDiffIK(tcpPoseInRobotFrame, nodeSet, tcp);
- std::map<std::string, float> jointValues;
- size_t i = 0;
- for (const auto& rn : nodeSet->getAllRobotNodes())
+ if (result.reached)
{
- jointValues[rn->getName()] = result.jointValues(i);
- ++i;
- }
+ gizmo.box.color(simox::Color::kit_green(64));
- robot.robot->setJointValues(jointValues);
- }
- else
- {
- gizmo.box.color(simox::Color::red(255, 64));
+ std::map<std::string, float> jointValues;
+ size_t i = 0;
+ for (const auto& rn : nodeSet->getAllRobotNodes())
+ {
+ jointValues[rn->getName()] = result.jointValues(i);
+ ++i;
+ }
+
+ robot.robot->setJointValues(jointValues);
+ }
+ else
+ {
+ gizmo.box.color(simox::Color::red(255, 64));
+ }
}
- } break;
+ break;
- case IkMethod::CompositeDiffIk:
- {
- // Code taken from: simox/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
+ case IkMethod::CompositeDiffIk:
+ {
+ // Code taken from: simox/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
- const float jointLimitAvoidance = 0;
- const float kGainManipulabilityAsNullspace = 0.01;
- const float kSoechtingAsNullspace = 0.0;
- const int steps = 100;
+ const float jointLimitAvoidance = 0;
+ const float kGainManipulabilityAsNullspace = 0.01;
+ const float kSoechtingAsNullspace = 0.0;
+ const int steps = 100;
- VirtualRobot::CompositeDiffIK ik(nodeSet);
- Eigen::Matrix4f pose = tcpPoseInRobotFrame;
+ VirtualRobot::CompositeDiffIK ik(nodeSet);
+ Eigen::Matrix4f pose = tcpPoseInRobotFrame;
- const bool ori = true;
- VirtualRobot::CompositeDiffIK::TargetPtr target1 = ik.addTarget(
- tcp, pose, ori ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position);
+ const bool ori = true;
+ VirtualRobot::CompositeDiffIK::TargetPtr target1 = ik.addTarget(
+ tcp,
+ pose,
+ ori ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position);
- if (jointLimitAvoidance > 0)
- {
- VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
- new VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidance(nodeSet));
- nsjla->kP = jointLimitAvoidance;
- for (auto node : nodeSet->getAllRobotNodes())
+ if (jointLimitAvoidance > 0)
{
- if (node->isLimitless())
+ VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
+ new VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidance(
+ nodeSet));
+ nsjla->kP = jointLimitAvoidance;
+ for (auto node : nodeSet->getAllRobotNodes())
{
- nsjla->setWeight(node->getName(), 0);
+ if (node->isLimitless())
+ {
+ nsjla->setWeight(node->getName(), 0);
+ }
}
+ ik.addNullspaceGradient(nsjla);
}
- ik.addNullspaceGradient(nsjla);
- }
- VirtualRobot::NullspaceManipulabilityPtr nsman = nullptr;
- if (kGainManipulabilityAsNullspace > 0)
- {
+ VirtualRobot::NullspaceManipulabilityPtr nsman = nullptr;
+ if (kGainManipulabilityAsNullspace > 0)
+ {
#if 0
std::cout << "Adding manipulability as nullspace target" << std::endl;
auto manipTracking = getManipulabilityTracking(nodeSet, nullptr);
@@ -201,85 +209,95 @@ namespace armar6::skills::components::armar6_ik_demo
nsman->kP = kGainManipulabilityAsNullspace;
ik.addNullspaceGradient(nsman);
#endif
- }
-
- if (kSoechtingAsNullspace > 0)
- {
- if (robot.robot->getName() == "Armar6" and nodeSet->getName() == "RightArm")
- {
- std::cout << "Adding soechting nullspace" << std::endl;
- VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
- armjoints.clavicula = robot.robot->getRobotNode("ArmR1_Cla1");
- armjoints.shoulder1 = robot.robot->getRobotNode("ArmR2_Sho1");
- armjoints.shoulder2 = robot.robot->getRobotNode("ArmR3_Sho2");
- armjoints.shoulder3 = robot.robot->getRobotNode("ArmR4_Sho3");
- armjoints.elbow = robot.robot->getRobotNode("ArmR5_Elb1");
-
- auto gradient = std::make_shared<VirtualRobot::SoechtingNullspaceGradient>(
- target1, "ArmR2_Sho1", VirtualRobot::Soechting::ArmType::Right, armjoints);
-
- gradient->kP = kSoechtingAsNullspace;
- ik.addNullspaceGradient(gradient);
- }
- else if (robot.robot->getName() == "Armar6" and nodeSet->getName() == "LeftArm")
- {
- std::cout << "Adding soechting nullspace" << std::endl;
- VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
- armjoints.clavicula = robot.robot->getRobotNode("ArmL1_Cla1");
- armjoints.shoulder1 = robot.robot->getRobotNode("ArmL2_Sho1");
- armjoints.shoulder2 = robot.robot->getRobotNode("ArmL3_Sho2");
- armjoints.shoulder3 = robot.robot->getRobotNode("ArmL4_Sho3");
- armjoints.elbow = robot.robot->getRobotNode("ArmL5_Elb1");
-
- auto gradient = std::make_shared<VirtualRobot::SoechtingNullspaceGradient>(
- target1, "ArmL2_Sho1", VirtualRobot::Soechting::ArmType::Left, armjoints);
- gradient->kP = kSoechtingAsNullspace;
- ik.addNullspaceGradient(gradient);
- }
- else
- {
- ARMARX_INFO << "Soechting currently supports only Armar6 and RightArm/LeftArm robot node set "
- "for first robot node set for demonstration purposes.";
}
- }
- {
- VirtualRobot::CompositeDiffIK::Parameters cp;
- cp.resetRnsValues = false;
- cp.returnIKSteps = true;
- cp.steps = 1;
- VirtualRobot::CompositeDiffIK::SolveState state;
- ik.solve(cp, state);
-
- int i = 0;
- while (i < steps or (steps < 0 and not ik.getLastResult().reached and i < 1000))
+ if (kSoechtingAsNullspace > 0)
{
- ik.step(cp, state, i);
- i++;
+ if (robot.robot->getName() == "Armar6" and nodeSet->getName() == "RightArm")
+ {
+ std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+ armjoints.clavicula = robot.robot->getRobotNode("ArmR1_Cla1");
+ armjoints.shoulder1 = robot.robot->getRobotNode("ArmR2_Sho1");
+ armjoints.shoulder2 = robot.robot->getRobotNode("ArmR3_Sho2");
+ armjoints.shoulder3 = robot.robot->getRobotNode("ArmR4_Sho3");
+ armjoints.elbow = robot.robot->getRobotNode("ArmR5_Elb1");
+
+ auto gradient =
+ std::make_shared<VirtualRobot::SoechtingNullspaceGradient>(
+ target1,
+ "ArmR2_Sho1",
+ VirtualRobot::Soechting::ArmType::Right,
+ armjoints);
+
+ gradient->kP = kSoechtingAsNullspace;
+ ik.addNullspaceGradient(gradient);
+ }
+ else if (robot.robot->getName() == "Armar6" and
+ nodeSet->getName() == "LeftArm")
+ {
+ std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+ armjoints.clavicula = robot.robot->getRobotNode("ArmL1_Cla1");
+ armjoints.shoulder1 = robot.robot->getRobotNode("ArmL2_Sho1");
+ armjoints.shoulder2 = robot.robot->getRobotNode("ArmL3_Sho2");
+ armjoints.shoulder3 = robot.robot->getRobotNode("ArmL4_Sho3");
+ armjoints.elbow = robot.robot->getRobotNode("ArmL5_Elb1");
+
+ auto gradient =
+ std::make_shared<VirtualRobot::SoechtingNullspaceGradient>(
+ target1,
+ "ArmL2_Sho1",
+ VirtualRobot::Soechting::ArmType::Left,
+ armjoints);
+ gradient->kP = kSoechtingAsNullspace;
+ ik.addNullspaceGradient(gradient);
+ }
+ else
+ {
+ ARMARX_INFO << "Soechting currently supports only Armar6 and "
+ "RightArm/LeftArm robot node set "
+ "for first robot node set for demonstration purposes.";
+ }
}
- if (ik.getLastResult().reached)
{
- gizmo.box.color(simox::Color::kit_green(64));
- robot.robot->setJointValues(ik.getRobotNodeSet()->getJointValueMap());
- }
- else
- {
- gizmo.box.color(simox::Color::red(255, 64));
+ VirtualRobot::CompositeDiffIK::Parameters cp;
+ cp.resetRnsValues = false;
+ cp.returnIKSteps = true;
+ cp.steps = 1;
+ VirtualRobot::CompositeDiffIK::SolveState state;
+ ik.solve(cp, state);
+
+ int i = 0;
+ while (i < steps or
+ (steps < 0 and not ik.getLastResult().reached and i < 1000))
+ {
+ ik.step(cp, state, i);
+ i++;
+ }
+
+ if (ik.getLastResult().reached)
+ {
+ gizmo.box.color(simox::Color::kit_green(64));
+ robot.robot->setJointValues(ik.getRobotNodeSet()->getJointValueMap());
+ }
+ else
+ {
+ gizmo.box.color(simox::Color::red(255, 64));
+ }
}
}
- } break;
+ break;
}
}
-
VirtualRobot::RobotNodeSetPtr nodeSet;
VirtualRobot::RobotNodePtr tcp;
IkMethod method = IkMethod::SimpleDiffIk;
};
-
struct PlatformManipulator : public Manipulator
{
PlatformManipulator()
@@ -287,12 +305,15 @@ namespace armar6::skills::components::armar6_ik_demo
gizmo.box.size({1000, 1000, 100});
}
- void visualize(viz::Client& arviz) override
+ void
+ visualize(viz::Client& arviz) override
{
gizmo.setLayer(arviz.layer(root->getName()));
gizmo.update();
}
- void runIk(IkDemo::Robot& robot) override
+
+ void
+ runIk(IkDemo::Robot& robot) override
{
robot.robot->setGlobalPose(gizmo.getCurrent());
}
@@ -300,8 +321,6 @@ namespace armar6::skills::components::armar6_ik_demo
VirtualRobot::RobotNodePtr root;
};
-
-
IkDemo::IkDemo()
{
}
@@ -310,40 +329,41 @@ namespace armar6::skills::components::armar6_ik_demo
{
}
-
IkDemo::Params::Params() :
robotFile("armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml"),
robotNodeSetNamesStr(simox::alg::join({"LeftArm", "RightArm"}, "; "))
{
}
-
- std::vector<std::string> IkDemo::Params::robotNodeSetNames() const
+ std::vector<std::string>
+ IkDemo::Params::robotNodeSetNames() const
{
bool trim = true;
return simox::alg::split(robotNodeSetNamesStr, ";", trim);
}
-
- void IkDemo::defineProperties(armarx::PropertyDefinitionContainer& defs)
+ void
+ IkDemo::defineProperties(armarx::PropertyDefinitionContainer& defs)
{
- defs.optional(params.robotFile, "p.robotFile",
+ defs.optional(params.robotFile,
+ "p.robotFile",
"The ArmarX data path to the robot XML file."
"\n For ARMAR-III: 'RobotAPI/robots/Armar3/ArmarIII.xml'"
- "\n For ARMAR-6: 'armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml'"
- );
- defs.optional(params.robotNodeSetNamesStr, "p.robotNodeSetNames",
+ "\n For ARMAR-6: 'armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml'");
+ defs.optional(params.robotNodeSetNamesStr,
+ "p.robotNodeSetNames",
"Names of robot node sets for TCPs.");
}
-
- void IkDemo::start()
+ void
+ IkDemo::start()
{
this->stage.reset();
{
params.robotFile = armarx::ArmarXDataPath::resolvePath(params.robotFile);
- robot.robot = VirtualRobot::RobotIO::loadRobot(params.robotFile, VirtualRobot::RobotIO::eStructure);
+ robot.robot = VirtualRobot::RobotIO::loadRobot(params.robotFile,
+ VirtualRobot::RobotIO::eStructure);
robot.layer = remote.arviz->layer("Robot");
@@ -365,7 +385,8 @@ namespace armar6::skills::components::armar6_ik_demo
for (const std::string& rnsName : params.robotNodeSetNames())
{
- ARMARX_CHECK(robot.robot->hasRobotNodeSet(rnsName)) << VAROUT(rnsName) << " must exist.";
+ ARMARX_CHECK(robot.robot->hasRobotNodeSet(rnsName))
+ << VAROUT(rnsName) << " must exist.";
const auto& rns = robot.robot->getRobotNodeSet(rnsName);
const auto tcp = rns->getTCP();
@@ -392,8 +413,8 @@ namespace armar6::skills::components::armar6_ik_demo
ARMARX_VERBOSE << "Initial commit at revision: " << result.revision();
}
-
- void IkDemo::update()
+ void
+ IkDemo::update()
{
viz::CommitResult result = remote.arviz->commit(stage);
@@ -421,8 +442,8 @@ namespace armar6::skills::components::armar6_ik_demo
}
}
-
- void IkDemo::runIk()
+ void
+ IkDemo::runIk()
{
for (auto& manip : manipulators)
{
@@ -434,4 +455,4 @@ namespace armar6::skills::components::armar6_ik_demo
stage.add(robot.layer);
}
-}
+} // namespace armar6::skills::components::armar6_ik_demo
diff --git a/source/RobotAPI/components/ik_demo/IkDemo.h b/source/RobotAPI/components/ik_demo/IkDemo.h
index b48b040efe2ee6f10272edea446701c7695c14d0..9d0de402fda1216f7f0a95eb041a1e5f12307289 100644
--- a/source/RobotAPI/components/ik_demo/IkDemo.h
+++ b/source/RobotAPI/components/ik_demo/IkDemo.h
@@ -6,17 +6,14 @@
#include <RobotAPI/components/ArViz/Client/Client.h>
-
namespace armar6::skills::components::armar6_ik_demo
{
struct Manipulator;
-
class IkDemo
{
public:
-
struct Params
{
Params();
@@ -25,21 +22,22 @@ namespace armar6::skills::components::armar6_ik_demo
std::string robotNodeSetNamesStr;
std::vector<std::string> robotNodeSetNames() const;
};
+
struct Remote
{
std::optional<armarx::viz::Client> arviz;
};
+
struct Robot
{
VirtualRobot::RobotPtr robot;
armarx::viz::Layer layer;
- armarx::viz::Robot visu { "robot" };
+ armarx::viz::Robot visu{"robot"};
};
public:
-
IkDemo();
~IkDemo();
@@ -54,7 +52,6 @@ namespace armar6::skills::components::armar6_ik_demo
public:
-
Remote remote;
Params params;
Robot robot;
@@ -62,7 +59,6 @@ namespace armar6::skills::components::armar6_ik_demo
armarx::viz::StagedCommit stage;
std::vector<std::unique_ptr<Manipulator>> manipulators;
-
};
-}
+} // namespace armar6::skills::components::armar6_ik_demo
diff --git a/source/RobotAPI/components/ik_demo/PoseGizmo.cpp b/source/RobotAPI/components/ik_demo/PoseGizmo.cpp
index 04153d3f8e8c2b412b948f3037c002b3d78e52ab..3c24809a616b5139f349925fab3784f1c47d10fa 100644
--- a/source/RobotAPI/components/ik_demo/PoseGizmo.cpp
+++ b/source/RobotAPI/components/ik_demo/PoseGizmo.cpp
@@ -3,7 +3,6 @@
namespace viz = armarx::viz;
-
namespace armarx::viz
{
@@ -12,21 +11,24 @@ namespace armarx::viz
box.enable(viz::interaction().selection().transform().hideDuringTransform());
}
- void PoseGizmo::setLayer(const Layer& layer_)
+ void
+ PoseGizmo::setLayer(const Layer& layer_)
{
this->layer = layer_;
this->layer.add(box);
this->layer.add(pose);
}
- void PoseGizmo::update()
+ void
+ PoseGizmo::update()
{
const Eigen::Matrix4f current = getCurrent();
box.pose(current);
pose.pose(current);
}
- void PoseGizmo::updateDuringTransform()
+ void
+ PoseGizmo::updateDuringTransform()
{
const Eigen::Matrix4f current = getCurrent();
box.pose(initial);
@@ -34,9 +36,7 @@ namespace armarx::viz
}
bool
- PoseGizmo::handleInteraction(
- const InteractionFeedback& interaction,
- StagedCommit* stage)
+ PoseGizmo::handleInteraction(const InteractionFeedback& interaction, StagedCommit* stage)
{
if (interaction.element() != box.data_->id)
{
@@ -45,58 +45,59 @@ namespace armarx::viz
switch (interaction.type())
{
- case viz::InteractionFeedbackType::Select:
- {
- // Nothing to do.
- } break;
-
- case viz::InteractionFeedbackType::Transform:
- {
- // Update state of tcp object
- transform = interaction.transformation();
-
- if (interaction.isTransformDuring())
+ case viz::InteractionFeedbackType::Select:
{
- updateDuringTransform();
+ // Nothing to do.
}
+ break;
- stage->add(layer);
- return true;
- } break;
+ case viz::InteractionFeedbackType::Transform:
+ {
+ // Update state of tcp object
+ transform = interaction.transformation();
- case viz::InteractionFeedbackType::Deselect:
- {
- // If an object is deselected, we apply the transformation
+ if (interaction.isTransformDuring())
+ {
+ updateDuringTransform();
+ }
- initial = getCurrent();
- transform.setIdentity();
+ stage->add(layer);
+ return true;
+ }
+ break;
- update();
- stage->add(layer);
+ case viz::InteractionFeedbackType::Deselect:
+ {
+ // If an object is deselected, we apply the transformation
- return true;
- } break;
+ initial = getCurrent();
+ transform.setIdentity();
- case viz::InteractionFeedbackType::ContextMenuChosen:
- {
- }
+ update();
+ stage->add(layer);
- default:
- {
- // Ignore other interaction types
- } break;
+ return true;
+ }
+ break;
+
+ case viz::InteractionFeedbackType::ContextMenuChosen:
+ {
+ }
+ default:
+ {
+ // Ignore other interaction types
+ }
+ break;
}
return false;
}
-
- Eigen::Matrix4f PoseGizmo::getCurrent() const
+ Eigen::Matrix4f
+ PoseGizmo::getCurrent() const
{
return transform * initial;
}
-}
-
-
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ik_demo/PoseGizmo.h b/source/RobotAPI/components/ik_demo/PoseGizmo.h
index 482eea5aa420ee1d302f7c2d5f6f94bfd944b1dc..44b0e5923bb23336f49cc6b944f9e976a5a2c304 100644
--- a/source/RobotAPI/components/ik_demo/PoseGizmo.h
+++ b/source/RobotAPI/components/ik_demo/PoseGizmo.h
@@ -1,9 +1,8 @@
#pragma once
+#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/ArViz/Client/Layer.h>
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
namespace armarx::viz
{
@@ -11,7 +10,6 @@ namespace armarx::viz
class PoseGizmo
{
public:
-
PoseGizmo();
void setLayer(const viz::Layer& layer);
@@ -26,15 +24,13 @@ namespace armarx::viz
public:
-
Eigen::Matrix4f initial = Eigen::Matrix4f::Identity();
Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
viz::Layer layer;
- viz::Box box { "box" };
- viz::Pose pose { "pose" };
-
+ viz::Box box{"box"};
+ viz::Pose pose{"pose"};
};
-}
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ik_demo/ik_demo.cpp b/source/RobotAPI/components/ik_demo/ik_demo.cpp
index 73b838422a9f579a290dcce2814e1b5c236173b6..54e06ed890fc5ecbf2ccd7ea28ce1ff4c028d245 100644
--- a/source/RobotAPI/components/ik_demo/ik_demo.cpp
+++ b/source/RobotAPI/components/ik_demo/ik_demo.cpp
@@ -37,23 +37,20 @@
namespace viz = armarx::viz;
-
namespace armar6::skills::components::armar6_ik_demo
{
- const std::string
- ik_demo::defaultName = "IkDemo";
-
+ const std::string ik_demo::defaultName = "IkDemo";
ik_demo::ik_demo() : impl(new IkDemo)
{
}
-
armarx::PropertyDefinitionsPtr
ik_demo::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr def =
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
// Publish to a topic (passing the TopicListenerPrx).
// def->topic(myTopicListener);
@@ -76,7 +73,6 @@ namespace armar6::skills::components::armar6_ik_demo
return def;
}
-
void
ik_demo::onInitComponent()
{
@@ -87,7 +83,6 @@ namespace armar6::skills::components::armar6_ik_demo
// setDebugObserverBatchModeEnabled(true);
}
-
void
ik_demo::onConnectComponent()
{
@@ -116,7 +111,6 @@ namespace armar6::skills::components::armar6_ik_demo
*/
}
-
void
ik_demo::onDisconnectComponent()
{
@@ -125,14 +119,13 @@ namespace armar6::skills::components::armar6_ik_demo
task = nullptr;
}
-
void
ik_demo::onExitComponent()
{
}
-
- void ik_demo::run()
+ void
+ ik_demo::run()
{
impl->start();
@@ -145,21 +138,18 @@ namespace armar6::skills::components::armar6_ik_demo
}
}
-
std::string
ik_demo::getDefaultName() const
{
return ik_demo::defaultName;
}
-
std::string
ik_demo::GetDefaultName()
{
return ik_demo::defaultName;
}
-
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void
Component::createRemoteGuiTab()
@@ -224,4 +214,4 @@ namespace armar6::skills::components::armar6_ik_demo
// ARMARX_REGISTER_COMPONENT_EXECUTABLE(ik_demo, ik_demo::GetDefaultName());
-} // namespace armar6::skills::components::armar6_ik_demo
+} // namespace armar6::skills::components::armar6_ik_demo
diff --git a/source/RobotAPI/components/ik_demo/ik_demo.h b/source/RobotAPI/components/ik_demo/ik_demo.h
index 45eeb06a386e8dcf67fd2c11e337b24e858d68a1..b19ceb37c1a59754ee2ce607eaa8012c6721b22e 100644
--- a/source/RobotAPI/components/ik_demo/ik_demo.h
+++ b/source/RobotAPI/components/ik_demo/ik_demo.h
@@ -42,16 +42,15 @@ namespace armar6::skills::components::armar6_ik_demo
class IkDemo;
-
class ik_demo :
virtual public armarx::Component
// , virtual public armar6::skills::components::armar6_ik_demo::ComponentInterface
// , virtual public armarx::DebugObserverComponentPluginUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- , virtual public armarx::ArVizComponentPluginUser
+ ,
+ virtual public armarx::ArVizComponentPluginUser
{
public:
-
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
@@ -62,7 +61,6 @@ namespace armar6::skills::components::armar6_ik_demo
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -92,7 +90,6 @@ namespace armar6::skills::components::armar6_ik_demo
private:
-
void run();
// Private methods go here.
@@ -107,17 +104,16 @@ namespace armar6::skills::components::armar6_ik_demo
private:
-
static const std::string defaultName;
std::unique_ptr<IkDemo> impl;
armarx::SimpleRunningTask<>::pointer_type task;
-
/// Properties shown in the Scenario GUI.
struct Properties
{
};
+
Properties properties;
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
@@ -131,7 +127,6 @@ namespace armar6::skills::components::armar6_ik_demo
};
RemoteGuiTab tab;
*/
-
};
-} // namespace armar6::skills::components::armar6_ik_demo
+} // namespace armar6::skills::components::armar6_ik_demo
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp
index 4b3e23d3e5c4c24224d44893ee446b8d5f833f1c..4bffcf3e6edb9ffb83b7126c338e82843f0a5cdc 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Callback.cpp
@@ -1,5 +1,6 @@
#include "Callback.h"
+
#include "RobotAPI/libraries/aron/core/data/variant/primitive/String.h"
namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp
index 421a532fdec1d55defd41e12bff4257d7dd32456..32e78eb453c7442052f14937b4edefb215034126 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Chaining.cpp
@@ -12,11 +12,11 @@ namespace armarx::skills::provider
SkillDescription
ChainingSkill::GetSkillDescription()
{
- return SkillDescription{.skillId = armarx::skills::SkillID{.skillName = "Chaining"},
- .description =
- "This skill calls the Timeout skill several times. At some point the "
- "execution is aborted due to a timeout of this skill.",
- .timeout = armarx::core::time::Duration::MilliSeconds(10000)};
+ return SkillDescription{
+ .skillId = armarx::skills::SkillID{.skillName = "Chaining"},
+ .description = "This skill calls the Timeout skill several times. At some point the "
+ "execution is aborted due to a timeout of this skill.",
+ .timeout = armarx::core::time::Duration::MilliSeconds(10000)};
}
Skill::MainResult
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
index d3e66e838eeae19ae089f4af76d4d9d4f9cfc33b..276a4534eafcc91a538b98301a9de22d3a63d922 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/HelloWorld.cpp
@@ -24,13 +24,13 @@ namespace armarx::skills::provider
root_profile_params.some_list_of_matrices.push_back(Eigen::Matrix3f::Zero());
root_profile_params.some_matrix = Eigen::Matrix3f::Zero();
- return SkillDescription{.skillId = skills::SkillID{.skillName = "HelloWorld"},
- .description = "This skill logs a message on ARMARX_IMPORTANT",
- .rootProfileDefaults = root_profile_params.toAron(),
- .timeout = armarx::core::time::Duration::MilliSeconds(1000),
- .parametersType =
- armarx::skills::Example::HelloWorldAcceptedType::ToAronType(),
- .resultType = armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
+ return SkillDescription{
+ .skillId = skills::SkillID{.skillName = "HelloWorld"},
+ .description = "This skill logs a message on ARMARX_IMPORTANT",
+ .rootProfileDefaults = root_profile_params.toAron(),
+ .timeout = armarx::core::time::Duration::MilliSeconds(1000),
+ .parametersType = armarx::skills::Example::HelloWorldAcceptedType::ToAronType(),
+ .resultType = armarx::skills::Example::HelloWorldAcceptedType::ToAronType()};
}
Skill::MainResult
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/InstantKill.cpp b/source/RobotAPI/components/skills/SkillProviderExample/InstantKill.cpp
index ca3e90e5bb70c0ee9bfb961b529bc1b0cc24aa49..f00ed00f12b355c7ef8cf62959ecb2e5edb84a57 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/InstantKill.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/InstantKill.cpp
@@ -1,9 +1,9 @@
-#include <cstdlib>
#include "InstantKill.h"
-#include <thread>
#include <chrono>
+#include <cstdlib>
+#include <thread>
namespace armarx::skills::provider
{
@@ -16,8 +16,7 @@ namespace armarx::skills::provider
InstantKillSkill::GetSkillDescription()
{
return SkillDescription{.skillId = armarx::skills::SkillID{.skillName = "InstantKill"},
- .description =
- "This skill calls Timeout and instantly aboirts it.",
+ .description = "This skill calls Timeout and instantly aboirts it.",
.timeout = armarx::core::time::Duration::MilliSeconds(50000)};
}
@@ -25,17 +24,17 @@ namespace armarx::skills::provider
InstantKillSkill::main(const MainInput& in)
{
- this->throwIfSkillShouldTerminate();
+ this->throwIfSkillShouldTerminate();
- SkillProxy prx(
- manager,
- skills::SkillID{.providerId = *getSkillId().providerId, .skillName = "Chaining"});
+ SkillProxy prx(
+ manager,
+ skills::SkillID{.providerId = *getSkillId().providerId, .skillName = "Chaining"});
- for (unsigned int i = 0; i < 25; ++i)
- {
- auto id = callSubskillAsync(prx);
- prx.abortSkillAsync(id);
- }
- return {TerminatedSkillStatus::Succeeded, nullptr};
+ for (unsigned int i = 0; i < 25; ++i)
+ {
+ auto id = callSubskillAsync(prx);
+ prx.abortSkillAsync(id);
+ }
+ return {TerminatedSkillStatus::Succeeded, nullptr};
}
} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/RandomChaining.cpp b/source/RobotAPI/components/skills/SkillProviderExample/RandomChaining.cpp
index 4ca2e3f337bb3bfb41109a525373d2df19d814dd..850e906e1c5785adbaa76dd0e3d6734909fa6291 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/RandomChaining.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/RandomChaining.cpp
@@ -1,21 +1,22 @@
-#include <cstdlib>
#include "RandomChaining.h"
-#include <thread>
#include <chrono>
+#include <cstdlib>
+#include <thread>
namespace armarx::skills::provider
{
- namespace util
- {
- int randomgen(int max, int min) //Pass in range
+ namespace util
{
- srand(time(NULL)); //Changed from rand(). srand() seeds rand for you.
- int random = rand() % max + min;
- return random;
- }
- }
+ int
+ randomgen(int max, int min) //Pass in range
+ {
+ srand(time(NULL)); //Changed from rand(). srand() seeds rand for you.
+ int random = rand() % max + min;
+ return random;
+ }
+ } // namespace util
RandomChainingSkill::RandomChainingSkill() : SimpleSkill(GetSkillDescription())
{
@@ -25,48 +26,52 @@ namespace armarx::skills::provider
RandomChainingSkill::GetSkillDescription()
{
return SkillDescription{.skillId = armarx::skills::SkillID{.skillName = "RandomChaining"},
- .description =
- "This skill calls 100 random subskills from the skillProviderExample excluding the segfault skill.",
+ .description = "This skill calls 100 random subskills from the "
+ "skillProviderExample excluding the segfault skill.",
.timeout = armarx::core::time::Duration::MilliSeconds(120000)};
}
Skill::MainResult
RandomChainingSkill::main(const MainInput& in)
{
- std::vector<std::string> subskillNames = {
- "Timeout", "Chaining", "Foo", "HelloWorld",
- "Incomplete", "ShowMeCallbacks", "BusyWaiting", "Recursive"
- };
+ std::vector<std::string> subskillNames = {"Timeout",
+ "Chaining",
+ "Foo",
+ "HelloWorld",
+ "Incomplete",
+ "ShowMeCallbacks",
+ "BusyWaiting",
+ "Recursive"};
- ARMARX_CHECK(subskillNames.size() > 0);
+ ARMARX_CHECK(subskillNames.size() > 0);
- for (unsigned int i = 0; i < 100; ++i)
- {
- this->throwIfSkillShouldTerminate();
+ for (unsigned int i = 0; i < 100; ++i)
+ {
+ this->throwIfSkillShouldTerminate();
- auto index = util::randomgen(subskillNames.size() -1, 0);
+ auto index = util::randomgen(subskillNames.size() - 1, 0);
- auto subskillName = subskillNames[index];
+ auto subskillName = subskillNames[index];
- SkillProxy prx(
- manager,
- skills::SkillID{.providerId = *getSkillId().providerId, .skillName = subskillName});
+ SkillProxy prx(
+ manager,
+ skills::SkillID{.providerId = *getSkillId().providerId, .skillName = subskillName});
- if (util::randomgen(10, 0) < 2)
- {
- callSubskill(prx);
- }
- else
- {
- callSubskillAsync(prx);
+ if (util::randomgen(10, 0) < 2)
+ {
+ callSubskill(prx);
+ }
+ else
+ {
+ callSubskillAsync(prx);
- auto sleep_milliseconds = util::randomgen(1000, 0);
+ auto sleep_milliseconds = util::randomgen(1000, 0);
- ARMARX_INFO << "SLEEP FOR " << sleep_milliseconds << "ms";
- std::this_thread::sleep_for(std::chrono::milliseconds(sleep_milliseconds));
+ ARMARX_INFO << "SLEEP FOR " << sleep_milliseconds << "ms";
+ std::this_thread::sleep_for(std::chrono::milliseconds(sleep_milliseconds));
+ }
}
- }
- return {TerminatedSkillStatus::Succeeded, nullptr};
+ return {TerminatedSkillStatus::Succeeded, nullptr};
}
} // namespace armarx::skills::provider
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Recursive.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Recursive.cpp
index 7ad3c457ac45d3cde2daf94fde6e89d11ee04b54..f2278c61e2a0f6c75117f2df205806b2e51ac8af 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/Recursive.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Recursive.cpp
@@ -5,7 +5,8 @@
namespace armarx::skills::provider
{
- RecursiveSkill::RecursiveSkill() : SimpleSpecializedSkill<skills::Example::RecursiveSkillParams>(GetSkillDescription())
+ RecursiveSkill::RecursiveSkill() :
+ SimpleSpecializedSkill<skills::Example::RecursiveSkillParams>(GetSkillDescription())
{
}
@@ -15,13 +16,13 @@ namespace armarx::skills::provider
armarx::skills::Example::RecursiveSkillParams root_profile_params;
root_profile_params.n = 10;
- return SkillDescription{.skillId = skills::SkillID{.skillName = "Recursive"},
- .description = "This skill calls itself recursively {n} times",
- .rootProfileDefaults = root_profile_params.toAron(),
- .timeout = armarx::core::time::Duration::MilliSeconds(10000),
- .parametersType =
- armarx::skills::Example::RecursiveSkillParams::ToAronType(),
- .resultType = armarx::skills::Example::RecursiveSkillParams::ToAronType()};
+ return SkillDescription{
+ .skillId = skills::SkillID{.skillName = "Recursive"},
+ .description = "This skill calls itself recursively {n} times",
+ .rootProfileDefaults = root_profile_params.toAron(),
+ .timeout = armarx::core::time::Duration::MilliSeconds(10000),
+ .parametersType = armarx::skills::Example::RecursiveSkillParams::ToAronType(),
+ .resultType = armarx::skills::Example::RecursiveSkillParams::ToAronType()};
}
Skill::MainResult
@@ -31,8 +32,9 @@ namespace armarx::skills::provider
if (n > 0)
{
- SkillProxy prx(manager,
- skills::SkillID{.providerId = *getSkillId().providerId, .skillName = "Recursive"});
+ SkillProxy prx(
+ manager,
+ skills::SkillID{.providerId = *getSkillId().providerId, .skillName = "Recursive"});
armarx::skills::Example::RecursiveSkillParams params;
params.n = n - 1;
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Recursive.h b/source/RobotAPI/components/skills/SkillProviderExample/Recursive.h
index ac96cce58a07cea59f6b118f6b410a46d7e09210..f0ccac48653f2097c9bc4cce1e4f0884bb647012 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/Recursive.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Recursive.h
@@ -23,9 +23,8 @@
#pragma once
// RobotAPI
-#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
-
#include <RobotAPI/components/skills/SkillProviderExample/aron/RecursiveSkillParams.aron.generated.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
namespace armarx::skills::provider
{
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/Segfault.cpp b/source/RobotAPI/components/skills/SkillProviderExample/Segfault.cpp
index e7a44b46f7f1c6b483946e75b4ed8994cfb2c154..548baaeaa5b3e3fb420153ff926f7c8472aa7a4e 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/Segfault.cpp
+++ b/source/RobotAPI/components/skills/SkillProviderExample/Segfault.cpp
@@ -12,7 +12,9 @@ namespace armarx::skills::provider
{
return SkillDescription{
.skillId = skills::SkillID{.skillName = "Segfault"},
- .description = "This skill segfaults while executing. This will kill (Signal 11) the Provider to test the stability of the GUI and other components in case of a skill issue.",
+ .description =
+ "This skill segfaults while executing. This will kill (Signal 11) the Provider to "
+ "test the stability of the GUI and other components in case of a skill issue.",
.timeout = armarx::core::time::Duration::MilliSeconds(100)};
}
diff --git a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
index bf4c851da0a71382e9e3c3fb8bc6315306bb8d20..b673efa4cfe51434b31d3edcc5b4b80e6e6301ed 100644
--- a/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
+++ b/source/RobotAPI/components/skills/SkillProviderExample/SkillProviderExample.h
@@ -29,17 +29,17 @@
// RobotAPI
#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
+#include "BusyWaiting.h"
#include "Callback.h"
#include "Chaining.h"
#include "ChainingAsync.h"
#include "HelloWorld.h"
#include "Incomplete.h"
-#include "Segfault.h"
-#include "Timeout.h"
-#include "RandomChaining.h"
#include "InstantKill.h"
-#include "BusyWaiting.h"
+#include "RandomChaining.h"
#include "Recursive.h"
+#include "Segfault.h"
+#include "Timeout.h"
namespace armarx::skills::provider
{
diff --git a/source/RobotAPI/components/units/ATINetFTUnit.cpp b/source/RobotAPI/components/units/ATINetFTUnit.cpp
index 865751992016837105d04bfc4e94311dbecd433d..6170bfae88279e77b7578f217b442bc441195009 100644
--- a/source/RobotAPI/components/units/ATINetFTUnit.cpp
+++ b/source/RobotAPI/components/units/ATINetFTUnit.cpp
@@ -23,30 +23,36 @@
*/
#include "ATINetFTUnit.h"
-#include <sys/socket.h>
-#include <arpa/inet.h>
-#include <iostream>
-#include <string>
-#include <netdb.h>
+
+#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
+
+#include <iostream>
+#include <string>
#include <vector>
-#include <inttypes.h>
+
+#include <arpa/inet.h>
+#include <netdb.h>
+#include <sys/socket.h>
+
namespace armarx
{
- void ATINetFTUnit::onInitComponent()
+ void
+ ATINetFTUnit::onInitComponent()
{
-
}
- void ATINetFTUnit::onConnectComponent()
+ void
+ ATINetFTUnit::onConnectComponent()
{
/*
* Read ATINetFT hostname and port from properties
* std::string receiverHostName("192.168.1.1:49152");
*/
- std::string ATINetFTReceiverHostName = getProperty<std::string>("ATINetFTReceiveHostName").getValue();
+ std::string ATINetFTReceiverHostName =
+ getProperty<std::string>("ATINetFTReceiveHostName").getValue();
std::string captureName = getProperty<std::string>("CaptureName").getValue();
ARMARX_INFO << "Capture Name " << captureName;
size_t pos = captureName.find("0");
@@ -89,19 +95,19 @@ namespace armarx
}*/
}
- void ATINetFTUnit::onDisconnectComponent()
+ void
+ ATINetFTUnit::onDisconnectComponent()
{
ARMARX_IMPORTANT << "Disconnecting from ATINetFT" << flush;
-
}
-
- void ATINetFTUnit::onExitComponent()
+ void
+ ATINetFTUnit::onExitComponent()
{
-
}
- void ATINetFTUnit::periodicExec()
+ void
+ ATINetFTUnit::periodicExec()
{
if (recordingStarted)
@@ -121,18 +127,17 @@ namespace armarx
writeFTValuesToFile(ftdata.data(), ftdata.size());
sampleIndex++;
}
-
}
-
}
- PropertyDefinitionsPtr ATINetFTUnit::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ ATINetFTUnit::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new ATINetFTUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(new ATINetFTUnitPropertyDefinitions(getConfigIdentifier()));
}
- void ATINetFTUnit::startRecording(const std::string& customName, const Ice::Current& c)
+ void
+ ATINetFTUnit::startRecording(const std::string& customName, const Ice::Current& c)
{
if (remoteTriggerEnabled)
{
@@ -158,7 +163,8 @@ namespace armarx
}
}
- void ATINetFTUnit::stopRecording(const Ice::Current& c)
+ void
+ ATINetFTUnit::stopRecording(const Ice::Current& c)
{
if (remoteTriggerEnabled)
{
@@ -175,15 +181,20 @@ namespace armarx
recordingFile.close();
recordingStarted = false;
}
-
}
}
- bool ATINetFTUnit::sendPacket(char* bytePacket, int bpSize)
+ bool
+ ATINetFTUnit::sendPacket(char* bytePacket, int bpSize)
{
if (remoteTriggerEnabled)
{
- if (sendto(senderSocket, bytePacket, bpSize, 0, (struct sockaddr*)&receiveHostAddr, sizeof(receiveHostAddr)) != bpSize)
+ if (sendto(senderSocket,
+ bytePacket,
+ bpSize,
+ 0,
+ (struct sockaddr*)&receiveHostAddr,
+ sizeof(receiveHostAddr)) != bpSize)
{
return false;
@@ -193,12 +204,18 @@ namespace armarx
return false;
}
- bool ATINetFTUnit::receivePacket(char* receiveBuf)
+ bool
+ ATINetFTUnit::receivePacket(char* receiveBuf)
{
int test = sizeof(receiveHostAddr);
- int receiveint = recvfrom(senderSocket, receiveBuf, receivePacketSize, 0, (struct sockaddr*) &receiveHostAddr, ((socklen_t*)&test));
+ int receiveint = recvfrom(senderSocket,
+ receiveBuf,
+ receivePacketSize,
+ 0,
+ (struct sockaddr*)&receiveHostAddr,
+ ((socklen_t*)&test));
if (receiveint < 0)
{
@@ -210,7 +227,8 @@ namespace armarx
return true;
}
- void ATINetFTUnit::convertToFTValues(char* receiveBuf, float* ftdata, int ftdataSize)
+ void
+ ATINetFTUnit::convertToFTValues(char* receiveBuf, float* ftdata, int ftdataSize)
{
//uint32_t rdt_sequence = ntohl(*(uint32_t*)&receiveBuf[0]); //was an unused variable
//uint32_t ft_sequence = ntohl(*(uint32_t*)&receiveBuf[4]); //was an unused variable
@@ -222,7 +240,8 @@ namespace armarx
}
}
- void ATINetFTUnit::writeFTValuesToFile(float* ftdata, int ftdataSize)
+ void
+ ATINetFTUnit::writeFTValuesToFile(float* ftdata, int ftdataSize)
{
recordingFile << sampleIndex << " ";
for (int i = 0; i < ftdataSize; i++)
@@ -232,7 +251,8 @@ namespace armarx
recordingFile << "\n";
}
- void ATINetFTUnit::establishATINetFTReceiveHostConnection(std::string receiverHostName)
+ void
+ ATINetFTUnit::establishATINetFTReceiveHostConnection(std::string receiverHostName)
{
remoteTriggerEnabled = false;
@@ -248,7 +268,7 @@ namespace armarx
return;
}
- if (bind(senderSocket, (struct sockaddr*) &senderHostAddr, sizeof(senderHostAddr)) < 0)
+ if (bind(senderSocket, (struct sockaddr*)&senderHostAddr, sizeof(senderHostAddr)) < 0)
{
return;
}
@@ -271,7 +291,7 @@ namespace armarx
return;
}
- addr_list = (struct in_addr**) he->h_addr_list;
+ addr_list = (struct in_addr**)he->h_addr_list;
for (i = 0; addr_list[i] != NULL; i++)
{
@@ -284,7 +304,7 @@ namespace armarx
receiveHostAddr.sin_port = htons(atoi(hostport.c_str()));
remoteTriggerEnabled = true;
- ARMARX_INFO << "Connection established to " << hostname << ":" << hostport;
+ ARMARX_INFO << "Connection established to " << hostname << ":" << hostport;
//if((receiverSocket=socket(AF_INET, SOCK_DGRAM, 0))<0) {
// return;
//}
@@ -295,6 +315,5 @@ namespace armarx
return;
}
return;
-
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/ATINetFTUnit.h b/source/RobotAPI/components/units/ATINetFTUnit.h
index e8fdfc296819d86815fbaff429aa318dd83e568a..9ba4393476bb057cf4ab22f12f371a6cceb95641 100644
--- a/source/RobotAPI/components/units/ATINetFTUnit.h
+++ b/source/RobotAPI/components/units/ATINetFTUnit.h
@@ -25,11 +25,14 @@
#pragma once
+#include <fstream>
+
+#include <netinet/in.h>
+
#include <ArmarXCore/core/Component.h>
+
#include <RobotAPI/interface/units/ATINetFTUnit.h>
#include <RobotAPI/interface/units/UnitInterface.h>
-#include <netinet/in.h>
-#include <fstream>
namespace armarx
{
@@ -37,25 +40,19 @@ namespace armarx
* @class ATINetFTUnitPropertyDefinitions
* @brief
*/
- class ATINetFTUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class ATINetFTUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- ATINetFTUnitPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ ATINetFTUnitPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>(
"ATINetFTHostName",
"i61ATINetFT1:801",
"ATINetFT host name and port, usually i61ATINetFT1:801 or i61ATINetFT2:801");
defineOptionalProperty<std::string>(
- "DatabasePathName",
- "",
- "Path name to motion database on receiving ATINetFT host");
+ "DatabasePathName", "", "Path name to motion database on receiving ATINetFT host");
defineOptionalProperty<std::string>(
- "CaptureName",
- "Trial01",
- "Custom name of first trial in motion recording session");
+ "CaptureName", "Trial01", "Custom name of first trial in motion recording session");
defineOptionalProperty<std::string>(
"ATINetFTReceiveHostName",
"i61ATINetFT1:801",
@@ -83,15 +80,14 @@ namespace armarx
* Documentation mainly uses the term subject instead of object, as that's the
* common term in the context of ATINetFT.
*/
- class ATINetFTUnit :
- virtual public armarx::Component,
- virtual public ATINetFTUnitInterface
+ class ATINetFTUnit : virtual public armarx::Component, virtual public ATINetFTUnitInterface
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "ATINetFTUnit";
}
@@ -101,26 +97,40 @@ namespace armarx
void stopRecording(const Ice::Current& c) override;
- bool isComponentOnline(const Ice::Current& c) override
+ bool
+ isComponentOnline(const Ice::Current& c) override
{
return remoteTriggerEnabled;
};
- void request(const Ice::Current& c) override {};
-
+ void request(const Ice::Current& c) override{};
- void release(const Ice::Current& c) override { }
+ void
+ release(const Ice::Current& c) override
+ {
+ }
- void init(const Ice::Current& c) override { }
+ void
+ init(const Ice::Current& c) override
+ {
+ }
- void start(const Ice::Current& c) override { }
+ void
+ start(const Ice::Current& c) override
+ {
+ }
- void stop(const Ice::Current& c) override { }
+ void
+ stop(const Ice::Current& c) override
+ {
+ }
- armarx::UnitExecutionState getExecutionState(const Ice::Current& c) override
+ armarx::UnitExecutionState
+ getExecutionState(const Ice::Current& c) override
{
return armarx::eUndefinedUnitExecutionState;
}
+
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
@@ -181,7 +191,5 @@ namespace armarx
std::string databasePathName;
struct sockaddr_in receiveHostAddr;
struct sockaddr_in senderHostAddr;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 4a99205ed9f2fd841ce793a5a5794ec4c948b660..688b86d04b0be1e858b82654f7fc9fa8012ffd67 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -22,21 +22,21 @@
#include "ForceTorqueObserver.h"
-#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
+#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
-#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
-#include <ArmarXCore/core/util/StringHelpers.h>
-#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
-#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
-
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
+#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#define RAWFORCE "_rawforcevectors"
#define OFFSETFORCE "_forceswithoffsetvectors"
@@ -46,19 +46,20 @@
#define FORCETORQUEVECTORS "_forceTorqueVectors"
#define POD_SUFFIX "_pod"
-
namespace armarx
{
ForceTorqueObserver::ForceTorqueObserver()
{
}
- void ForceTorqueObserver::setTopicName(std::string topicName)
+ void
+ ForceTorqueObserver::setTopicName(std::string topicName)
{
this->topicName = topicName;
}
- void ForceTorqueObserver::onInitObserver()
+ void
+ ForceTorqueObserver::onInitObserver()
{
if (topicName.empty())
{
@@ -82,7 +83,8 @@ namespace armarx
usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
offeringTopic("DebugDrawerUpdates");
- auto sensorRobotNodeSplit = armarx::Split(getProperty<std::string>("SensorRobotNodeMapping").getValue(), ",");
+ auto sensorRobotNodeSplit =
+ armarx::Split(getProperty<std::string>("SensorRobotNodeMapping").getValue(), ",");
for (auto& elem : sensorRobotNodeSplit)
{
simox::alg::trim(elem);
@@ -97,22 +99,36 @@ namespace armarx
}
}
- void ForceTorqueObserver::onConnectObserver()
+ void
+ ForceTorqueObserver::onConnectObserver()
{
- robot = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
- localRobot = RemoteRobot::createLocalCloneFromFile(robot, VirtualRobot::RobotIO::eStructure);
+ robot = getProxy<RobotStateComponentInterfacePrx>(
+ getProperty<std::string>("RobotStateComponentName").getValue());
+ localRobot =
+ RemoteRobot::createLocalCloneFromFile(robot, VirtualRobot::RobotIO::eStructure);
debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
if (getProperty<bool>("VisualizeForce").getValue())
{
- visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, 1000 / getProperty<int>("VisualizeForceUpdateFrequency").getValue(), false, "visualizerFunction");
+ visualizerTask = new PeriodicTask<ForceTorqueObserver>(
+ this,
+ &ForceTorqueObserver::visualizerFunction,
+ 1000 / getProperty<int>("VisualizeForceUpdateFrequency").getValue(),
+ false,
+ "visualizerFunction");
visualizerTask->start();
}
- updateRobotTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::updateRobot, 1000 / getProperty<int>("RobotUpdateFrequency").getValue(), false, "updateRobot", false);
+ updateRobotTask = new PeriodicTask<ForceTorqueObserver>(
+ this,
+ &ForceTorqueObserver::updateRobot,
+ 1000 / getProperty<int>("RobotUpdateFrequency").getValue(),
+ false,
+ "updateRobot",
+ false);
updateRobotTask->start();
-
}
- void ForceTorqueObserver::visualizerFunction()
+ void
+ ForceTorqueObserver::visualizerFunction()
{
if (!localRobot)
{
@@ -134,91 +150,95 @@ namespace armarx
{
// if (localRobot->hasRobotNode(channel.first))
{
- DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::emptyCurrent));
+ DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(
+ getForceDatafield(channel.first, Ice::emptyCurrent));
FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
- if (frameAlreadyReported.count(value->frame) > 0 || (value->frame != GlobalFrame && !value->frame.empty() && !localRobot->hasRobotNode(value->frame)))
+ if (frameAlreadyReported.count(value->frame) > 0 ||
+ (value->frame != GlobalFrame && !value->frame.empty() &&
+ !localRobot->hasRobotNode(value->frame)))
{
continue;
}
frameAlreadyReported.insert(value->frame);
auto force = value->toGlobal(localRobot);
- ARMARX_DEBUG << deactivateSpam(5, channel.first) << "new force " << channel.first << " : " << force->toEigen() << " original frame: " << value->frame;
+ ARMARX_DEBUG << deactivateSpam(5, channel.first) << "new force "
+ << channel.first << " : " << force->toEigen()
+ << " original frame: " << value->frame;
float forceMag = force->toEigen().norm() * forceFactor;
- Vector3Ptr pos = new Vector3(localRobot->getRobotNode(value->frame)->getGlobalPose());
+ Vector3Ptr pos =
+ new Vector3(localRobot->getRobotNode(value->frame)->getGlobalPose());
forceMag = std::min(1.0f, forceMag);
batchPrx->setArrowVisu("Forces",
"Force" + channel.first,
pos,
force,
- DrawColor {forceMag, 1.0f - forceMag, 0.0f, 0.5f},
+ DrawColor{forceMag, 1.0f - forceMag, 0.0f, 0.5f},
std::max(arrowLength * forceMag, 10.f),
3);
- field = DatafieldRefPtr::dynamicCast(getTorqueDatafield(channel.first, Ice::emptyCurrent));
+ field = DatafieldRefPtr::dynamicCast(
+ getTorqueDatafield(channel.first, Ice::emptyCurrent));
value = field->getDataField()->get<FramedDirection>();
auto torque = value;
// ARMARX_INFO << deactivateSpam(1, torque->frame) << "Reporting for " << channel.first << " in frame " << torque->frame;
- Eigen::Vector3f dirXglobal = FramedDirection(Eigen::Vector3f::UnitX(), torque->frame, torque->agent).toGlobalEigen(localRobot);
- Eigen::Vector3f dirYglobal = FramedDirection(Eigen::Vector3f::UnitY(), torque->frame, torque->agent).toGlobalEigen(localRobot);
- Eigen::Vector3f dirZglobal = FramedDirection(Eigen::Vector3f::UnitZ(), torque->frame, torque->agent).toGlobalEigen(localRobot);
+ Eigen::Vector3f dirXglobal =
+ FramedDirection(Eigen::Vector3f::UnitX(), torque->frame, torque->agent)
+ .toGlobalEigen(localRobot);
+ Eigen::Vector3f dirYglobal =
+ FramedDirection(Eigen::Vector3f::UnitY(), torque->frame, torque->agent)
+ .toGlobalEigen(localRobot);
+ Eigen::Vector3f dirZglobal =
+ FramedDirection(Eigen::Vector3f::UnitZ(), torque->frame, torque->agent)
+ .toGlobalEigen(localRobot);
// ARMARX_INFO << channel.first << VAROUT(torque) << VAROUT(dirXglobal) << VAROUT(dirYglobal) << VAROUT(dirZglobal);
if (fabs(torque->x) > torqueVisuDeadZone)
{
batchPrx->setCircleArrowVisu("Torques",
- "TorqueX" + channel.first,
+ "TorqueX" + channel.first,
pos,
new Vector3(dirXglobal),
50,
torque->x / maxTorque,
3 * std::max(1.0f, torque->x / maxTorque),
- DrawColor {1.0f, 0.0f, 0.0f, 0.5f}
- );
+ DrawColor{1.0f, 0.0f, 0.0f, 0.5f});
}
else
{
- batchPrx->removeCircleVisu("Torques",
- "TorqueX" + channel.first);
+ batchPrx->removeCircleVisu("Torques", "TorqueX" + channel.first);
}
if (fabs(torque->y) > torqueVisuDeadZone)
{
batchPrx->setCircleArrowVisu("Torques",
- "TorqueY" + channel.first,
+ "TorqueY" + channel.first,
pos,
new Vector3(dirYglobal),
50,
torque->y / maxTorque,
3 * std::max(1.0f, torque->y / maxTorque),
- DrawColor {0.0f, 1.0f, 0.0f, 0.5f}
- );
+ DrawColor{0.0f, 1.0f, 0.0f, 0.5f});
}
else
{
- batchPrx->removeCircleVisu("Torques",
- "TorqueY" + channel.first);
-
+ batchPrx->removeCircleVisu("Torques", "TorqueY" + channel.first);
}
if (fabs(torque->z) > torqueVisuDeadZone)
{
batchPrx->setCircleArrowVisu("Torques",
- "TorqueZ" + channel.first,
+ "TorqueZ" + channel.first,
pos,
new Vector3(dirZglobal),
50,
torque->z / maxTorque,
3 * std::max(1.0f, torque->z / maxTorque),
- DrawColor {0.0f, 0.0f, 1.0f, 0.5f}
- );
+ DrawColor{0.0f, 0.0f, 1.0f, 0.5f});
}
else
{
- batchPrx->removeCircleVisu("Torques",
- "TorqueZ" + channel.first);
-
+ batchPrx->removeCircleVisu("Torques", "TorqueZ" + channel.first);
}
-
}
// else
@@ -228,48 +248,54 @@ namespace armarx
}
catch (...)
{
-
}
}
}
batchPrx->ice_flushBatchRequests();
}
-
- PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ ForceTorqueObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new ForceTorqueObserverPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new ForceTorqueObserverPropertyDefinitions(getConfigIdentifier()));
}
- void ForceTorqueObserver::updateRobot()
+ void
+ ForceTorqueObserver::updateRobot()
{
std::unique_lock lock(dataMutex);
RemoteRobot::synchronizeLocalClone(localRobot, robot);
}
-
- void ForceTorqueObserver::offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id)
+ void
+ ForceTorqueObserver::offerValue(const std::string& nodeName,
+ const std::string& type,
+ const FramedDirectionBasePtr& value,
+ const DataFieldIdentifierPtr& id)
{
FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(value);
-
-
try
{
setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
}
catch (...)
{
- ARMARX_INFO << "Creating force/torque fields for node " << nodeName << " with id " << id->getIdentifierStr();
+ ARMARX_INFO << "Creating force/torque fields for node " << nodeName << " with id "
+ << id->getIdentifierStr();
if (!existsDataField(id->channelName, id->datafieldName))
{
if (!existsChannel(id->channelName))
{
- offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
+ offerChannel(id->channelName,
+ type + " vectors on specific parts of the robot.");
}
- offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
+ offerDataFieldWithDefault(id->channelName,
+ id->datafieldName,
+ Variant(value),
+ "3D vector for " + type + " of " + nodeName);
}
}
@@ -280,9 +306,9 @@ namespace armarx
try
{
StringVariantBaseMap values;
- values[id->datafieldName + "_x" ] = new Variant(vec->x);
- values[id->datafieldName + "_y" ] = new Variant(vec->y);
- values[id->datafieldName + "_z" ] = new Variant(vec->z);
+ values[id->datafieldName + "_x"] = new Variant(vec->x);
+ values[id->datafieldName + "_y"] = new Variant(vec->y);
+ values[id->datafieldName + "_z"] = new Variant(vec->z);
values[id->datafieldName + "_frame"] = new Variant(vec->frame);
setDataFieldsFlatCopy(pod_channelName, values);
}
@@ -291,24 +317,35 @@ namespace armarx
ARMARX_INFO << "Creating force/torque pod fields";
if (!existsChannel(pod_channelName))
{
- offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
-
+ offerChannel(pod_channelName,
+ id->datafieldName + " on " + nodeName + " as plain old data (pod)");
}
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_x", Variant(vec->x), type + " on X axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_y", Variant(vec->y), type + " on Y axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_z", Variant(vec->z), type + " on Z axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_frame", Variant(vec->frame), "Frame of " + value->frame);
+ offerOrUpdateDataField(
+ pod_channelName, id->datafieldName + "_x", Variant(vec->x), type + " on X axis");
+ offerOrUpdateDataField(
+ pod_channelName, id->datafieldName + "_y", Variant(vec->y), type + " on Y axis");
+ offerOrUpdateDataField(
+ pod_channelName, id->datafieldName + "_z", Variant(vec->z), type + " on Z axis");
+ offerOrUpdateDataField(pod_channelName,
+ id->datafieldName + "_frame",
+ Variant(vec->frame),
+ "Frame of " + value->frame);
}
-
-
-
}
- void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current& c)
+ void
+ armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName,
+ const FramedDirectionBasePtr& forces,
+ const FramedDirectionBasePtr& torques,
+ const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
- auto offerForceAndTorqueValue = [ =, this ](std::string const & sensorNodeName, std::string const & frame, std::string channelName, const FramedDirectionBasePtr & forces, const FramedDirectionBasePtr & torques)
+ auto offerForceAndTorqueValue = [=, this](std::string const& sensorNodeName,
+ std::string const& frame,
+ std::string channelName,
+ const FramedDirectionBasePtr& forces,
+ const FramedDirectionBasePtr& torques)
{
try
{
@@ -358,7 +395,6 @@ namespace armarx
}
-
FramedDirectionPtr realTorques = FramedDirectionPtr::dynamicCast(torques);
realTorques->changeFrame(localRobot, forces->frame);
@@ -376,52 +412,61 @@ namespace armarx
forcesCopy->changeFrame(localRobot, robotNodeName);
torquesCopy->changeFrame(localRobot, robotNodeName);
- offerForceAndTorqueValue(sensorName, robotNodeName, channelName, forcesCopy, torquesCopy);
+ offerForceAndTorqueValue(
+ sensorName, robotNodeName, channelName, forcesCopy, torquesCopy);
}
}
-
- DatafieldRefBasePtr armarx::ForceTorqueObserver::createNulledDatafield(const DatafieldRefBasePtr& forceTorqueDatafieldRef, const Ice::Current&)
+ DatafieldRefBasePtr
+ armarx::ForceTorqueObserver::createNulledDatafield(
+ const DatafieldRefBasePtr& forceTorqueDatafieldRef,
+ const Ice::Current&)
{
return createFilteredDatafield(new filters::OffsetFilter(), forceTorqueDatafieldRef);
}
- DatafieldRefBasePtr ForceTorqueObserver::getForceDatafield(const std::string& nodeName, const Ice::Current&)
+ DatafieldRefBasePtr
+ ForceTorqueObserver::getForceDatafield(const std::string& nodeName, const Ice::Current&)
{
auto id = getForceDatafieldId(nodeName, nodeName);
if (!existsChannel(id->channelName))
{
- throw UserException("No sensor for node '" + nodeName + "'available: channel " + id->channelName);
+ throw UserException("No sensor for node '" + nodeName + "'available: channel " +
+ id->channelName);
}
if (!existsDataField(id->channelName, id->datafieldName))
{
- throw UserException("No sensor for node '" + nodeName + "'available: datafield " + id->datafieldName);
+ throw UserException("No sensor for node '" + nodeName + "'available: datafield " +
+ id->datafieldName);
}
return new DatafieldRef(this, id->channelName, id->datafieldName);
-
}
- DatafieldRefBasePtr ForceTorqueObserver::getTorqueDatafield(const std::string& nodeName, const Ice::Current&)
+ DatafieldRefBasePtr
+ ForceTorqueObserver::getTorqueDatafield(const std::string& nodeName, const Ice::Current&)
{
auto id = getTorqueDatafieldId(nodeName, nodeName);
if (!existsChannel(id->channelName))
{
- throw UserException("No sensor for node '" + nodeName + "'available: channel " + id->channelName);
+ throw UserException("No sensor for node '" + nodeName + "'available: channel " +
+ id->channelName);
}
if (!existsDataField(id->channelName, id->datafieldName))
{
- throw UserException("No sensor for node '" + nodeName + "'available: datafield " + id->datafieldName);
+ throw UserException("No sensor for node '" + nodeName + "'available: datafield " +
+ id->datafieldName);
}
return new DatafieldRef(this, id->channelName, id->datafieldName);
}
- DataFieldIdentifierPtr ForceTorqueObserver::getForceDatafieldId(const std::string& nodeName, const std::string& frame)
+ DataFieldIdentifierPtr
+ ForceTorqueObserver::getForceDatafieldId(const std::string& nodeName, const std::string& frame)
{
std::string channelName;
@@ -439,7 +484,8 @@ namespace armarx
return id;
}
- DataFieldIdentifierPtr ForceTorqueObserver::getTorqueDatafieldId(const std::string& nodeName, const std::string& frame)
+ DataFieldIdentifierPtr
+ ForceTorqueObserver::getTorqueDatafieldId(const std::string& nodeName, const std::string& frame)
{
std::string channelName;
@@ -457,7 +503,8 @@ namespace armarx
return id;
}
- void ForceTorqueObserver::onDisconnectComponent()
+ void
+ ForceTorqueObserver::onDisconnectComponent()
{
if (visualizerTask)
{
@@ -469,8 +516,8 @@ namespace armarx
}
}
- void ForceTorqueObserver::onExitObserver()
+ void
+ ForceTorqueObserver::onExitObserver()
{
-
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index 0f17d8a498377664b98882e26dff3c9b8df1bb50..a837af572616dc2a28c4f7004665a46b88798a09 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -22,12 +22,13 @@
#pragma once
-#include <RobotAPI/interface/units/ForceTorqueUnit.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
+#include <mutex>
+
#include <ArmarXCore/observers/Observer.h>
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include <mutex>
+#include <RobotAPI/interface/units/ForceTorqueUnit.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx
{
@@ -35,24 +36,43 @@ namespace armarx
* \class ForceTorqueObserverPropertyDefinitions
* \brief
*/
- class ForceTorqueObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class ForceTorqueObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- ForceTorqueObserverPropertyDefinitions(std::string prefix):
+ ForceTorqueObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("ForceTorqueTopicName", "Name of the ForceTorqueUnit Topic");
- defineOptionalProperty<bool>("VisualizeForce", true, "Visualize the force with an arrow in the debug drawer");
- defineOptionalProperty<int>("RobotUpdateFrequency", 50, "Update frequency of the local robot");
- defineOptionalProperty<int>("VisualizeForceUpdateFrequency", 30, "Frequency with which the force is visualized");
- defineOptionalProperty<float>("ForceVisualizerFactor", 0.01f, "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
- defineOptionalProperty<float>("MaxExpectedTorqueValue", 30, "The torque visualization circle reaches the full circle at this value");
- defineOptionalProperty<float>("TorqueVisuDeadZone", 1, "Torques below this threshold are not visualized.");
- defineOptionalProperty<float>("MaxForceArrowLength", 150, "Length of the force visu arrow in mm");
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
- defineOptionalProperty<std::string>("SensorRobotNodeMapping", "", "Triplets of sensor node name, target frame robot node name and optional channel name: Sensor values are also reported in the frame of the robot node: e. g. SensorName:RobotNodeName[:ChannelName],SensorName2:RobotNodeName2[:ChannelName2]");
-
+ defineRequiredProperty<std::string>("ForceTorqueTopicName",
+ "Name of the ForceTorqueUnit Topic");
+ defineOptionalProperty<bool>(
+ "VisualizeForce", true, "Visualize the force with an arrow in the debug drawer");
+ defineOptionalProperty<int>(
+ "RobotUpdateFrequency", 50, "Update frequency of the local robot");
+ defineOptionalProperty<int>("VisualizeForceUpdateFrequency",
+ 30,
+ "Frequency with which the force is visualized");
+ defineOptionalProperty<float>(
+ "ForceVisualizerFactor",
+ 0.01f,
+ "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
+ defineOptionalProperty<float>(
+ "MaxExpectedTorqueValue",
+ 30,
+ "The torque visualization circle reaches the full circle at this value");
+ defineOptionalProperty<float>(
+ "TorqueVisuDeadZone", 1, "Torques below this threshold are not visualized.");
+ defineOptionalProperty<float>(
+ "MaxForceArrowLength", 150, "Length of the force visu arrow in mm");
+ defineOptionalProperty<std::string>(
+ "RobotStateComponentName",
+ "RobotStateComponent",
+ "Name of the RobotStateComponent that should be used");
+ defineOptionalProperty<std::string>(
+ "SensorRobotNodeMapping",
+ "",
+ "Triplets of sensor node name, target frame robot node name and optional channel "
+ "name: Sensor values are also reported in the frame of the robot node: e. g. "
+ "SensorName:RobotNodeName[:ChannelName],SensorName2:RobotNodeName2[:ChannelName2]");
}
};
@@ -74,17 +94,22 @@ namespace armarx
void setTopicName(std::string topicName);
// framework hooks
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "ForceTorqueUnitObserver";
}
+
void onInitObserver() override;
void onConnectObserver() override;
void onExitObserver() override;
void visualizerFunction();
- void reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current&) override;
+ void reportSensorValues(const std::string& sensorNodeName,
+ const FramedDirectionBasePtr& forces,
+ const FramedDirectionBasePtr& torques,
+ const Ice::Current&) override;
/**
* @see PropertyUser::createPropertyDefinitions()
@@ -102,24 +127,32 @@ namespace armarx
PeriodicTask<ForceTorqueObserver>::pointer_type visualizerTask;
PeriodicTask<ForceTorqueObserver>::pointer_type updateRobotTask;
// One sensor can be reported in multiple frames => multimap
- std::multimap<std::string, std::pair<std::string, std::string> > sensorRobotNodeMapping;
+ std::multimap<std::string, std::pair<std::string, std::string>> sensorRobotNodeMapping;
- void offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id);
+ void offerValue(const std::string& nodeName,
+ const std::string& type,
+ const FramedDirectionBasePtr& value,
+ const DataFieldIdentifierPtr& id);
// ForceTorqueUnitObserverInterface interface
public:
- DatafieldRefBasePtr createNulledDatafield(const DatafieldRefBasePtr& forceTorqueDatafieldRef, const Ice::Current&) override;
+ DatafieldRefBasePtr
+ createNulledDatafield(const DatafieldRefBasePtr& forceTorqueDatafieldRef,
+ const Ice::Current&) override;
- DatafieldRefBasePtr getForceDatafield(const std::string& nodeName, const Ice::Current&) override;
- DatafieldRefBasePtr getTorqueDatafield(const std::string& nodeName, const Ice::Current&) override;
+ DatafieldRefBasePtr getForceDatafield(const std::string& nodeName,
+ const Ice::Current&) override;
+ DatafieldRefBasePtr getTorqueDatafield(const std::string& nodeName,
+ const Ice::Current&) override;
- DataFieldIdentifierPtr getForceDatafieldId(const std::string& nodeName, const std::string& frame);
- DataFieldIdentifierPtr getTorqueDatafieldId(const std::string& nodeName, const std::string& frame);
+ DataFieldIdentifierPtr getForceDatafieldId(const std::string& nodeName,
+ const std::string& frame);
+ DataFieldIdentifierPtr getTorqueDatafieldId(const std::string& nodeName,
+ const std::string& frame);
// ManagedIceObject interface
protected:
void onDisconnectComponent() override;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/ForceTorqueUnit.cpp b/source/RobotAPI/components/units/ForceTorqueUnit.cpp
index 50cebe8c58ba2ff19f30636299dc23e8dab569a8..ab28922bdb78a2531acb4c7eb0851f8f3deba9bd 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnit.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueUnit.cpp
@@ -26,27 +26,32 @@
namespace armarx
{
- void ForceTorqueUnit::onInitComponent()
+ void
+ ForceTorqueUnit::onInitComponent()
{
listenerName = getProperty<std::string>("ForceTorqueTopicName").getValue();
offeringTopic(listenerName);
onInitForceTorqueUnit();
}
- void ForceTorqueUnit::onConnectComponent()
+ void
+ ForceTorqueUnit::onConnectComponent()
{
ARMARX_INFO << "setting report topic to " << listenerName << flush;
listenerPrx = getTopic<ForceTorqueUnitListenerPrx>(listenerName);
onStartForceTorqueUnit();
}
- void ForceTorqueUnit::onExitComponent()
+ void
+ ForceTorqueUnit::onExitComponent()
{
onExitForceTorqueUnit();
}
- PropertyDefinitionsPtr ForceTorqueUnit::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ ForceTorqueUnit::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new ForceTorqueUnitPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new ForceTorqueUnitPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/ForceTorqueUnit.h b/source/RobotAPI/components/units/ForceTorqueUnit.h
index d61360637f06d1d1d33ecc2d8c0fe283b7ed0b34..74c712bee16d4b6418c01b50a25d5b3bc0f125f0 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnit.h
+++ b/source/RobotAPI/components/units/ForceTorqueUnit.h
@@ -25,15 +25,15 @@
#pragma once
+#include <string>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/components/units/SensorActorUnit.h>
+#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/units/ForceTorqueUnit.h>
-#include <string>
-
namespace armarx
{
/**
@@ -46,8 +46,12 @@ namespace armarx
ForceTorqueUnitPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
- defineOptionalProperty<std::string>("ForceTorqueTopicName", "ForceTorqueValues", "Name of the topic on which the sensor values are provided");
+ defineRequiredProperty<std::string>(
+ "AgentName", "Name of the agent for which the sensor values are provided");
+ defineOptionalProperty<std::string>(
+ "ForceTorqueTopicName",
+ "ForceTorqueValues",
+ "Name of the topic on which the sensor values are provided");
// No required properties
@@ -69,12 +73,11 @@ namespace armarx
* @ingroup Component-ForceTorqueUnit
* @brief The ForceTorqueUnit class
*/
- class ForceTorqueUnit :
- virtual public ForceTorqueUnitInterface,
- virtual public SensorActorUnit
+ class ForceTorqueUnit : virtual public ForceTorqueUnitInterface, virtual public SensorActorUnit
{
public:
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "ForceTorqueUnit";
}
@@ -93,4 +96,4 @@ namespace armarx
ForceTorqueUnitListenerPrx listenerPrx;
std::string listenerName;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
index 2a4d495c80c135afa06ba12be235f4b81754affe..95db203fa2f28ff8ed3f5f4d39bcddf80e5f3a5f 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.cpp
@@ -25,13 +25,14 @@
#include "ForceTorqueUnitSimulation.h"
-#include <ArmarXCore/core/util/StringHelpers.h>
-
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
+
namespace armarx
{
- void ForceTorqueUnitSimulation::onInitForceTorqueUnit()
+ void
+ ForceTorqueUnitSimulation::onInitForceTorqueUnit()
{
int intervalMs = getProperty<int>("IntervalMs").getValue();
@@ -41,29 +42,41 @@ namespace armarx
simox::alg::trim(sensor);
}
- for (std::vector<std::string>::iterator s = sensorNamesList.begin(); s != sensorNamesList.end(); s++)
+ for (std::vector<std::string>::iterator s = sensorNamesList.begin();
+ s != sensorNamesList.end();
+ s++)
{
- forces[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
- torques[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
+ forces[*s] = new armarx::FramedDirection(
+ Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
+ torques[*s] = new armarx::FramedDirection(
+ Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
}
- ARMARX_VERBOSE << "Starting ForceTorqueUnitSimulation with " << intervalMs << " ms interval";
- simulationTask = new PeriodicTask<ForceTorqueUnitSimulation>(this, &ForceTorqueUnitSimulation::simulationFunction, intervalMs, false, "ForceTorqueUnitSimulation", false);
+ ARMARX_VERBOSE << "Starting ForceTorqueUnitSimulation with " << intervalMs
+ << " ms interval";
+ simulationTask = new PeriodicTask<ForceTorqueUnitSimulation>(
+ this,
+ &ForceTorqueUnitSimulation::simulationFunction,
+ intervalMs,
+ false,
+ "ForceTorqueUnitSimulation",
+ false);
}
- void ForceTorqueUnitSimulation::onStartForceTorqueUnit()
+ void
+ ForceTorqueUnitSimulation::onStartForceTorqueUnit()
{
simulationTask->start();
}
-
- void ForceTorqueUnitSimulation::onExitForceTorqueUnit()
+ void
+ ForceTorqueUnitSimulation::onExitForceTorqueUnit()
{
simulationTask->stop();
}
-
- void ForceTorqueUnitSimulation::simulationFunction()
+ void
+ ForceTorqueUnitSimulation::simulationFunction()
{
for (auto& sensor : sensorNamesList)
@@ -75,19 +88,24 @@ namespace armarx
//listenerPrx->reportSensorValues(torques);
}
- void ForceTorqueUnitSimulation::setOffset(const FramedDirectionBasePtr& forceOffsets, const FramedDirectionBasePtr& torqueOffsets, const Ice::Current& c)
+ void
+ ForceTorqueUnitSimulation::setOffset(const FramedDirectionBasePtr& forceOffsets,
+ const FramedDirectionBasePtr& torqueOffsets,
+ const Ice::Current& c)
{
// Ignore
}
- void ForceTorqueUnitSimulation::setToNull(const Ice::Current& c)
+ void
+ ForceTorqueUnitSimulation::setToNull(const Ice::Current& c)
{
// Ignore
}
-
- PropertyDefinitionsPtr ForceTorqueUnitSimulation::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ ForceTorqueUnitSimulation::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new ForceTorqueUnitSimulationPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new ForceTorqueUnitSimulationPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
index cc3115aa4d96c40baab5a7aed418abbe813db229..74256977275e14430687d4e0b717abe3ee8f340d 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
+++ b/source/RobotAPI/components/units/ForceTorqueUnitSimulation.h
@@ -24,15 +24,16 @@
#pragma once
-#include "ForceTorqueUnit.h"
+#include <string>
+
+#include <IceUtil/Time.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <IceUtil/Time.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
-#include <string>
+#include "ForceTorqueUnit.h"
namespace armarx
{
@@ -40,16 +41,19 @@ namespace armarx
* \class ForceTorqueUnitSimulationPropertyDefinitions
* \brief
*/
- class ForceTorqueUnitSimulationPropertyDefinitions :
- public ForceTorqueUnitPropertyDefinitions
+ class ForceTorqueUnitSimulationPropertyDefinitions : public ForceTorqueUnitPropertyDefinitions
{
public:
ForceTorqueUnitSimulationPropertyDefinitions(std::string prefix) :
ForceTorqueUnitPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("SensorNames", "simulated sensor name. seperated by comma");
+ defineRequiredProperty<std::string>("SensorNames",
+ "simulated sensor name. seperated by comma");
defineRequiredProperty<std::string>("AgentName", "name of the robot agent");
- defineOptionalProperty<int>("IntervalMs", 50, "The time in milliseconds between two calls to the simulation method.");
+ defineOptionalProperty<int>(
+ "IntervalMs",
+ 50,
+ "The time in milliseconds between two calls to the simulation method.");
}
};
@@ -61,11 +65,11 @@ namespace armarx
* The unit is given a list of sensor names as a property. It then publishes force/torque values under these names.
* The published values will always be zero.
*/
- class ForceTorqueUnitSimulation :
- virtual public ForceTorqueUnit
+ class ForceTorqueUnitSimulation : virtual public ForceTorqueUnit
{
public:
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "ForceTorqueUnitSimulation";
}
@@ -79,7 +83,9 @@ namespace armarx
/**
* \warning Not implemented yet
*/
- void setOffset(const FramedDirectionBasePtr& forceOffsets, const FramedDirectionBasePtr& torqueOffsets, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setOffset(const FramedDirectionBasePtr& forceOffsets,
+ const FramedDirectionBasePtr& torqueOffsets,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \warning Not implemented yet
@@ -97,5 +103,4 @@ namespace armarx
Ice::StringSeq sensorNamesList;
PeriodicTask<ForceTorqueUnitSimulation>::pointer_type simulationTask;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/GamepadUnitObserver.cpp b/source/RobotAPI/components/units/GamepadUnitObserver.cpp
index b1848c5325d931a623c828e1dd58973121beedb5..51454e8c7233a5ccbae11cdc660b4e7cf6d252d7 100644
--- a/source/RobotAPI/components/units/GamepadUnitObserver.cpp
+++ b/source/RobotAPI/components/units/GamepadUnitObserver.cpp
@@ -23,19 +23,18 @@
*/
#include "GamepadUnitObserver.h"
-
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
namespace armarx
{
- void GamepadUnitObserver::onInitObserver()
+ void
+ GamepadUnitObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("GamepadTopicName").getValue());
@@ -47,27 +46,33 @@ namespace armarx
offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
}
-
-
- void GamepadUnitObserver::onConnectObserver()
+ void
+ GamepadUnitObserver::onConnectObserver()
{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(
+ getProperty<std::string>("DebugDrawerTopic").getValue());
}
-
- void GamepadUnitObserver::onExitObserver()
+ void
+ GamepadUnitObserver::onExitObserver()
{
debugDrawerPrx->removePoseVisu("IMU", "orientation");
debugDrawerPrx->removeLineVisu("IMU", "acceleration");
}
-
- PropertyDefinitionsPtr GamepadUnitObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ GamepadUnitObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new GamepadUnitObserverPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new GamepadUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
- void GamepadUnitObserver::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
+ void
+ GamepadUnitObserver::reportGamepadState(const std::string& device,
+ const std::string& name,
+ const GamepadData& data,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
@@ -81,28 +86,44 @@ namespace armarx
//ARMARX_IMPORTANT << deactivateSpam(1) << "observed " << device << " with name " << name;
//axis
- offerOrUpdateDataField(device, "leftStickX", Variant(data.leftStickX), "X value of the left analog stick");
- offerOrUpdateDataField(device, "leftStickY", Variant(data.leftStickY), "Y value of the left analog stick");
- offerOrUpdateDataField(device, "rightStickX", Variant(data.rightStickX), "X value of the right analog stick");
- offerOrUpdateDataField(device, "rightStickY", Variant(data.rightStickY), "Y value of the right analog stick");
+ offerOrUpdateDataField(
+ device, "leftStickX", Variant(data.leftStickX), "X value of the left analog stick");
+ offerOrUpdateDataField(
+ device, "leftStickY", Variant(data.leftStickY), "Y value of the left analog stick");
+ offerOrUpdateDataField(
+ device, "rightStickX", Variant(data.rightStickX), "X value of the right analog stick");
+ offerOrUpdateDataField(
+ device, "rightStickY", Variant(data.rightStickY), "Y value of the right analog stick");
offerOrUpdateDataField(device, "dPadX", Variant(data.dPadX), "X value of the digital pad");
offerOrUpdateDataField(device, "dPadY", Variant(data.dPadY), "y value of the digital pad");
- offerOrUpdateDataField(device, "leftTrigger", Variant(data.leftTrigger), "value of the left analog trigger");
- offerOrUpdateDataField(device, "rightTrigger", Variant(data.rightTrigger), "value of the right analog trigger");
+ offerOrUpdateDataField(
+ device, "leftTrigger", Variant(data.leftTrigger), "value of the left analog trigger");
+ offerOrUpdateDataField(device,
+ "rightTrigger",
+ Variant(data.rightTrigger),
+ "value of the right analog trigger");
//buttons
offerOrUpdateDataField(device, "aButton", Variant(data.aButton), "A button pressed");
- offerOrUpdateDataField(device, "backButton", Variant(data.backButton), "Back button pressed");
+ offerOrUpdateDataField(
+ device, "backButton", Variant(data.backButton), "Back button pressed");
offerOrUpdateDataField(device, "bButton", Variant(data.bButton), "B button pressed");
- offerOrUpdateDataField(device, "leftButton", Variant(data.leftButton), "Left shoulder button pressed");
- offerOrUpdateDataField(device, "leftStickButton", Variant(data.leftStickButton), "Left stick button pressed");
- offerOrUpdateDataField(device, "rightButton", Variant(data.rightButton), "Right shoulder button pressed");
- offerOrUpdateDataField(device, "rightStickButton", Variant(data.rightStickButton), "Right stick button pressed");
- offerOrUpdateDataField(device, "startButton", Variant(data.startButton), "Start button pressed");
- offerOrUpdateDataField(device, "theMiddleButton", Variant(data.theMiddleButton), "The middle button pressed");
+ offerOrUpdateDataField(
+ device, "leftButton", Variant(data.leftButton), "Left shoulder button pressed");
+ offerOrUpdateDataField(
+ device, "leftStickButton", Variant(data.leftStickButton), "Left stick button pressed");
+ offerOrUpdateDataField(
+ device, "rightButton", Variant(data.rightButton), "Right shoulder button pressed");
+ offerOrUpdateDataField(device,
+ "rightStickButton",
+ Variant(data.rightStickButton),
+ "Right stick button pressed");
+ offerOrUpdateDataField(
+ device, "startButton", Variant(data.startButton), "Start button pressed");
+ offerOrUpdateDataField(
+ device, "theMiddleButton", Variant(data.theMiddleButton), "The middle button pressed");
offerOrUpdateDataField(device, "xButton", Variant(data.xButton), "X button pressed");
offerOrUpdateDataField(device, "yButton", Variant(data.yButton), "Y button pressed");
updateChannel(device);
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/GamepadUnitObserver.h b/source/RobotAPI/components/units/GamepadUnitObserver.h
index 2f4d59c2a7a44ede66adaec1663be2263b6d25de..7cb39110a0a6c938783b7c3934921d2866fd2a85 100644
--- a/source/RobotAPI/components/units/GamepadUnitObserver.h
+++ b/source/RobotAPI/components/units/GamepadUnitObserver.h
@@ -24,32 +24,33 @@
#pragma once
-#include <RobotAPI/interface/units/GamepadUnit.h>
+#include <mutex>
+
#include <ArmarXCore/observers/Observer.h>
+
+#include <RobotAPI/interface/units/GamepadUnit.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <mutex>
-
namespace armarx
{
/**
* \class GamepadUnitObserverPropertyDefinitions
* \brief
*/
- class GamepadUnitObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class GamepadUnitObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- GamepadUnitObserverPropertyDefinitions(std::string prefix):
+ GamepadUnitObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
- defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ defineOptionalProperty<std::string>(
+ "GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
+ defineOptionalProperty<std::string>(
+ "DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
}
};
-
/**
* \class GamepadUnitObserver
* \ingroup RobotAPI-SensorActorUnits-observers
@@ -58,17 +59,19 @@ namespace armarx
* The GamepadUnitObserver monitors @IMU sensor values published by GamepadUnit-implementations and offers condition checks on these values.
* Available condition checks are: *updated*, *larger*, *equals* and *smaller*.
*/
- class GamepadUnitObserver :
- virtual public Observer,
- virtual public GamepadUnitObserverInterface
+ class GamepadUnitObserver : virtual public Observer, virtual public GamepadUnitObserverInterface
{
public:
- GamepadUnitObserver() {}
+ GamepadUnitObserver()
+ {
+ }
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "GamepadUnitObserver";
}
+
void onInitObserver() override;
void onConnectObserver() override;
void onExitObserver() override;
@@ -86,7 +89,10 @@ namespace armarx
// GamepadUnitListener interface
public:
- void reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c) override;
+ void reportGamepadState(const std::string& device,
+ const std::string& name,
+ const GamepadData& data,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c) override;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.h b/source/RobotAPI/components/units/GraspCandidateObserver.h
index 91169049b9376b41a8deaddf7f93ed9100979c28..6313b40f0bf2e400cc2fea14887dc76b27696de5 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.h
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.h
@@ -23,12 +23,13 @@
#pragma once
+#include <mutex>
+
#include <ArmarXCore/observers/Observer.h>
+
#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
-#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h>
-
-#include <mutex>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
namespace armarx
{
@@ -36,15 +37,19 @@ namespace armarx
* \class GraspCandidateObserverPropertyDefinitions
* \brief
*/
- class GraspCandidateObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class GraspCandidateObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- GraspCandidateObserverPropertyDefinitions(std::string prefix):
+ GraspCandidateObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("GraspCandidatesTopicName", "GraspCandidatesTopic", "Name of the Grasp Candidate Topic");
- defineOptionalProperty<std::string>("ConfigTopicName", "GraspCandidateProviderConfigTopic", "Name of the Grasp Candidate Provider Config Topic");
+ defineOptionalProperty<std::string>("GraspCandidatesTopicName",
+ "GraspCandidatesTopic",
+ "Name of the Grasp Candidate Topic");
+ defineOptionalProperty<std::string>(
+ "ConfigTopicName",
+ "GraspCandidateProviderConfigTopic",
+ "Name of the Grasp Candidate Provider Config Topic");
}
};
@@ -61,10 +66,12 @@ namespace armarx
GraspCandidateObserver();
// framework hooks
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "GraspCandidateObserver";
}
+
void onInitObserver() override;
void onConnectObserver() override;
@@ -74,42 +81,69 @@ namespace armarx
PropertyDefinitionsPtr createPropertyDefinitions() override;
public:
- static bool FilterMatches(const grasping::CandidateFilterConditionPtr& filter, const std::string& providerName, const grasping::GraspCandidatePtr& candidate);
+ static bool FilterMatches(const grasping::CandidateFilterConditionPtr& filter,
+ const std::string& providerName,
+ const grasping::GraspCandidatePtr& candidate);
static std::string ObjectTypeToString(objpose::ObjectType type);
// GraspCandidateProviderListener interface
public:
- void reportProviderInfo(const std::string& providerName, const grasping::ProviderInfoPtr& info, const ::Ice::Current& = Ice::emptyCurrent) override;
- void reportGraspCandidates(const std::string& providerName, const grasping::GraspCandidateSeq& candidates, const ::Ice::Current& = Ice::emptyCurrent) override;
- void reportBimanualGraspCandidates(const std::string& providerName, const grasping::BimanualGraspCandidateSeq& candidates, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void reportProviderInfo(const std::string& providerName,
+ const grasping::ProviderInfoPtr& info,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void reportGraspCandidates(const std::string& providerName,
+ const grasping::GraspCandidateSeq& candidates,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ void reportBimanualGraspCandidates(const std::string& providerName,
+ const grasping::BimanualGraspCandidateSeq& candidates,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
// GraspCandidateObserverInterface interface
public:
- grasping::InfoMap getAvailableProvidersWithInfo(const ::Ice::Current& = Ice::emptyCurrent) override;
+ grasping::InfoMap
+ getAvailableProvidersWithInfo(const ::Ice::Current& = Ice::emptyCurrent) override;
grasping::StringSeq getAvailableProviderNames(const ::Ice::Current&) override;
- grasping::ProviderInfoPtr getProviderInfo(const std::string& providerName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ grasping::ProviderInfoPtr
+ getProviderInfo(const std::string& providerName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
bool hasProvider(const std::string& providerName, const Ice::Current& c) override;
- grasping::GraspCandidateSeq getAllCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
- grasping::GraspCandidateSeq getCandidatesByProvider(const std::string& providerName, const Ice::Current& c = Ice::emptyCurrent) override;
- grasping::GraspCandidateSeq getCandidatesByProviders(const Ice::StringSeq& providerNames, const Ice::Current& c = Ice::emptyCurrent) override;
- grasping::GraspCandidateSeq getCandidatesByFilter(const grasping::CandidateFilterConditionPtr& filter, const ::Ice::Current& = Ice::emptyCurrent) override;
- Ice::Int getUpdateCounterByProvider(const std::string& providerName, const ::Ice::Current& = Ice::emptyCurrent) override;
+ grasping::GraspCandidateSeq
+ getAllCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
+ grasping::GraspCandidateSeq
+ getCandidatesByProvider(const std::string& providerName,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ grasping::GraspCandidateSeq
+ getCandidatesByProviders(const Ice::StringSeq& providerNames,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ grasping::GraspCandidateSeq
+ getCandidatesByFilter(const grasping::CandidateFilterConditionPtr& filter,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ Ice::Int getUpdateCounterByProvider(const std::string& providerName,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
grasping::IntMap getAllUpdateCounters(const Ice::Current& providerName) override;
- bool setProviderConfig(const std::string& providerName, const StringVariantBaseMap& config, const ::Ice::Current& = Ice::emptyCurrent) override;
+ bool setProviderConfig(const std::string& providerName,
+ const StringVariantBaseMap& config,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
- void setSelectedCandidates(const grasping::GraspCandidateSeq& candidates, const ::Ice::Current& = Ice::emptyCurrent) override;
- grasping::GraspCandidateSeq getSelectedCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setSelectedCandidates(const grasping::GraspCandidateSeq& candidates,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ grasping::GraspCandidateSeq
+ getSelectedCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
// bimanual stuff
- grasping::BimanualGraspCandidateSeq getAllBimanualCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
+ grasping::BimanualGraspCandidateSeq
+ getAllBimanualCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
// void setSelectedBimanualCandidates(::armarx::grasping::BimanualGraspCandidateSeq, const ::Ice::Current&) = 0;
- void setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates, const ::Ice::Current& = Ice::emptyCurrent) override;
- grasping::BimanualGraspCandidateSeq getSelectedBimanualCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
+ void setSelectedBimanualCandidates(const grasping::BimanualGraspCandidateSeq& candidates,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ grasping::BimanualGraspCandidateSeq
+ getSelectedBimanualCandidates(const ::Ice::Current& = Ice::emptyCurrent) override;
- void clearCandidatesByProvider(const std::string& providerName, const Ice::Current& c) override;
+ void clearCandidatesByProvider(const std::string& providerName,
+ const Ice::Current& c) override;
private:
bool hasProvider(const std::string& providerName);
@@ -132,5 +166,4 @@ namespace armarx
void handleProviderUpdate(const std::string& providerName, int candidateCount);
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/HandUnit.cpp b/source/RobotAPI/components/units/HandUnit.cpp
index 28cbd6157b2f03757dd0c6f476a9ea61caa48bbb..ee665cc409aaaa44079aabc88d4e0f173bb8ea24 100644
--- a/source/RobotAPI/components/units/HandUnit.cpp
+++ b/source/RobotAPI/components/units/HandUnit.cpp
@@ -28,8 +28,8 @@
#include <vector>
#include <VirtualRobot/EndEffector/EndEffector.h>
-#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
diff --git a/source/RobotAPI/components/units/HandUnitSimulation.cpp b/source/RobotAPI/components/units/HandUnitSimulation.cpp
index 40688ad93b793c04f90ad4c8b2f649870b13a797..5a5b984ccbf66cc89dde96fe9508550d2be05755 100644
--- a/source/RobotAPI/components/units/HandUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/HandUnitSimulation.cpp
@@ -25,22 +25,24 @@
#include "HandUnitSimulation.h"
-#include <Eigen/Geometry>
-
#include <cmath>
+#include <Eigen/Geometry>
+
#include <VirtualRobot/EndEffector/EndEffector.h>
#include <VirtualRobot/RobotConfig.h>
namespace armarx
{
- void HandUnitSimulation::onInitHandUnit()
+ void
+ HandUnitSimulation::onInitHandUnit()
{
kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
usingProxy(kinematicUnitName);
}
- void HandUnitSimulation::onStartHandUnit()
+ void
+ HandUnitSimulation::onStartHandUnit()
{
kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
if (!kinematicUnitPrx)
@@ -49,16 +51,20 @@ namespace armarx
}
}
- void HandUnitSimulation::onExitHandUnit()
+ void
+ HandUnitSimulation::onExitHandUnit()
{
}
- PropertyDefinitionsPtr HandUnitSimulation::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ HandUnitSimulation::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new HandUnitSimulationPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new HandUnitSimulationPropertyDefinitions(getConfigIdentifier()));
}
- void armarx::HandUnitSimulation::setShape(const std::string& shapeName, const Ice::Current&)
+ void
+ armarx::HandUnitSimulation::setShape(const std::string& shapeName, const Ice::Current&)
{
std::string myShapeName = shapeName;
ARMARX_INFO << "Setting shape " << myShapeName;
@@ -72,7 +78,8 @@ namespace armarx
if (!eef->hasPreshape(myShapeName))
{
- ARMARX_INFO << "Shape with name " << myShapeName << " not known in eef " << eef->getName() << ". Looking for partial match";
+ ARMARX_INFO << "Shape with name " << myShapeName << " not known in eef "
+ << eef->getName() << ". Looking for partial match";
bool foundMatch = false;
@@ -89,13 +96,14 @@ namespace armarx
if (!foundMatch)
{
- ARMARX_WARNING << "No match found for " << myShapeName << " in eef " << eef->getName() << " available shapes: " << eef->getPreshapes();
+ ARMARX_WARNING << "No match found for " << myShapeName << " in eef "
+ << eef->getName() << " available shapes: " << eef->getPreshapes();
return;
}
}
VirtualRobot::RobotConfigPtr config = eef->getPreshape(myShapeName);
- std::map < std::string, float > jointAngles = config->getRobotNodeJointValueMap();
+ std::map<std::string, float> jointAngles = config->getRobotNodeJointValueMap();
NameControlModeMap controlModes;
@@ -108,7 +116,8 @@ namespace armarx
kinematicUnitPrx->setJointAngles(jointAngles);
}
- void armarx::HandUnitSimulation::setJointAngles(const NameValueMap& jointAngles, const Ice::Current&)
+ void
+ armarx::HandUnitSimulation::setJointAngles(const NameValueMap& jointAngles, const Ice::Current&)
{
NameControlModeMap controlModes;
@@ -120,4 +129,4 @@ namespace armarx
kinematicUnitPrx->switchControlMode(controlModes);
kinematicUnitPrx->setJointAngles(jointAngles);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/HandUnitSimulation.h b/source/RobotAPI/components/units/HandUnitSimulation.h
index d5fff658f3e0c76a56669ef8e1b3fbddfd6a4d22..ede37a7194d54023ffb390a13e51d479e0d8c3ab 100644
--- a/source/RobotAPI/components/units/HandUnitSimulation.h
+++ b/source/RobotAPI/components/units/HandUnitSimulation.h
@@ -33,18 +33,17 @@ namespace armarx
* \class HandUnitSimulationPropertyDefinitions
* \brief Defines all necessary properties for armarx::HandUnitSimulation
*/
- class HandUnitSimulationPropertyDefinitions:
- public HandUnitPropertyDefinitions
+ class HandUnitSimulationPropertyDefinitions : public HandUnitPropertyDefinitions
{
public:
- HandUnitSimulationPropertyDefinitions(std::string prefix):
+ HandUnitSimulationPropertyDefinitions(std::string prefix) :
HandUnitPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("KinematicUnitName", "Name of the kinematic unit that should be used");
+ defineRequiredProperty<std::string>("KinematicUnitName",
+ "Name of the kinematic unit that should be used");
}
};
-
/**
* \class HandUnitSimulation
* \ingroup RobotAPI-SensorActorUnits-simulation
@@ -53,12 +52,12 @@ namespace armarx
* An instance of a HandUnitSimulation provides means to open, close, and shape hands.
* It uses the HandUnitListener Ice interface to report updates of its current state
*/
- class HandUnitSimulation :
- virtual public HandUnit
+ class HandUnitSimulation : virtual public HandUnit
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "HandUnitSimulation";
}
@@ -72,12 +71,14 @@ namespace armarx
*
* \warning Not implemented yet!
*/
- void setShape(const std::string& shapeName, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setShape(const std::string& shapeName,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \warning Not implemented yet!
*/
- void setJointAngles(const NameValueMap& jointAngles, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointAngles(const NameValueMap& jointAngles,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \see armarx::PropertyUser::createPropertyDefinitions()
@@ -87,5 +88,4 @@ namespace armarx
protected:
KinematicUnitInterfacePrx kinematicUnitPrx;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/HapticObserver.cpp b/source/RobotAPI/components/units/HapticObserver.cpp
index 0fe52df490098255ef86b56bd43025017a2997b5..2748edb6c76b00020cc267c1048cd20f48d9e454 100644
--- a/source/RobotAPI/components/units/HapticObserver.cpp
+++ b/source/RobotAPI/components/units/HapticObserver.cpp
@@ -23,30 +23,34 @@
*/
#include "HapticObserver.h"
-#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <Eigen/Dense>
+
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
-#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
-#include <Eigen/Dense>
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
namespace armarx
{
HapticObserver::HapticObserver()
{
- statisticsTask = new PeriodicTask<HapticObserver>(this, &HapticObserver::updateStatistics, 10, false);
+ statisticsTask =
+ new PeriodicTask<HapticObserver>(this, &HapticObserver::updateStatistics, 10, false);
}
- void HapticObserver::setTopicName(std::string topicName)
+ void
+ HapticObserver::setTopicName(std::string topicName)
{
this->topicName = topicName;
}
- void HapticObserver::onInitObserver()
+ void
+ HapticObserver::onInitObserver()
{
if (topicName.empty())
{
@@ -64,17 +68,24 @@ namespace armarx
offerConditionCheck("smaller", new ConditionCheckSmaller());
}
- void HapticObserver::onConnectObserver()
+ void
+ HapticObserver::onConnectObserver()
{
statisticsTask->start();
}
- void HapticObserver::onExitObserver()
+ void
+ HapticObserver::onExitObserver()
{
statisticsTask->stop();
}
- void HapticObserver::reportSensorValues(const std::string& device, const std::string& name, const armarx::MatrixFloatBasePtr& values, const armarx::TimestampBasePtr& timestamp, const Ice::Current&)
+ void
+ HapticObserver::reportSensorValues(const std::string& device,
+ const std::string& name,
+ const armarx::MatrixFloatBasePtr& values,
+ const armarx::TimestampBasePtr& timestamp,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
@@ -95,8 +106,10 @@ namespace armarx
if (!existsChannel(channelName))
{
offerChannel(channelName, "Haptic data");
- offerDataFieldWithDefault(channelName, "device", Variant(device), "Device of the tactile sensor");
- offerDataFieldWithDefault(channelName, "name", Variant(name), "Name of the tactile sensor");
+ offerDataFieldWithDefault(
+ channelName, "device", Variant(device), "Device of the tactile sensor");
+ offerDataFieldWithDefault(
+ channelName, "name", Variant(name), "Name of the tactile sensor");
offerDataFieldWithDefault(channelName, "matrix", matrix, "Raw tactile matrix data");
offerDataFieldWithDefault(channelName, "max", Variant(max), "Maximum value");
offerDataFieldWithDefault(channelName, "mean", Variant(mean), "Mean value");
@@ -109,7 +122,8 @@ namespace armarx
{
std::stringstream s;
s << "entry_" << i << "," << j;
- offerDataFieldWithDefault(channelName, s.str(), Variant(M(i, j)), "Individual matrix entry");
+ offerDataFieldWithDefault(
+ channelName, s.str(), Variant(M(i, j)), "Individual matrix entry");
}
}
@@ -133,7 +147,6 @@ namespace armarx
setDataField(channelName, s.str(), Variant(M(i, j)));
}
}
-
}
/*if(statistics.count(device) > 0)
@@ -152,12 +165,14 @@ namespace armarx
updateChannel(channelName);
}
- PropertyDefinitionsPtr HapticObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ HapticObserver::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new HapticObserverPropertyDefinitions(getConfigIdentifier()));
}
- void HapticObserver::updateStatistics()
+ void
+ HapticObserver::updateStatistics()
{
/*std::unique_lock lock(dataMutex);
//ARMARX_LOG << "updateStatistics";
@@ -172,4 +187,4 @@ namespace armarx
updateChannel(device);
}*/
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/HapticObserver.h b/source/RobotAPI/components/units/HapticObserver.h
index cac9a85c903e4c83125489f97c8c65dd3b368ff5..00c6dfacd04e4afea71fe278d2111712e2993a15 100644
--- a/source/RobotAPI/components/units/HapticObserver.h
+++ b/source/RobotAPI/components/units/HapticObserver.h
@@ -24,13 +24,14 @@
#pragma once
-#include <RobotAPI/interface/units/HapticUnit.h>
+#include <mutex>
+
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/observers/Observer.h>
-#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
#include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
-#include <mutex>
+#include <RobotAPI/interface/units/HapticUnit.h>
namespace armarx
{
@@ -38,14 +39,13 @@ namespace armarx
* \class HapticObserverPropertyDefinitions
* \brief
*/
- class HapticObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class HapticObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- HapticObserverPropertyDefinitions(std::string prefix):
- ObserverPropertyDefinitions(prefix)
+ HapticObserverPropertyDefinitions(std::string prefix) : ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("HapticTopicName", "HapticValues", "Name of the HapticUnit Topic");
+ defineOptionalProperty<std::string>(
+ "HapticTopicName", "HapticValues", "Name of the HapticUnit Topic");
}
};
@@ -63,7 +63,8 @@ namespace armarx
this->pos = 0;
}
- void add(long timestamp)
+ void
+ add(long timestamp)
{
long delta = timestamp - lastTimestamp;
@@ -90,7 +91,8 @@ namespace armarx
return sum / (deltas.size() + 1);
}*/
- long average()
+ long
+ average()
{
if (deltas.size() == 0)
{
@@ -122,9 +124,7 @@ namespace armarx
* The HapticObserver monitors haptic sensor values published by HapticUnit-implementations and offers condition checks on these values.
* Available condition checks are: *updated*, *larger*, *equals* and *smaller*.
*/
- class HapticObserver :
- virtual public Observer,
- virtual public HapticUnitObserverInterface
+ class HapticObserver : virtual public Observer, virtual public HapticUnitObserverInterface
{
public:
HapticObserver();
@@ -132,20 +132,27 @@ namespace armarx
void setTopicName(std::string topicName);
// framework hooks
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "HapticUnitObserver";
}
+
void onInitObserver() override;
void onConnectObserver() override;
void onExitObserver() override;
- void reportSensorValues(const ::std::string& device, const ::std::string& name, const ::armarx::MatrixFloatBasePtr& values, const ::armarx::TimestampBasePtr& timestamp, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void reportSensorValues(const ::std::string& device,
+ const ::std::string& name,
+ const ::armarx::MatrixFloatBasePtr& values,
+ const ::armarx::TimestampBasePtr& timestamp,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
/**
* \see PropertyUser::createPropertyDefinitions()
*/
PropertyDefinitionsPtr createPropertyDefinitions() override;
+
private:
std::mutex dataMutex;
std::string topicName;
@@ -154,7 +161,5 @@ namespace armarx
void updateStatistics();
std::map<std::string, HapticSampleStatistics> statistics;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/HapticUnit.cpp b/source/RobotAPI/components/units/HapticUnit.cpp
index 4f1c3a103bb2e37eddcf8f76332080b576ee6ae9..31735139cedf4874fd1a909d411167965f8bf934 100644
--- a/source/RobotAPI/components/units/HapticUnit.cpp
+++ b/source/RobotAPI/components/units/HapticUnit.cpp
@@ -26,25 +26,30 @@
namespace armarx
{
- void HapticUnit::onInitComponent()
+ void
+ HapticUnit::onInitComponent()
{
offeringTopic(getProperty<std::string>("HapticTopicName").getValue());
onInitHapticUnit();
}
- void HapticUnit::onConnectComponent()
+ void
+ HapticUnit::onConnectComponent()
{
- hapticTopicPrx = getTopic<HapticUnitListenerPrx>(getProperty<std::string>("HapticTopicName").getValue());
+ hapticTopicPrx =
+ getTopic<HapticUnitListenerPrx>(getProperty<std::string>("HapticTopicName").getValue());
onStartHapticUnit();
}
- void HapticUnit::onExitComponent()
+ void
+ HapticUnit::onExitComponent()
{
onExitHapticUnit();
}
- PropertyDefinitionsPtr HapticUnit::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ HapticUnit::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new HapticUnitPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/HapticUnit.h b/source/RobotAPI/components/units/HapticUnit.h
index 26050b48062569e7d154ee2d82e6683e90a67589..a8fcffa5ea51125c3711ad415c93d92ce6092126 100644
--- a/source/RobotAPI/components/units/HapticUnit.h
+++ b/source/RobotAPI/components/units/HapticUnit.h
@@ -24,15 +24,15 @@
#pragma once
+#include <string>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/components/units/SensorActorUnit.h>
+#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/units/HapticUnit.h>
-#include <string>
-
namespace armarx
{
/**
@@ -42,10 +42,10 @@ namespace armarx
class HapticUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- HapticUnitPropertyDefinitions(std::string prefix) :
- ComponentPropertyDefinitions(prefix)
+ HapticUnitPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("HapticTopicName", "HapticValues", "Name of the Haptic Topic.");
+ defineOptionalProperty<std::string>(
+ "HapticTopicName", "HapticValues", "Name of the Haptic Topic.");
}
};
@@ -59,12 +59,11 @@ namespace armarx
* @ingroup Component-HapticUnit
* @brief The HapticUnit class
*/
- class HapticUnit :
- virtual public HapticUnitInterface,
- virtual public SensorActorUnit
+ class HapticUnit : virtual public HapticUnitInterface, virtual public SensorActorUnit
{
public:
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "HapticUnit";
}
@@ -82,4 +81,4 @@ namespace armarx
protected:
HapticUnitListenerPrx hapticTopicPrx;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 1bbf0abff5599b23ff0ca01acaa4087bdeac1704..e52a9d85baa48af10382f144310e178be64e91a9 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -23,31 +23,26 @@
*/
#include "HeadIKUnit.h"
+#include <memory>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/util/StringHelpers.h>
-
-#include <VirtualRobot/XML/RobotIO.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/IK/GazeIK.h>
#include <VirtualRobot/LinkedCoordinate.h>
+#include <VirtualRobot/XML/RobotIO.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-#include <memory>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
namespace armarx
{
- HeadIKUnit::HeadIKUnit() :
- requested(false),
- cycleTime(30),
- newTargetSet(false)
+ HeadIKUnit::HeadIKUnit() : requested(false), cycleTime(30), newTargetSet(false)
{
targetPosition = new FramedPosition();
}
-
- void HeadIKUnit::onInitComponent()
+ void
+ HeadIKUnit::onInitComponent()
{
std::unique_lock lock(accessMutex);
@@ -55,29 +50,32 @@ namespace armarx
usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
-
cycleTime = getProperty<int>("CycleTime").getValue();
offeringTopic("DebugDrawerUpdates");
offeringTopic(getProperty<std::string>("HeadIKUnitTopicName").getValue());
}
-
- void HeadIKUnit::onConnectComponent()
+ void
+ HeadIKUnit::onConnectComponent()
{
std::unique_lock lock(accessMutex);
drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
- kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
- robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
+ kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(
+ getProperty<std::string>("KinematicUnitName").getValue());
+ robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(
+ getProperty<std::string>("RobotStateComponentName").getValue());
//remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
- localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ localRobot = RemoteRobot::createLocalCloneFromFile(
+ robotStateComponentPrx, VirtualRobot::RobotIO::RobotDescription::eStructure);
// localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
- headIKUnitListener = getTopic<armarx::HeadIKUnitListenerPrx>(getProperty<std::string>("HeadIKUnitTopicName").getValue());
+ headIKUnitListener = getTopic<armarx::HeadIKUnitListenerPrx>(
+ getProperty<std::string>("HeadIKUnitTopicName").getValue());
//std::string robotModelFile;
//ArmarXDataPath::getAbsolutePath("Armar3/robotmodel/ArmarIII.xml", robotModelFile);
@@ -92,13 +90,14 @@ namespace armarx
execTask->stop();
}
- execTask = new PeriodicTask<HeadIKUnit>(this, &HeadIKUnit::periodicExec, cycleTime, false, "HeadIKCalculator");
+ execTask = new PeriodicTask<HeadIKUnit>(
+ this, &HeadIKUnit::periodicExec, cycleTime, false, "HeadIKCalculator");
execTask->setDelayWarningTolerance(300);
execTask->start();
-
}
- void HeadIKUnit::onDisconnectComponent()
+ void
+ HeadIKUnit::onDisconnectComponent()
{
release();
@@ -110,20 +109,19 @@ namespace armarx
}
-
if (execTask)
{
execTask->stop();
}
}
- void HeadIKUnit::onExitComponent()
+ void
+ HeadIKUnit::onExitComponent()
{
}
-
-
- void HeadIKUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
+ void
+ HeadIKUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
{
std::unique_lock lock(accessMutex);
@@ -135,8 +133,10 @@ namespace armarx
}
}
-
- void HeadIKUnit::setHeadTarget(const std::string& robotNodeSetName, const FramedPositionBasePtr& targetPosition, const Ice::Current& c)
+ void
+ HeadIKUnit::setHeadTarget(const std::string& robotNodeSetName,
+ const FramedPositionBasePtr& targetPosition,
+ const Ice::Current& c)
{
std::unique_lock lock(accessMutex);
@@ -150,16 +150,14 @@ namespace armarx
this->targetPosition->z = targetPosition->z;
this->targetPosition->frame = targetPosition->frame;
- FramedPositionPtr globalTarget = FramedPositionPtr::dynamicCast(targetPosition)->toGlobal(robotStateComponentPrx->getSynchronizedRobot());
+ FramedPositionPtr globalTarget =
+ FramedPositionPtr::dynamicCast(targetPosition)
+ ->toGlobal(robotStateComponentPrx->getSynchronizedRobot());
if (drawer && getProperty<bool>("VisualizeIKTarget").getValue())
{
- drawer->setSphereDebugLayerVisu("HeadViewTarget",
- globalTarget,
- DrawColor {1, 0, 0, 0.7},
- 15);
-
-
+ drawer->setSphereDebugLayerVisu(
+ "HeadViewTarget", globalTarget, DrawColor{1, 0, 0, 0.7}, 15);
}
ARMARX_DEBUG << "new Head target set: " << *globalTarget << " for " << robotNodeSetName;
@@ -167,22 +165,23 @@ namespace armarx
newTargetSet = true;
}
-
-
-
- void HeadIKUnit::init(const Ice::Current& c)
+ void
+ HeadIKUnit::init(const Ice::Current& c)
{
}
- void HeadIKUnit::start(const Ice::Current& c)
+ void
+ HeadIKUnit::start(const Ice::Current& c)
{
}
- void HeadIKUnit::stop(const Ice::Current& c)
+ void
+ HeadIKUnit::stop(const Ice::Current& c)
{
}
- UnitExecutionState HeadIKUnit::getExecutionState(const Ice::Current& c)
+ UnitExecutionState
+ HeadIKUnit::getExecutionState(const Ice::Current& c)
{
switch (getState())
{
@@ -202,10 +201,8 @@ namespace armarx
}
}
-
-
-
- void HeadIKUnit::request(const Ice::Current& c)
+ void
+ HeadIKUnit::request(const Ice::Current& c)
{
std::unique_lock lock(accessMutex);
@@ -217,15 +214,14 @@ namespace armarx
execTask->stop();
}
- execTask = new PeriodicTask<HeadIKUnit>(this, &HeadIKUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
+ execTask = new PeriodicTask<HeadIKUnit>(
+ this, &HeadIKUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
execTask->start();
ARMARX_IMPORTANT << "Requested HeadIKUnit";
}
-
-
-
- void HeadIKUnit::release(const Ice::Current& c)
+ void
+ HeadIKUnit::release(const Ice::Current& c)
{
std::unique_lock lock(accessMutex);
@@ -251,10 +247,8 @@ namespace armarx
ARMARX_INFO << "Released HeadIKUnit";
}
-
-
-
- void HeadIKUnit::periodicExec()
+ void
+ HeadIKUnit::periodicExec()
{
bool doCalculation = false;
@@ -307,10 +301,13 @@ namespace armarx
auto tcpNode = kinematicChain->getTCP();
VirtualRobot::RobotNodePrismaticPtr virtualPrismaticJoint;
- virtualPrismaticJoint = std::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
+ virtualPrismaticJoint =
+ std::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
if (!virtualPrismaticJoint)
{
- ARMARX_WARNING << deactivateSpam(10, robotNodeSetName) << "Head IK Kinematic Chain TCP is not a prismatic joint! skipping joint set";
+ ARMARX_WARNING << deactivateSpam(10, robotNodeSetName)
+ << "Head IK Kinematic Chain TCP is not a prismatic joint! "
+ "skipping joint set";
continue;
}
@@ -325,7 +322,8 @@ namespace armarx
}
}
- ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualPrismaticJoint->getName()) << " " << VAROUT(kinematicChain->getName());
+ ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualPrismaticJoint->getName()) << " "
+ << VAROUT(kinematicChain->getName());
VirtualRobot::GazeIK ikSolver(kinematicChain, virtualPrismaticJoint);
ikSolver.enableJointLimitAvoidance(true);
ikSolver.setup(10, 30, 20);
@@ -339,9 +337,12 @@ namespace armarx
if (duration.toMilliSeconds() > 500)
{
- ARMARX_INFO << "Calculating Gaze IK took " << duration.toMilliSeconds() << " ms";
+ ARMARX_INFO << "Calculating Gaze IK took " << duration.toMilliSeconds()
+ << " ms";
}
- Eigen::Vector3f position = globalPos->toEigen() - kinematicChain->getTCP()->getGlobalPose().block(0, 3, 3, 1);
+ Eigen::Vector3f position =
+ globalPos->toEigen() -
+ kinematicChain->getTCP()->getGlobalPose().block(0, 3, 3, 1);
error = position.norm();
if (!solutionFound)
{
@@ -350,7 +351,8 @@ namespace armarx
{
foundSolution = true;
selectedRobotNodeSetName = robotNodeSetName;
- ARMARX_INFO << "IKSolver found no solution! applying best solution with " << error << "mm error";
+ ARMARX_INFO << "IKSolver found no solution! applying best solution with "
+ << error << "mm error";
break;
}
}
@@ -366,30 +368,32 @@ namespace armarx
ARMARX_WARNING << "IKSolver found no solution! " << error << "mm error";
return;
}
- ARMARX_DEBUG << "Solution found with " << selectedRobotNodeSetName << " - remaining error: " << error << " mm";
+ ARMARX_DEBUG << "Solution found with " << selectedRobotNodeSetName
+ << " - remaining error: " << error << " mm";
- if (drawer && localRobot->hasRobotNode("Cameras") && getProperty<bool>("VisualizeIKTarget").getValue())
+ if (drawer && localRobot->hasRobotNode("Cameras") &&
+ getProperty<bool>("VisualizeIKTarget").getValue())
{
- Vector3Ptr startPos = new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose());
- drawer->setSphereDebugLayerVisu("HeadViewTargetSolution",
- startPos,
- DrawColor {0, 1, 1, 0.2},
- 17);
+ Vector3Ptr startPos =
+ new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose());
+ drawer->setSphereDebugLayerVisu(
+ "HeadViewTargetSolution", startPos, DrawColor{0, 1, 1, 0.2}, 17);
}
auto tcpNode = kinematicChain->getTCP();
for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
{
if (kinematicChain->getNode(i) != tcpNode)
{
- targetJointAngles[kinematicChain->getNode(i)->getName()] = kinematicChain->getNode(i)->getJointValue();
+ targetJointAngles[kinematicChain->getNode(i)->getName()] =
+ kinematicChain->getNode(i)->getJointValue();
controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
}
- ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": " << kinematicChain->getNode(i)->getJointValue();
+ ARMARX_DEBUG << kinematicChain->getNode(i)->getName() << ": "
+ << kinematicChain->getNode(i)->getJointValue();
}
-
if (headIKUnitListener)
{
headIKUnitListener->reportHeadTargetChanged(targetJointAngles, globalPos);
@@ -405,18 +409,14 @@ namespace armarx
{
ARMARX_IMPORTANT << "setJointAngles failed";
}
-
}
}
-
-
-
- PropertyDefinitionsPtr HeadIKUnit::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ HeadIKUnit::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new HeadIKUnitPropertyDefinitions(getConfigIdentifier()));
}
-
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/HeadIKUnit.h b/source/RobotAPI/components/units/HeadIKUnit.h
index b814b519aae4f377a55e0610759e52663007ad50..b74da3121cd573d062d10ca13380103ec3efa634 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.h
+++ b/source/RobotAPI/components/units/HeadIKUnit.h
@@ -23,34 +23,38 @@
#pragma once
+#include <mutex>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+
+#include <RobotAPI/interface/units/HeadIKUnit.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/interface/units/HeadIKUnit.h>
-
-#include <mutex>
-
-
namespace armarx
{
/**
* \class HeadIKUnitPropertyDefinitions
* \brief
*/
- class HeadIKUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class HeadIKUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- HeadIKUnitPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ HeadIKUnitPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit Proxy");
- defineOptionalProperty<std::string>("HeadIKUnitTopicName", "HeadIKUnitTopic", "Name of the HeadIKUnit Topic");
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
- defineOptionalProperty<bool>("VisualizeIKTarget", true, "Visualize the current IK target using the debug drawer");
+ defineRequiredProperty<std::string>("KinematicUnitName",
+ "Name of the KinematicUnit Proxy");
+ defineOptionalProperty<std::string>(
+ "HeadIKUnitTopicName", "HeadIKUnitTopic", "Name of the HeadIKUnit Topic");
+ defineOptionalProperty<std::string>(
+ "RobotStateComponentName",
+ "RobotStateComponent",
+ "Name of the RobotStateComponent that should be used");
+ defineOptionalProperty<bool>("VisualizeIKTarget",
+ true,
+ "Visualize the current IK target using the debug drawer");
defineOptionalProperty<int>("CycleTime", 30, "Cycle time of the tcp control in ms");
}
};
@@ -73,14 +77,19 @@ namespace armarx
void onConnectComponent() override;
void onDisconnectComponent() override;
void onExitComponent() override;
- std::string getDefaultName() const override
+
+ std::string
+ getDefaultName() const override
{
return "HeadIKUnit";
}
// HeadIKUnitInterface interface
- void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::emptyCurrent) override;
- void setHeadTarget(const std::string& robotNodeSetName, const FramedPositionBasePtr& targetPosition, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setCycleTime(Ice::Int milliseconds,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void setHeadTarget(const std::string& robotNodeSetName,
+ const FramedPositionBasePtr& targetPosition,
+ const Ice::Current& c = Ice::emptyCurrent) override;
// UnitExecutionManagementInterface interface
void init(const Ice::Current& c = Ice::emptyCurrent) override;
@@ -117,6 +126,4 @@ namespace armarx
armarx::HeadIKUnitListenerPrx headIKUnitListener;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnit.cpp b/source/RobotAPI/components/units/InertialMeasurementUnit.cpp
index 5b4610fb2a1f1bc7694845f444955491e066b2b1..9875642e8c49ea572154c0082ae17ca17ffd79f5 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnit.cpp
+++ b/source/RobotAPI/components/units/InertialMeasurementUnit.cpp
@@ -24,36 +24,38 @@
#include "InertialMeasurementUnit.h"
-
namespace armarx
{
- void InertialMeasurementUnit::onInitComponent()
+ void
+ InertialMeasurementUnit::onInitComponent()
{
offeringTopic(getProperty<std::string>("IMUTopicName").getValue());
onInitIMU();
}
-
- void InertialMeasurementUnit::onConnectComponent()
+ void
+ InertialMeasurementUnit::onConnectComponent()
{
- IMUTopicPrx = getTopic<InertialMeasurementUnitListenerPrx>(getProperty<std::string>("IMUTopicName").getValue());
+ IMUTopicPrx = getTopic<InertialMeasurementUnitListenerPrx>(
+ getProperty<std::string>("IMUTopicName").getValue());
onStartIMU();
}
-
- void InertialMeasurementUnit::onDisconnectComponent()
+ void
+ InertialMeasurementUnit::onDisconnectComponent()
{
}
-
- void InertialMeasurementUnit::onExitComponent()
+ void
+ InertialMeasurementUnit::onExitComponent()
{
onExitIMU();
}
- PropertyDefinitionsPtr InertialMeasurementUnit::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ InertialMeasurementUnit::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new InertialMeasurementUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new InertialMeasurementUnitPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnit.h b/source/RobotAPI/components/units/InertialMeasurementUnit.h
index 2e5e956ad4944065fcd8a4457242b8f8d6901ca3..4166522ef9d29d257f5454b9f3262b5978c3d62c 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnit.h
+++ b/source/RobotAPI/components/units/InertialMeasurementUnit.h
@@ -25,25 +25,26 @@
#pragma once
-#include "SensorActorUnit.h"
-
#include <ArmarXCore/core/Component.h>
+
#include <RobotAPI/interface/units/InertialMeasurementUnit.h>
+#include "SensorActorUnit.h"
+
namespace armarx
{
/**
* \class InertialMeasurementUnitPropertyDefinitions
* \brief
*/
- class InertialMeasurementUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class InertialMeasurementUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- InertialMeasurementUnitPropertyDefinitions(std::string prefix):
+ InertialMeasurementUnitPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("IMUTopicName", "IMUValues", "Name of the IMU Topic.");
+ defineOptionalProperty<std::string>(
+ "IMUTopicName", "IMUValues", "Name of the IMU Topic.");
}
};
@@ -60,7 +61,8 @@ namespace armarx
/**
* \see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "InertialMeasurementUnit";
}
@@ -100,5 +102,4 @@ namespace armarx
InertialMeasurementUnitListenerPrx IMUTopicPrx;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
index dc73454fe838fafeddebbbfca0915efaca5752f0..f070fe122594dbfbc0aa6ada78dc32ab5e243880 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
@@ -32,10 +32,10 @@
#include <RobotAPI/libraries/core/Pose.h>
-
namespace armarx
{
- void InertialMeasurementUnitObserver::onInitObserver()
+ void
+ InertialMeasurementUnitObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("IMUTopicName").getValue());
@@ -50,15 +50,18 @@ namespace armarx
}
}
- void InertialMeasurementUnitObserver::onConnectObserver()
+ void
+ InertialMeasurementUnitObserver::onConnectObserver()
{
if (getProperty<bool>("EnableVisualization").getValue())
{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(
+ getProperty<std::string>("DebugDrawerTopic").getValue());
}
}
- void InertialMeasurementUnitObserver::onExitObserver()
+ void
+ InertialMeasurementUnitObserver::onExitObserver()
{
if (getProperty<bool>("EnableVisualization").getValue())
{
@@ -67,9 +70,12 @@ namespace armarx
}
}
- void InertialMeasurementUnitObserver::reportSensorValues(
- const std::string& device, const std::string& name, const IMUData& values,
- const TimestampBasePtr& timestamp, const Ice::Current&)
+ void
+ InertialMeasurementUnitObserver::reportSensorValues(const std::string& device,
+ const std::string& name,
+ const IMUData& values,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
@@ -87,34 +93,45 @@ namespace armarx
if (values.acceleration.size() > 0)
{
ARMARX_CHECK_EXPRESSION(values.acceleration.size() == 3);
- acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
+ acceleration = new Vector3(
+ values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
offerValue(device, "acceleration", acceleration);
}
if (values.gyroscopeRotation.size() > 0)
{
ARMARX_CHECK_EXPRESSION(values.gyroscopeRotation.size() == 3);
- Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
+ Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0),
+ values.gyroscopeRotation.at(1),
+ values.gyroscopeRotation.at(2));
offerValue(device, "gyroscopeRotation", gyroscopeRotation);
}
if (values.magneticRotation.size() > 0)
{
ARMARX_CHECK_EXPRESSION(values.magneticRotation.size() == 3);
- Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
+ Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0),
+ values.magneticRotation.at(1),
+ values.magneticRotation.at(2));
offerValue(device, "magneticRotation", magneticRotation);
}
if (values.orientationQuaternion.size() > 0)
{
ARMARX_CHECK_EXPRESSION(values.orientationQuaternion.size() == 4);
- orientationQuaternion = new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
- offerOrUpdateDataField(device, "orientationQuaternion", orientationQuaternion, "orientation quaternion values");
+ orientationQuaternion = new Quaternion(values.orientationQuaternion.at(0),
+ values.orientationQuaternion.at(1),
+ values.orientationQuaternion.at(2),
+ values.orientationQuaternion.at(3));
+ offerOrUpdateDataField(device,
+ "orientationQuaternion",
+ orientationQuaternion,
+ "orientation quaternion values");
}
offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
updateChannel(device);
-
- if (orientationQuaternion && acceleration && getProperty<bool>("EnableVisualization").getValue())
+ if (orientationQuaternion && acceleration &&
+ getProperty<bool>("EnableVisualization").getValue())
{
Eigen::Vector3f zero;
zero.setZero();
@@ -128,7 +145,8 @@ namespace armarx
Eigen::Vector3f ac = acceleration->toEigen();
ac *= 10;
- debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
+ debugDrawerPrx->setLineVisu(
+ "IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
@@ -136,7 +154,10 @@ namespace armarx
}
}
- void InertialMeasurementUnitObserver::offerValue(const std::string& device, const std::string& fieldName, const Vector3Ptr& vec)
+ void
+ InertialMeasurementUnitObserver::offerValue(const std::string& device,
+ const std::string& fieldName,
+ const Vector3Ptr& vec)
{
offerOrUpdateDataField(device, fieldName, vec, fieldName + " values");
offerOrUpdateDataField(device, fieldName + "_x", vec->x, fieldName + "_x value");
@@ -144,8 +165,10 @@ namespace armarx
offerOrUpdateDataField(device, fieldName + "_z", vec->z, fieldName + "_z value");
}
- PropertyDefinitionsPtr InertialMeasurementUnitObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ InertialMeasurementUnitObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new InertialMeasurementUnitObserverPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new InertialMeasurementUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
index 728e2dc385906114fa09c2b7b044990196a94a2a..ee849f238914cc7c31045dd5fbd34f38d2ce32e9 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
@@ -24,33 +24,35 @@
#pragma once
-#include <RobotAPI/interface/units/InertialMeasurementUnit.h>
+#include <mutex>
+
#include <ArmarXCore/observers/Observer.h>
+
+#include <RobotAPI/interface/units/InertialMeasurementUnit.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <mutex>
-
namespace armarx
{
/**
* \class InertialMeasurementUnitObserverPropertyDefinitions
* \brief
*/
- class InertialMeasurementUnitObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class InertialMeasurementUnitObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- InertialMeasurementUnitObserverPropertyDefinitions(std::string prefix):
+ InertialMeasurementUnitObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("IMUTopicName", "IMUValues", "Name of the IMU Topic.");
- defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
- defineOptionalProperty<bool>("EnableVisualization", true, "Enable/Disable DebugDrawer visualizations");
+ defineOptionalProperty<std::string>(
+ "IMUTopicName", "IMUValues", "Name of the IMU Topic.");
+ defineOptionalProperty<std::string>(
+ "DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ defineOptionalProperty<bool>(
+ "EnableVisualization", true, "Enable/Disable DebugDrawer visualizations");
}
};
-
/**
* \class InertialMeasurementUnitObserver
* \ingroup RobotAPI-SensorActorUnits-observers
@@ -64,17 +66,25 @@ namespace armarx
virtual public InertialMeasurementUnitObserverInterface
{
public:
- InertialMeasurementUnitObserver() {}
+ InertialMeasurementUnitObserver()
+ {
+ }
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "InertialMeasurementUnitObserver";
}
+
void onInitObserver() override;
void onConnectObserver() override;
void onExitObserver() override;
- void reportSensorValues(const std::string& device, const std::string& name, const IMUData& values, const TimestampBasePtr& timestamp, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportSensorValues(const std::string& device,
+ const std::string& name,
+ const IMUData& values,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/**
* @see PropertyUser::createPropertyDefinitions()
@@ -87,7 +97,7 @@ namespace armarx
DebugDrawerInterfacePrx debugDrawerPrx;
- void offerValue(const std::string& device, const std::string& fieldName, const Vector3Ptr& vec);
+ void
+ offerValue(const std::string& device, const std::string& fieldName, const Vector3Ptr& vec);
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/KinematicUnit.cpp b/source/RobotAPI/components/units/KinematicUnit.cpp
index c2e0dc1854b402f00beb894231cf44d1377ff75a..4f6db0e2196b58a4d7ee4a9f480050bdc118cdd2 100644
--- a/source/RobotAPI/components/units/KinematicUnit.cpp
+++ b/source/RobotAPI/components/units/KinematicUnit.cpp
@@ -25,20 +25,21 @@
#include "KinematicUnit.h"
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/VirtualRobotException.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
namespace armarx
{
- void KinematicUnit::onInitComponent()
+ void
+ KinematicUnit::onInitComponent()
{
// read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
// the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
@@ -48,7 +49,7 @@ namespace armarx
{
std::vector<std::string> result;
const auto& packages = armarx::Application::GetProjectDependencies();
- std::set<std::string> packageSet {packages.begin(), packages.end()};
+ std::set<std::string> packageSet{packages.begin(), packages.end()};
packageSet.emplace(Application::GetProjectName());
packageSet.emplace(project);
packageSet.erase("");
@@ -70,7 +71,8 @@ namespace armarx
auto pathsString = finder.getDataDir();
Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ includePaths.insert(
+ includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
std::string robotFile = relativeRobotFile;
if (!ArmarXDataPath::getAbsolutePath(robotFile, robotFile, includePaths))
@@ -81,7 +83,8 @@ namespace armarx
// read the robot
try
{
- robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+ robot =
+ VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
}
catch (VirtualRobot::VirtualRobotException& e)
{
@@ -98,7 +101,8 @@ namespace armarx
robotNodes = robotNodeSetPtr->getAllRobotNodes();
// component dependencies
- listenerName = getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State";
+ listenerName =
+ getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State";
offeringTopic(listenerName);
offeringTopicFromProperty("SkillProviderTopic");
@@ -106,8 +110,8 @@ namespace armarx
this->onInitKinematicUnit();
}
-
- void KinematicUnit::onConnectComponent()
+ void
+ KinematicUnit::onConnectComponent()
{
ARMARX_INFO << "setting report topic to " << listenerName << flush;
listenerPrx = getTopic<KinematicUnitListenerPrx>(listenerName);
@@ -115,29 +119,34 @@ namespace armarx
this->onStartKinematicUnit();
}
-
- void KinematicUnit::onExitComponent()
+ void
+ KinematicUnit::onExitComponent()
{
this->onExitKinematicUnit();
}
-
- void KinematicUnit::switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c)
+ void
+ KinematicUnit::switchControlMode(const NameControlModeMap& targetJointModes,
+ const Ice::Current& c)
{
}
- void KinematicUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c)
+ void
+ KinematicUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c)
{
}
- void KinematicUnit::setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c)
+ void
+ KinematicUnit::setJointVelocities(const NameValueMap& targetJointVelocities,
+ const Ice::Current& c)
{
}
- void KinematicUnit::setJointAnglesToZero()
+ void
+ KinematicUnit::setJointAnglesToZero()
{
NameControlModeMap c;
- NameValueMap t;
+ NameValueMap t;
for (const auto& j : getJoints())
{
c[j] = ControlMode::ePositionControl;
@@ -146,10 +155,12 @@ namespace armarx
switchControlMode(c);
setJointAngles(t);
}
- void KinematicUnit::setJointVelocitiesToZero()
+
+ void
+ KinematicUnit::setJointVelocitiesToZero()
{
NameControlModeMap c;
- NameValueMap t;
+ NameValueMap t;
for (const auto& j : getJoints())
{
c[j] = ControlMode::eVelocityControl;
@@ -159,28 +170,32 @@ namespace armarx
setJointVelocities(t);
}
- void KinematicUnit::requestKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c)
+ void
+ KinematicUnit::requestKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c)
{
SensorActorUnit::request(c);
}
- void KinematicUnit::releaseKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c)
+ void
+ KinematicUnit::releaseKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c)
{
SensorActorUnit::release(c);
}
-
- std::string KinematicUnit::getRobotFilename(const Ice::Current&) const
+ std::string
+ KinematicUnit::getRobotFilename(const Ice::Current&) const
{
return relativeRobotFile;
}
- std::vector<std::string> KinematicUnit::getArmarXPackages(const Ice::Current&) const
+ std::vector<std::string>
+ KinematicUnit::getArmarXPackages(const Ice::Current&) const
{
return armarXPackages;
}
- std::string KinematicUnit::getRobotName(const Ice::Current&) const
+ std::string
+ KinematicUnit::getRobotName(const Ice::Current&) const
{
if (robot)
{
@@ -192,7 +207,8 @@ namespace armarx
}
}
- std::string KinematicUnit::getRobotNodeSetName(const Ice::Current&) const
+ std::string
+ KinematicUnit::getRobotNodeSetName(const Ice::Current&) const
{
if (robotNodeSetName.empty())
{
@@ -201,13 +217,15 @@ namespace armarx
return robotNodeSetName;
}
- std::string KinematicUnit::getReportTopicName(const Ice::Current&) const
+ std::string
+ KinematicUnit::getReportTopicName(const Ice::Current&) const
{
return listenerName;
}
- PropertyDefinitionsPtr KinematicUnit::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ KinematicUnit::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new KinematicUnitPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/KinematicUnit.h b/source/RobotAPI/components/units/KinematicUnit.h
index 74146b034692247907da52d1cb5dfa4ee604d772..35dc3b05a210fb110678106e5e2d0ee3d7e8a8ff 100644
--- a/source/RobotAPI/components/units/KinematicUnit.h
+++ b/source/RobotAPI/components/units/KinematicUnit.h
@@ -24,40 +24,45 @@
#pragma once
-#include <RobotAPI/components/units/SensorActorUnit.h>
+#include <vector>
+
+#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <vector>
-
namespace armarx
{
/**
* \class KinematicUnitPropertyDefinitions
* \brief
*/
- class KinematicUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class KinematicUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- KinematicUnitPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ KinematicUnitPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("RobotNodeSetName", "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
- defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
- defineOptionalProperty<std::string>("RobotFileNameProject", "", "Project in which the robot filename is located (if robot is loaded from an external project)");
- defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
- defineOptionalProperty<std::string>("SkillProviderTopic", "SkillProviderTopic", "Topic where skill providers provide their skills");
+ defineRequiredProperty<std::string>(
+ "RobotNodeSetName",
+ "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
+ defineRequiredProperty<std::string>("RobotFileName",
+ "Robot file name, e.g. robot_model.xml");
+ defineOptionalProperty<std::string>("RobotFileNameProject",
+ "",
+ "Project in which the robot filename is located "
+ "(if robot is loaded from an external project)");
+ defineOptionalProperty<std::string>(
+ "TopicPrefix", "", "Prefix for the sensor value topic name.");
+ defineOptionalProperty<std::string>("SkillProviderTopic",
+ "SkillProviderTopic",
+ "Topic where skill providers provide their skills");
}
};
-
/**
* \defgroup Component-KinematicUnit KinematicUnit
* \ingroup RobotAPI-SensorActorUnits
@@ -74,13 +79,12 @@ namespace armarx
* @ingroup Component-KinematicUnit
* @brief The KinematicUnit class
*/
- class KinematicUnit :
- virtual public KinematicUnitInterface,
- virtual public SensorActorUnit
+ class KinematicUnit : virtual public KinematicUnitInterface, virtual public SensorActorUnit
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "KinematicUnit";
}
@@ -98,7 +102,8 @@ namespace armarx
* \brief getArmarXPackages
* \return All dependent packages, which might contain a robot file.
*/
- std::vector< std::string > getArmarXPackages(const Ice::Current& = Ice::emptyCurrent) const override;
+ std::vector<std::string>
+ getArmarXPackages(const Ice::Current& = Ice::emptyCurrent) const override;
/**
*
@@ -122,11 +127,16 @@ namespace armarx
virtual void onExitKinematicUnit() = 0;
// proxy implementation
- virtual void requestKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c = Ice::emptyCurrent);
- virtual void releaseKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c = Ice::emptyCurrent);
- void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = Ice::emptyCurrent) override;
- void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = Ice::emptyCurrent) override;
- void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = Ice::emptyCurrent) override;
+ virtual void requestKinematicUnit(const Ice::StringSeq& nodes,
+ const Ice::Current& c = Ice::emptyCurrent);
+ virtual void releaseKinematicUnit(const Ice::StringSeq& nodes,
+ const Ice::Current& c = Ice::emptyCurrent);
+ void switchControlMode(const NameControlModeMap& targetJointModes,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointAngles(const NameValueMap& targetJointAngles,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointVelocities(const NameValueMap& targetJointVelocities,
+ const Ice::Current& c = Ice::emptyCurrent) override;
//other
@@ -145,7 +155,7 @@ namespace armarx
VirtualRobot::RobotPtr robot;
std::string robotNodeSetName;
std::string listenerName;
- std::vector< VirtualRobot::RobotNodePtr > robotNodes;
+ std::vector<VirtualRobot::RobotNodePtr> robotNodes;
std::vector<std::string> armarXPackages;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index 64fcc6bf313220e49c6b79d998213962f04ddc12..1dd1472c7afaf0c8936d3c4609149589a3be7a9d 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -21,31 +21,33 @@
*/
#include "KinematicUnitObserver.h"
-#include "KinematicUnit.h"
-#include <ArmarXCore/observers/checks/ConditionCheckValid.h>
-#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
+#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <ArmarXCore/observers/checks/ConditionCheckValid.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include "KinematicUnit.h"
namespace armarx
{
// ********************************************************************
// observer framework hooks
// ********************************************************************
- void KinematicUnitObserver::onInitObserver()
+ void
+ KinematicUnitObserver::onInitObserver()
{
robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
@@ -61,7 +63,8 @@ namespace armarx
usingTopic(getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State");
}
- void KinematicUnitObserver::onConnectObserver()
+ void
+ KinematicUnitObserver::onConnectObserver()
{
// read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
// the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
@@ -75,7 +78,8 @@ namespace armarx
auto pathsString = finder.getDataDir();
Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ includePaths.insert(
+ includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
if (!ArmarXDataPath::getAbsolutePath(robotFile, robotFile, includePaths))
@@ -83,7 +87,8 @@ namespace armarx
throw UserException("Could not find robot file " + robotFile);
}
- VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+ VirtualRobot::RobotPtr robot =
+ VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
if (robotNodeSetName == "")
{
@@ -92,43 +97,74 @@ namespace armarx
auto robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
- std::vector< VirtualRobot::RobotNodePtr > robotNodes;
+ std::vector<VirtualRobot::RobotNodePtr> robotNodes;
robotNodes = robotNodeSetPtr->getAllRobotNodes();
auto robotNodeNames = robotNodeSetPtr->getNodeNames();
this->robotNodes = std::set<std::string>(robotNodeNames.begin(), robotNodeNames.end());
// register all channels
offerChannel("jointangles", "Joint values of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointaccelerations", "Joint accelerations of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointtorques", "Joint torques of the" + robotNodeSetName + " kinematic chain");
- offerChannel("jointcurrents", "Joint currents of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointmotortemperatures", "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
- offerChannel("jointcontrolmodes", "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointvelocities",
+ "Joint velocities of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointaccelerations",
+ "Joint accelerations of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointtorques",
+ "Joint torques of the" + robotNodeSetName + " kinematic chain");
+ offerChannel("jointcurrents",
+ "Joint currents of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointmotortemperatures",
+ "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
+ offerChannel("jointcontrolmodes",
+ "Joint motor temperatures of the " + robotNodeSetName + " kinematic chain");
// register all data fields
- for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
+ for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin();
+ it != robotNodes.end();
+ it++)
{
std::string jointName = (*it)->getName();
ARMARX_VERBOSE << "* " << jointName << std::endl;
- offerDataFieldWithDefault("jointcontrolmodes", jointName, ControlModeToString(eUnknown), "Controlmode of the " + jointName + " joint");
- offerDataField("jointangles", jointName, VariantType::Float, "Joint angle of the " + jointName + " joint in radians");
- offerDataField("jointvelocities", jointName, VariantType::Float, "Joint velocity of the " + jointName + " joint");
- offerDataField("jointaccelerations", jointName, VariantType::Float, "Joint acceleration of the " + jointName + " joint");
- offerDataField("jointtorques", jointName, VariantType::Float, "Joint torque of the " + jointName + " joint");
- offerDataField("jointcurrents", jointName, VariantType::Float, "Joint current of the " + jointName + " joint");
- offerDataField("jointmotortemperatures", jointName, VariantType::Float, "Joint motor temperature of the " + jointName + " joint");
+ offerDataFieldWithDefault("jointcontrolmodes",
+ jointName,
+ ControlModeToString(eUnknown),
+ "Controlmode of the " + jointName + " joint");
+ offerDataField("jointangles",
+ jointName,
+ VariantType::Float,
+ "Joint angle of the " + jointName + " joint in radians");
+ offerDataField("jointvelocities",
+ jointName,
+ VariantType::Float,
+ "Joint velocity of the " + jointName + " joint");
+ offerDataField("jointaccelerations",
+ jointName,
+ VariantType::Float,
+ "Joint acceleration of the " + jointName + " joint");
+ offerDataField("jointtorques",
+ jointName,
+ VariantType::Float,
+ "Joint torque of the " + jointName + " joint");
+ offerDataField("jointcurrents",
+ jointName,
+ VariantType::Float,
+ "Joint current of the " + jointName + " joint");
+ offerDataField("jointmotortemperatures",
+ jointName,
+ VariantType::Float,
+ "Joint motor temperature of the " + jointName + " joint");
}
updateChannel("jointcontrolmodes");
}
-
-
// ********************************************************************
// KinematicUnitListener interface implementation
// ********************************************************************
- void KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+ void
+ KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& jointModes,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c)
{
try
{
@@ -139,7 +175,8 @@ namespace armarx
for (auto elem : jointModes)
{
- setDataFieldFlatCopy("jointcontrolmodes", elem.first, new Variant(ControlModeToString(elem.second)));
+ setDataFieldFlatCopy(
+ "jointcontrolmodes", elem.first, new Variant(ControlModeToString(elem.second)));
}
updateChannel("jointcontrolmodes");
@@ -150,7 +187,11 @@ namespace armarx
}
}
- void KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+ void
+ KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c)
{
try
{
@@ -165,7 +206,6 @@ namespace armarx
updateChannel("jointangles");
-
}
catch (...)
{
@@ -173,8 +213,11 @@ namespace armarx
}
}
-
- void KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+ void
+ KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVelocities,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c)
{
try
{
@@ -194,7 +237,11 @@ namespace armarx
}
}
- void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+ void
+ KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c)
{
try
{
@@ -214,7 +261,11 @@ namespace armarx
}
}
- void KinematicUnitObserver::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+ void
+ KinematicUnitObserver::reportJointAccelerations(const NameValueMap& jointAccelerations,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c)
{
try
{
@@ -224,7 +275,8 @@ namespace armarx
}
- nameValueMapToDataFields("jointaccelerations", jointAccelerations, timestamp, aValueChanged);
+ nameValueMapToDataFields(
+ "jointaccelerations", jointAccelerations, timestamp, aValueChanged);
updateChannel("jointaccelerations");
}
@@ -234,7 +286,11 @@ namespace armarx
}
}
- void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+ void
+ KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c)
{
try
{
@@ -254,7 +310,11 @@ namespace armarx
}
}
- void KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+ void
+ KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c)
{
try
{
@@ -264,7 +324,8 @@ namespace armarx
}
- nameValueMapToDataFields("jointmotortemperatures", jointMotorTemperatures, timestamp, aValueChanged);
+ nameValueMapToDataFields(
+ "jointmotortemperatures", jointMotorTemperatures, timestamp, aValueChanged);
updateChannel("jointmotortemperatures");
}
@@ -274,14 +335,22 @@ namespace armarx
}
}
- void KinematicUnitObserver::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+ void
+ KinematicUnitObserver::reportJointStatuses(const NameStatusMap& jointStatuses,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c)
{
}
// ********************************************************************
// private methods
// ********************************************************************
- void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap, Ice::Long timestamp, bool aValueChanged)
+ void
+ KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelName,
+ const NameValueMap& nameValueMap,
+ Ice::Long timestamp,
+ bool aValueChanged)
{
// ARMARX_INFO << deactivateSpam(10) << " timestamp is " << (IceUtil::Time::now() - IceUtil::Time::microSeconds(timestamp)).toMicroSecondsDouble() << " µs old";
bool newChannel;
@@ -293,7 +362,7 @@ namespace armarx
if (aValueChanged || newChannel)
{
- std::unordered_map< ::std::string, ::armarx::VariantBasePtr> map;
+ std::unordered_map<::std::string, ::armarx::VariantBasePtr> map;
if (timestamp < 0)
{
for (const auto& it : nameValueMap)
@@ -310,7 +379,8 @@ namespace armarx
{
if (robotNodes.count(it.first))
{
- map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
+ map[it.first] = new TimedVariant(new Variant(it.second),
+ IceUtil::Time::microSeconds(timestamp));
}
}
}
@@ -320,12 +390,12 @@ namespace armarx
{
updateDatafieldTimestamps(channelName, timestamp);
}
-
}
- PropertyDefinitionsPtr KinematicUnitObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ KinematicUnitObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new KinematicUnitObserverPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new KinematicUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index 5b21e89d0cb300c331cd105b907439da70cc937a..a21bd9a5c1289389b66d13aa67ded34834d7c124 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -22,35 +22,41 @@
#pragma once
-#include <ArmarXCore/core/system/ImportExport.h>
+#include <mutex>
+#include <string>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
+#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
#include <ArmarXCore/observers/ConditionCheck.h>
#include <ArmarXCore/observers/Observer.h>
-#include <string>
-#include <mutex>
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
namespace armarx
{
ARMARX_CREATE_CHECK(KinematicUnitObserver, equals)
ARMARX_CREATE_CHECK(KinematicUnitObserver, larger)
- class KinematicUnitObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class KinematicUnitObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- KinematicUnitObserverPropertyDefinitions(std::string prefix):
+ KinematicUnitObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("RobotNodeSetName", "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
- defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
- defineOptionalProperty<std::string>("RobotFileNameProject", "", "Project in which the robot filename is located (if robot is loaded from an external project)");
- defineOptionalProperty<std::string>("TopicPrefix", "", "Prefix for the sensor value topic name.");
-
+ defineRequiredProperty<std::string>(
+ "RobotNodeSetName",
+ "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
+ defineRequiredProperty<std::string>("RobotFileName",
+ "Robot file name, e.g. robot_model.xml");
+ defineOptionalProperty<std::string>("RobotFileNameProject",
+ "",
+ "Project in which the robot filename is located "
+ "(if robot is loaded from an external project)");
+ defineOptionalProperty<std::string>(
+ "TopicPrefix", "", "Prefix for the sensor value topic name.");
}
};
@@ -78,16 +84,41 @@ namespace armarx
// slice interface implementation
- void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
-
- std::string getDefaultName() const override
+ void reportControlModeChanged(const NameControlModeMap& jointModes,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointAngles(const NameValueMap& jointAngles,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointVelocities(const NameValueMap& jointVelocities,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointTorques(const NameValueMap& jointTorques,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointAccelerations(const NameValueMap& jointAccelerations,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c) override;
+ void reportJointCurrents(const NameValueMap& jointCurrents,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportJointStatuses(const NameStatusMap& jointStatuses,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+
+ std::string
+ getDefaultName() const override
{
return "KinematicUnitObserver";
}
@@ -97,7 +128,8 @@ namespace armarx
*/
PropertyDefinitionsPtr createPropertyDefinitions() override;
- static std::string ControlModeToString(ControlMode mode)
+ static std::string
+ ControlModeToString(ControlMode mode)
{
switch (mode)
{
@@ -120,9 +152,10 @@ namespace armarx
default:
return "Unknown";
}
-
}
- static ControlMode StringToControlMode(const std::string& mode)
+
+ static ControlMode
+ StringToControlMode(const std::string& mode)
{
if (mode == "Disabled")
{
@@ -153,18 +186,18 @@ namespace armarx
}
protected:
- void nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap, Ice::Long timestamp, bool aValueChanged);
+ void nameValueMapToDataFields(const std::string& channelName,
+ const NameValueMap& nameValueMap,
+ Ice::Long timestamp,
+ bool aValueChanged);
std::set<std::string> initializedChannels;
std::mutex initializedChannelsMutex;
+
private:
std::string robotNodeSetName;
std::set<std::string> robotNodes;
};
-
-
-
-
/**
@class KinematicUnitDatafieldCreator
@brief
@@ -173,7 +206,9 @@ namespace armarx
class KinematicUnitDatafieldCreator
{
public:
- KinematicUnitDatafieldCreator(const std::string& channelName): _channelName(channelName) {}
+ KinematicUnitDatafieldCreator(const std::string& channelName) : _channelName(channelName)
+ {
+ }
/**
* @brief Function to create a Channel Representation for a KinematicUnitObserver
@@ -182,7 +217,9 @@ namespace armarx
* in the simox-robot-xml-file
* @return returns a ChannelRepresentationPtr
*/
- DataFieldIdentifier getDatafield(const std::string& kinematicUnitObserverName, const std::string& jointName) const
+ DataFieldIdentifier
+ getDatafield(const std::string& kinematicUnitObserverName,
+ const std::string& jointName) const
{
if (kinematicUnitObserverName.empty())
{
@@ -200,7 +237,7 @@ namespace armarx
private:
std::string _channelName;
};
-}
+} // namespace armarx
namespace armarx::channels::KinematicUnitObserver
{
@@ -209,4 +246,4 @@ namespace armarx::channels::KinematicUnitObserver
const KinematicUnitDatafieldCreator jointTorques("jointTorques");
const KinematicUnitDatafieldCreator jointCurrents("jointCurrents");
const KinematicUnitDatafieldCreator jointTemperatures("jointTemperatures");
-}
+} // namespace armarx::channels::KinematicUnitObserver
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index d4c729ae937af20e5c2f282530ae13459f4d7dda..4558be58aad260ed0dbf33c0f16df8fc916d28cc 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -25,41 +25,48 @@
#include "KinematicUnitSimulation.h"
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include <algorithm>
+
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/VirtualRobotException.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/util/algorithm.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-
-#include <algorithm>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
using namespace armarx;
-
-KinematicUnitSimulationPropertyDefinitions::KinematicUnitSimulationPropertyDefinitions(std::string prefix) :
+KinematicUnitSimulationPropertyDefinitions::KinematicUnitSimulationPropertyDefinitions(
+ std::string prefix) :
KinematicUnitPropertyDefinitions(prefix)
{
- defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
- defineOptionalProperty<float>("Noise", 0.0f, "Gaussian noise is added to the velocity. Value in Degree");
- defineOptionalProperty<bool>("UsePDControllerForJointControl", true, "If true a PD controller is also used in Position Mode instead of setting the joint angles instantly");
+ defineOptionalProperty<int>(
+ "IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
+ defineOptionalProperty<float>(
+ "Noise", 0.0f, "Gaussian noise is added to the velocity. Value in Degree");
+ defineOptionalProperty<bool>("UsePDControllerForJointControl",
+ true,
+ "If true a PD controller is also used in Position Mode instead of "
+ "setting the joint angles instantly");
defineOptionalProperty<float>("Kp", 3, "proportional gain of the PID position controller.");
defineOptionalProperty<float>("Ki", 0.001f, "integral gain of the PID position controller.");
defineOptionalProperty<float>("Kd", 0.0, "derivative gain of the PID position controller.");
-
}
-
-void KinematicUnitSimulation::onInitKinematicUnit()
+void
+KinematicUnitSimulation::onInitKinematicUnit()
{
ARMARX_TRACE;
ARMARX_INFO << "Init Simulation";
usePDControllerForPosMode = getProperty<bool>("UsePDControllerForJointControl").getValue();
- for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
+ for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin();
+ it != robotNodes.end();
+ it++)
{
std::string jointName = (*it)->getName();
jointInfos[jointName].limitLo = (*it)->getJointLimitLo();
@@ -73,7 +80,8 @@ void KinematicUnitSimulation::onInitKinematicUnit()
}
}
- ARMARX_VERBOSE << jointName << " with limits: lo: " << jointInfos[jointName].limitLo << " hi: " << jointInfos[jointName].limitHi;
+ ARMARX_VERBOSE << jointName << " with limits: lo: " << jointInfos[jointName].limitLo
+ << " hi: " << jointInfos[jointName].limitHi;
}
ARMARX_TRACE;
{
@@ -81,15 +89,17 @@ void KinematicUnitSimulation::onInitKinematicUnit()
std::unique_lock lock(jointStatesMutex);
// initialize JoinStates
- for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
+ for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin();
+ it != robotNodes.end();
+ it++)
{
std::string jointName = (*it)->getName();
jointStates[jointName] = KinematicUnitSimulationJointState();
jointStates[jointName].init();
- positionControlPIDController[jointName] = PIDControllerPtr(new PIDController(
- getProperty<float>("Kp").getValue(),
- getProperty<float>("Ki").getValue(),
- getProperty<float>("Kd").getValue()));
+ positionControlPIDController[jointName] =
+ PIDControllerPtr(new PIDController(getProperty<float>("Kp").getValue(),
+ getProperty<float>("Ki").getValue(),
+ getProperty<float>("Kd").getValue()));
}
}
ARMARX_TRACE;
@@ -101,45 +111,52 @@ void KinematicUnitSimulation::onInitKinematicUnit()
}
intervalMs = getProperty<int>("IntervalMs").getValue();
ARMARX_VERBOSE << "Starting kinematic unit simulation with " << intervalMs << " ms interval";
- simulationTask = new PeriodicTask<KinematicUnitSimulation>(this, &KinematicUnitSimulation::simulationFunction, intervalMs, false, "KinematicUnitSimulation");
+ simulationTask =
+ new PeriodicTask<KinematicUnitSimulation>(this,
+ &KinematicUnitSimulation::simulationFunction,
+ intervalMs,
+ false,
+ "KinematicUnitSimulation");
}
-void KinematicUnitSimulation::onStartKinematicUnit()
+void
+KinematicUnitSimulation::onStartKinematicUnit()
{
lastExecutionTime = TimeUtil::GetTime();
simulationTask->start();
}
-
-void KinematicUnitSimulation::onExitKinematicUnit()
+void
+KinematicUnitSimulation::onExitKinematicUnit()
{
simulationTask->stop();
}
-
-void KinematicUnitSimulation::simulationFunction()
+void
+KinematicUnitSimulation::simulationFunction()
{
// the time it took until this method was called again
- double timeDeltaInSeconds = (TimeUtil::GetTime() - lastExecutionTime).toMilliSecondsDouble() / 1000.0;
+ double timeDeltaInSeconds =
+ (TimeUtil::GetTime() - lastExecutionTime).toMilliSecondsDouble() / 1000.0;
lastExecutionTime = TimeUtil::GetTime();
// name value maps for reporting
- std::lock_guard guard {sensorDataMutex};
+ std::lock_guard guard{sensorDataMutex};
sensorAngles.clear();
sensorVelocities.clear();
bool aPosValueChanged = false;
bool aVelValueChanged = false;
auto signedMin = [](float newValue, float minAbsValue)
- {
- return std::copysign(std::min<float>(fabs(newValue), fabs(minAbsValue)), newValue);
- };
+ { return std::copysign(std::min<float>(fabs(newValue), fabs(minAbsValue)), newValue); };
{
std::unique_lock lock(jointStatesMutex);
- for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it != jointStates.end(); it++)
+ for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin();
+ it != jointStates.end();
+ it++)
{
float newAngle = 0.0f;
@@ -173,17 +190,19 @@ void KinematicUnitSimulation::simulationFunction()
PIDControllerPtr pid = positionControlPIDController[it->first];
if (pid)
{
- float velValue = (it->second.velocity != 0.0f ? signedMin(pid->getControlValue(), it->second.velocity) : pid->getControlValue()); //restrain to max velocity
+ float velValue =
+ (it->second.velocity != 0.0f
+ ? signedMin(pid->getControlValue(), it->second.velocity)
+ : pid->getControlValue()); //restrain to max velocity
if (noise > 0)
{
velValue *= 1 + distribution(generator);
}
pid->update(it->second.angle);
- newAngle = it->second.angle +
- velValue *
- timeDeltaInSeconds;
- if (fabs(previousJointStates[it->first].velocityActual - velValue) > 0.00000001)
+ newAngle = it->second.angle + velValue * timeDeltaInSeconds;
+ if (fabs(previousJointStates[it->first].velocityActual - velValue) >
+ 0.00000001)
{
aVelValueChanged = true;
}
@@ -203,7 +222,8 @@ void KinematicUnitSimulation::simulationFunction()
}
// Check if joint existed before
- KinematicUnitSimulationJointStates::iterator itPrev = previousJointStates.find(it->first);
+ KinematicUnitSimulationJointStates::iterator itPrev =
+ previousJointStates.find(it->first);
if (itPrev == previousJointStates.end())
{
@@ -219,7 +239,6 @@ void KinematicUnitSimulation::simulationFunction()
}
-
if (jointInfo.continuousJoint())
{
if (newAngle < 0)
@@ -230,7 +249,6 @@ void KinematicUnitSimulation::simulationFunction()
{
newAngle = fmod(newAngle + M_PI, 2 * M_PI) - M_PI;
}
-
}
sensorAngles[it->first] = newAngle;
@@ -244,20 +262,25 @@ void KinematicUnitSimulation::simulationFunction()
listenerPrx->reportJointVelocities(sensorVelocities, timestamp, aVelValueChanged);
}
-void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c)
+void
+KinematicUnitSimulation::switchControlMode(const NameControlModeMap& targetJointModes,
+ const Ice::Current& c)
{
bool aValueChanged = false;
NameControlModeMap changedControlModes;
{
std::unique_lock lock(jointStatesMutex);
- for (NameControlModeMap::const_iterator it = targetJointModes.begin(); it != targetJointModes.end(); it++)
+ for (NameControlModeMap::const_iterator it = targetJointModes.begin();
+ it != targetJointModes.end();
+ it++)
{
// target values
std::string targetJointName = it->first;
ControlMode targetControlMode = it->second;
- KinematicUnitSimulationJointStates::iterator iterJoints = jointStates.find(targetJointName);
+ KinematicUnitSimulationJointStates::iterator iterJoints =
+ jointStates.find(targetJointName);
// check whether there is a joint with this name and set joint angle
if (iterJoints != jointStates.end())
@@ -277,11 +300,13 @@ void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& target
}
}
// only report control modes which have been changed
- listenerPrx->reportControlModeChanged(changedControlModes, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
+ listenerPrx->reportControlModeChanged(
+ changedControlModes, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
}
-
-void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c)
+void
+KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngles,
+ const Ice::Current& c)
{
std::unique_lock lock(jointStatesMutex);
@@ -289,7 +314,8 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
NameValueMap actualJointAngles;
bool aValueChanged = false;
- for (NameValueMap::const_iterator it = targetJointAngles.begin(); it != targetJointAngles.end(); it++)
+ for (NameValueMap::const_iterator it = targetJointAngles.begin(); it != targetJointAngles.end();
+ it++)
{
// target values
std::string targetJointName = it->first;
@@ -330,30 +356,33 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
}
pid->setTarget(targetJointAngle);
-
-
}
}
}
else
{
- ARMARX_WARNING << deactivateSpam(1) << "Joint '" << targetJointName << "' not found!";
+ ARMARX_WARNING << deactivateSpam(1) << "Joint '" << targetJointName << "' not found!";
}
}
if (aValueChanged)
{
- listenerPrx->reportJointAngles(actualJointAngles, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
+ listenerPrx->reportJointAngles(
+ actualJointAngles, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
}
}
-void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current&)
+void
+KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJointVelocities,
+ const Ice::Current&)
{
std::unique_lock lock(jointStatesMutex);
NameValueMap actualJointVelocities;
bool aValueChanged = false;
- for (NameValueMap::const_iterator it = targetJointVelocities.begin(); it != targetJointVelocities.end(); it++)
+ for (NameValueMap::const_iterator it = targetJointVelocities.begin();
+ it != targetJointVelocities.end();
+ it++)
{
// target values
std::string targetJointName = it->first;
@@ -376,17 +405,22 @@ void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJoint
if (aValueChanged)
{
- listenerPrx->reportJointVelocities(actualJointVelocities, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
+ listenerPrx->reportJointVelocities(
+ actualJointVelocities, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
}
}
-void KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current&)
+void
+KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTorques,
+ const Ice::Current&)
{
std::unique_lock lock(jointStatesMutex);
NameValueMap actualJointTorques;
bool aValueChanged = false;
- for (NameValueMap::const_iterator it = targetJointTorques.begin(); it != targetJointTorques.end(); it++)
+ for (NameValueMap::const_iterator it = targetJointTorques.begin();
+ it != targetJointTorques.end();
+ it++)
{
// target values
std::string targetJointName = it->first;
@@ -409,15 +443,19 @@ void KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTor
if (aValueChanged)
{
- listenerPrx->reportJointTorques(actualJointTorques, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
+ listenerPrx->reportJointTorques(
+ actualJointTorques, aValueChanged, TimeUtil::GetTime().toMicroSeconds());
}
}
-NameControlModeMap KinematicUnitSimulation::getControlModes(const Ice::Current&)
+NameControlModeMap
+KinematicUnitSimulation::getControlModes(const Ice::Current&)
{
NameControlModeMap result;
- for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it != jointStates.end(); it++)
+ for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin();
+ it != jointStates.end();
+ it++)
{
result[it->first] = it->second.controlMode;
}
@@ -425,22 +463,29 @@ NameControlModeMap KinematicUnitSimulation::getControlModes(const Ice::Current&)
return result;
}
-void KinematicUnitSimulation::setJointAccelerations(const NameValueMap& targetJointAccelerations, const Ice::Current&)
+void
+KinematicUnitSimulation::setJointAccelerations(const NameValueMap& targetJointAccelerations,
+ const Ice::Current&)
{
- (void) targetJointAccelerations;
+ (void)targetJointAccelerations;
}
-void KinematicUnitSimulation::setJointDecelerations(const NameValueMap& targetJointDecelerations, const Ice::Current&)
+void
+KinematicUnitSimulation::setJointDecelerations(const NameValueMap& targetJointDecelerations,
+ const Ice::Current&)
{
- (void) targetJointDecelerations;
+ (void)targetJointDecelerations;
}
-void KinematicUnitSimulation::stop(const Ice::Current& c)
+void
+KinematicUnitSimulation::stop(const Ice::Current& c)
{
std::unique_lock lock(jointStatesMutex);
- for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin(); it != jointStates.end(); it++)
+ for (KinematicUnitSimulationJointStates::iterator it = jointStates.begin();
+ it != jointStates.end();
+ it++)
{
it->second.velocity = 0;
}
@@ -448,37 +493,41 @@ void KinematicUnitSimulation::stop(const Ice::Current& c)
SensorActorUnit::stop(c);
}
-
-PropertyDefinitionsPtr KinematicUnitSimulation::createPropertyDefinitions()
+PropertyDefinitionsPtr
+KinematicUnitSimulation::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(
- new KinematicUnitSimulationPropertyDefinitions(getConfigIdentifier()));
+ new KinematicUnitSimulationPropertyDefinitions(getConfigIdentifier()));
}
-NameValueMap KinematicUnitSimulation::getJointAngles(const Ice::Current& c) const
+NameValueMap
+KinematicUnitSimulation::getJointAngles(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {sensorDataMutex};
+ std::lock_guard<std::mutex> guard{sensorDataMutex};
return sensorAngles;
}
-NameValueMap KinematicUnitSimulation::getJointVelocities(const Ice::Current& c) const
+NameValueMap
+KinematicUnitSimulation::getJointVelocities(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {sensorDataMutex};
+ std::lock_guard<std::mutex> guard{sensorDataMutex};
return sensorVelocities;
}
-Ice::StringSeq KinematicUnitSimulation::getJoints(const Ice::Current& c) const
+Ice::StringSeq
+KinematicUnitSimulation::getJoints(const Ice::Current& c) const
{
- std::lock_guard<std::mutex> guard {sensorDataMutex};
+ std::lock_guard<std::mutex> guard{sensorDataMutex};
return getMapKeys(sensorAngles);
}
-DebugInfo KinematicUnitSimulation::getDebugInfo(const Ice::Current& c) const
+DebugInfo
+KinematicUnitSimulation::getDebugInfo(const Ice::Current& c) const
{
std::lock_guard g{jointStatesMutex};
DebugInfo debugInfo;
- for(const auto& [jointName, info]: jointStates)
+ for (const auto& [jointName, info] : jointStates)
{
debugInfo.jointAngles[jointName] = info.angle;
debugInfo.jointVelocities[jointName] = info.velocity;
@@ -492,17 +541,19 @@ DebugInfo KinematicUnitSimulation::getDebugInfo(const Ice::Current& c) const
return debugInfo;
}
-
-
-bool mapEntryEqualsString(std::pair<std::string, KinematicUnitSimulationJointState> iter, std::string compareString)
+bool
+mapEntryEqualsString(std::pair<std::string, KinematicUnitSimulationJointState> iter,
+ std::string compareString)
{
return (iter.first == compareString);
}
-void KinematicUnitSimulation::requestJoints(const Ice::StringSeq& joints, const Ice::Current& c)
+void
+KinematicUnitSimulation::requestJoints(const Ice::StringSeq& joints, const Ice::Current& c)
{
// if one of the joints belongs to this unit, lock access to this unit for other components except for the requesting one
- KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
+ KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(
+ jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
if (jointStates.end() != it)
{
@@ -510,10 +561,12 @@ void KinematicUnitSimulation::requestJoints(const Ice::StringSeq& joints, const
}
}
-void KinematicUnitSimulation::releaseJoints(const Ice::StringSeq& joints, const Ice::Current& c)
+void
+KinematicUnitSimulation::releaseJoints(const Ice::StringSeq& joints, const Ice::Current& c)
{
// if one of the joints belongs to this unit, unlock access
- KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
+ KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(
+ jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
if (jointStates.end() != it)
{
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index 869d71cd1e5212297ca0f198316a2a0b74a9c58f..1f188fe8f0b0f3f51cfd9168bb40a2411823a6b3 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -24,16 +24,17 @@
#pragma once
-#include "KinematicUnit.h"
+#include <mutex>
+#include <random>
+
+#include <IceUtil/Time.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-#include <IceUtil/Time.h>
+#include <RobotAPI/libraries/core/PIDController.h>
-#include <random>
-#include <mutex>
+#include "KinematicUnit.h"
namespace armarx
{
@@ -53,7 +54,8 @@ namespace armarx
init();
}
- void init()
+ void
+ init()
{
controlMode = eDisabled;
angle = 0.0f;
@@ -70,7 +72,9 @@ namespace armarx
float torque;
float temperature;
};
- using KinematicUnitSimulationJointStates = std::map<std::string, KinematicUnitSimulationJointState>;
+
+ using KinematicUnitSimulationJointStates =
+ std::map<std::string, KinematicUnitSimulationJointState>;
/**
* \class JointInfo.
@@ -88,16 +92,21 @@ namespace armarx
reset();
}
- void reset()
+ void
+ reset()
{
limitLo = 0.0f;
limitHi = 0.0f;
}
- bool hasLimits() const
+
+ bool
+ hasLimits() const
{
return (limitLo != limitHi);
}
- bool continuousJoint() const
+
+ bool
+ continuousJoint() const
{
return !hasLimits() || limitLo - limitHi >= 360.0f;
}
@@ -105,7 +114,9 @@ namespace armarx
float limitLo;
float limitHi;
};
- using KinematicUnitSimulationJointInfos = std::map<std::string, KinematicUnitSimulationJointInfo>;
+
+ using KinematicUnitSimulationJointInfos =
+ std::map<std::string, KinematicUnitSimulationJointInfo>;
/**
* \class KinematicUnitSimulationPropertyDefinitions
@@ -122,12 +133,12 @@ namespace armarx
* \brief Simulates robot kinematics
* \ingroup RobotAPI-SensorActorUnits-simulation
*/
- class KinematicUnitSimulation :
- virtual public KinematicUnit
+ class KinematicUnitSimulation : virtual public KinematicUnit
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "KinematicUnitSimulation";
}
@@ -139,20 +150,28 @@ namespace armarx
void simulationFunction();
// proxy implementation
- void requestJoints(const Ice::StringSeq& joints, const Ice::Current& c = Ice::emptyCurrent) override;
- void releaseJoints(const Ice::StringSeq& joints, const Ice::Current& c = Ice::emptyCurrent) override;
- void switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c = Ice::emptyCurrent) override;
- void setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c = Ice::emptyCurrent) override;
- void setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c = Ice::emptyCurrent) override;
- void setJointTorques(const NameValueMap& targetJointTorques, const Ice::Current& c = Ice::emptyCurrent) override;
+ void requestJoints(const Ice::StringSeq& joints,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void releaseJoints(const Ice::StringSeq& joints,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void switchControlMode(const NameControlModeMap& targetJointModes,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointAngles(const NameValueMap& targetJointAngles,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointVelocities(const NameValueMap& targetJointVelocities,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointTorques(const NameValueMap& targetJointTorques,
+ const Ice::Current& c = Ice::emptyCurrent) override;
NameControlModeMap getControlModes(const Ice::Current& c = Ice::emptyCurrent) override;
/// @warning Not implemented yet!
- void setJointAccelerations(const NameValueMap& targetJointAccelerations, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointAccelerations(const NameValueMap& targetJointAccelerations,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/// @warning Not implemented yet!
- void setJointDecelerations(const NameValueMap& targetJointDecelerations, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setJointDecelerations(const NameValueMap& targetJointDecelerations,
+ const Ice::Current& c = Ice::emptyCurrent) override;
void stop(const Ice::Current& c = Ice::emptyCurrent) override;
@@ -183,4 +202,4 @@ namespace armarx
NameValueMap sensorAngles;
NameValueMap sensorVelocities;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
index 04bd47d5a9cd4931ad9783391e346c88d29aa4ce..aac3879a6e0073d42b2d17150579cd690cb4a665 100644
--- a/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.cpp
@@ -22,6 +22,7 @@
*/
#include "LaserScannerUnitObserver.h"
+#include <cfloat>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
@@ -31,11 +32,10 @@
#include <RobotAPI/libraries/core/Pose.h>
-#include <cfloat>
-
namespace armarx
{
- void LaserScannerUnitObserver::onInitObserver()
+ void
+ LaserScannerUnitObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("LaserScannerTopicName").getValue());
@@ -45,24 +45,29 @@ namespace armarx
offerConditionCheck("smaller", new ConditionCheckSmaller());
}
-
-
- void LaserScannerUnitObserver::onConnectObserver()
+ void
+ LaserScannerUnitObserver::onConnectObserver()
{
}
-
- void LaserScannerUnitObserver::onExitObserver()
+ void
+ LaserScannerUnitObserver::onExitObserver()
{
}
-
- PropertyDefinitionsPtr LaserScannerUnitObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ LaserScannerUnitObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new LaserScannerUnitObserverPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new LaserScannerUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
- void LaserScannerUnitObserver::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan, const TimestampBasePtr& timestamp, const Ice::Current& c)
+ void
+ LaserScannerUnitObserver::reportSensorValues(const std::string& device,
+ const std::string& name,
+ const LaserScan& scan,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
@@ -98,14 +103,16 @@ namespace armarx
if (scan.size() > 0)
{
offerOrUpdateDataField(device, "minDistance", minDistance, "minimal distance in scan");
- offerOrUpdateDataField(device, "minAngle", minAngle, "angle with minimal distance in scan");
+ offerOrUpdateDataField(
+ device, "minAngle", minAngle, "angle with minimal distance in scan");
offerOrUpdateDataField(device, "maxDistance", maxDistance, "maximal distance in scan");
- offerOrUpdateDataField(device, "maxAngle", maxAngle, "angle with maximal distance in scan");
+ offerOrUpdateDataField(
+ device, "maxAngle", maxAngle, "angle with maximal distance in scan");
float averageDistance = distanceSum / scan.size();
- offerOrUpdateDataField(device, "averageDistance", averageDistance, "average distance in scan");
+ offerOrUpdateDataField(
+ device, "averageDistance", averageDistance, "average distance in scan");
}
updateChannel(device);
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/LaserScannerUnitObserver.h b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
index 05aa07cb9c36668d72d4fa8046c855c28e0ab735..f8c897987e1d8233583cde3e82e7995ca1b7f608 100644
--- a/source/RobotAPI/components/units/LaserScannerUnitObserver.h
+++ b/source/RobotAPI/components/units/LaserScannerUnitObserver.h
@@ -24,31 +24,31 @@
#pragma once
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <mutex>
+
#include <ArmarXCore/observers/Observer.h>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <mutex>
-
namespace armarx
{
/**
* \class LaserScannerUnitObserverPropertyDefinitions
* \brief
*/
- class LaserScannerUnitObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class LaserScannerUnitObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- LaserScannerUnitObserverPropertyDefinitions(std::string prefix):
+ LaserScannerUnitObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
+ defineOptionalProperty<std::string>(
+ "LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
}
};
-
/**
* \class LaserScannerUnitObserver
* \ingroup RobotAPI-SensorActorUnits-observers
@@ -61,12 +61,16 @@ namespace armarx
virtual public LaserScannerUnitObserverInterface
{
public:
- LaserScannerUnitObserver() {}
+ LaserScannerUnitObserver()
+ {
+ }
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "LaserScannerUnitObserver";
}
+
void onInitObserver() override;
void onConnectObserver() override;
void onExitObserver() override;
@@ -75,12 +79,13 @@ namespace armarx
*/
PropertyDefinitionsPtr createPropertyDefinitions() override;
- void reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan,
- const TimestampBasePtr& timestamp, const Ice::Current& c) override;
+ void reportSensorValues(const std::string& device,
+ const std::string& name,
+ const LaserScan& scan,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c) override;
private:
std::mutex dataMutex;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/MetaWearIMUObserver.cpp b/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
index 95937330857902c4127d0ffe3b35eebff3e8d86d..0b7f2962776cd421860ae5232034bf84458a5cd5 100644
--- a/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
+++ b/source/RobotAPI/components/units/MetaWearIMUObserver.cpp
@@ -23,21 +23,18 @@
*/
#include "MetaWearIMUObserver.h"
-
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
-
-#include <RobotAPI/libraries/core/Pose.h>
-
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx
{
- void MetaWearIMUObserver::onInitObserver()
+ void
+ MetaWearIMUObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("MetaWearTopicName").getValue());
@@ -49,20 +46,23 @@ namespace armarx
offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
}
-
-
- void MetaWearIMUObserver::onConnectObserver()
+ void
+ MetaWearIMUObserver::onConnectObserver()
{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(
+ getProperty<std::string>("DebugDrawerTopic").getValue());
}
-
- void MetaWearIMUObserver::onExitObserver()
+ void
+ MetaWearIMUObserver::onExitObserver()
{
-
}
- void armarx::MetaWearIMUObserver::reportIMUValues(const std::string& name, const MetaWearIMUData& data, const TimestampBasePtr& timestamp, const Ice::Current&)
+ void
+ armarx::MetaWearIMUObserver::reportIMUValues(const std::string& name,
+ const MetaWearIMUData& data,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
@@ -76,15 +76,19 @@ namespace armarx
offerQuaternion(name, "orientationQuaternion", data.orientationQuaternion);
updateChannel(name);
-
}
- PropertyDefinitionsPtr MetaWearIMUObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ MetaWearIMUObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new MetaWearIMUObserverPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new MetaWearIMUObserverPropertyDefinitions(getConfigIdentifier()));
}
- void MetaWearIMUObserver::offerVector3(const std::string& channelName, const std::string& dfName, const std::vector<float>& data)
+ void
+ MetaWearIMUObserver::offerVector3(const std::string& channelName,
+ const std::string& dfName,
+ const std::vector<float>& data)
{
if (data.size() == 3)
{
@@ -93,11 +97,15 @@ namespace armarx
}
else if (data.size() != 0)
{
- ARMARX_WARNING << "data." << dfName << ".size() != 3 && data." << dfName << ".size() != 0";
+ ARMARX_WARNING << "data." << dfName << ".size() != 3 && data." << dfName
+ << ".size() != 0";
}
}
- void MetaWearIMUObserver::offerQuaternion(const std::string& channelName, const std::string& dfName, const std::vector<float>& data)
+ void
+ MetaWearIMUObserver::offerQuaternion(const std::string& channelName,
+ const std::string& dfName,
+ const std::vector<float>& data)
{
if (data.size() == 4)
{
@@ -106,7 +114,8 @@ namespace armarx
}
else if (data.size() != 0)
{
- ARMARX_WARNING << "data." << dfName << ".size() != 4 && data." << dfName << ".size() != 0";
+ ARMARX_WARNING << "data." << dfName << ".size() != 4 && data." << dfName
+ << ".size() != 0";
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/MetaWearIMUObserver.h b/source/RobotAPI/components/units/MetaWearIMUObserver.h
index 87eb6b49fbd390a66aef98c4f0a22d174115d659..d8ac644842460a8a85fb00c93d313896c20f43dd 100644
--- a/source/RobotAPI/components/units/MetaWearIMUObserver.h
+++ b/source/RobotAPI/components/units/MetaWearIMUObserver.h
@@ -24,49 +24,54 @@
#pragma once
-#include <RobotAPI/interface/units/MetaWearIMU.h>
+#include <mutex>
+
#include <ArmarXCore/observers/Observer.h>
+
+#include <RobotAPI/interface/units/MetaWearIMU.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <mutex>
-
namespace armarx
{
/**
* \class MetaWearIMUObserverPropertyDefinitions
* \brief
*/
- class MetaWearIMUObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class MetaWearIMUObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- MetaWearIMUObserverPropertyDefinitions(std::string prefix):
+ MetaWearIMUObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("MetaWearTopicName", "MetaWearData", "Name of the MetaWear topic");
- defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ defineOptionalProperty<std::string>(
+ "MetaWearTopicName", "MetaWearData", "Name of the MetaWear topic");
+ defineOptionalProperty<std::string>(
+ "DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
}
};
-
-
- class MetaWearIMUObserver :
- virtual public Observer,
- virtual public MetaWearIMUObserverInterface
+ class MetaWearIMUObserver : virtual public Observer, virtual public MetaWearIMUObserverInterface
{
public:
- MetaWearIMUObserver() {}
+ MetaWearIMUObserver()
+ {
+ }
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "MetaWearIMUObserver";
}
+
void onInitObserver() override;
void onConnectObserver() override;
void onExitObserver() override;
- void reportIMUValues(const std::string& name, const MetaWearIMUData& data, const TimestampBasePtr& timestamp, const Ice::Current&) override;
+ void reportIMUValues(const std::string& name,
+ const MetaWearIMUData& data,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current&) override;
/**
* @see PropertyUser::createPropertyDefinitions()
@@ -78,9 +83,11 @@ namespace armarx
std::mutex dataMutex;
DebugDrawerInterfacePrx debugDrawerPrx;
- void offerVector3(const std::string& channelName, const std::string& dfName, const std::vector<float>& data);
- void offerQuaternion(const std::string& channelName, const std::string& dfName, const std::vector<float>& data);
-
+ void offerVector3(const std::string& channelName,
+ const std::string& dfName,
+ const std::vector<float>& data);
+ void offerQuaternion(const std::string& channelName,
+ const std::string& dfName,
+ const std::vector<float>& data);
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
index 3166408f4190460632625b1739066112fd1d2965..f5bcaececda5812fbb7d191c51588717d9c408a6 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
@@ -40,7 +40,6 @@
// ArmarX
#include <ArmarXCore/observers/variant/Variant.h>
-
namespace
{
@@ -50,7 +49,6 @@ namespace
v = std::numeric_limits<float>::infinity();
}
-
void
invalidate(Eigen::Vector2f& v)
{
@@ -58,7 +56,6 @@ namespace
v.y() = std::numeric_limits<float>::infinity();
}
-
void
invalidate(Eigen::Vector3f& v)
{
@@ -67,13 +64,13 @@ namespace
v.z() = std::numeric_limits<float>::infinity();
}
- template<typename T>
- void invalidate(std::deque<T>& d)
+ template <typename T>
+ void
+ invalidate(std::deque<T>& d)
{
d.clear();
}
-
std::string
to_string(armarx::ObstacleAvoidingPlatformUnit::control_mode mode)
{
@@ -89,11 +86,10 @@ namespace
}
}
-}
-
+} // namespace
-const std::string
-armarx::ObstacleAvoidingPlatformUnit::default_name = "ObstacleAvoidingPlatformUnit";
+const std::string armarx::ObstacleAvoidingPlatformUnit::default_name =
+ "ObstacleAvoidingPlatformUnit";
armarx::ObstacleAvoidingPlatformUnit::ObstacleAvoidingPlatformUnit() = default;
@@ -101,7 +97,6 @@ armarx::ObstacleAvoidingPlatformUnit::ObstacleAvoidingPlatformUnit() = default;
armarx::ObstacleAvoidingPlatformUnit::~ObstacleAvoidingPlatformUnit() = default;
-
void
armarx::ObstacleAvoidingPlatformUnit::onInitPlatformUnit()
{
@@ -110,7 +105,6 @@ armarx::ObstacleAvoidingPlatformUnit::onInitPlatformUnit()
ARMARX_DEBUG << "Initialized " << getName() << ".";
}
-
void
armarx::ObstacleAvoidingPlatformUnit::onStartPlatformUnit()
{
@@ -131,7 +125,6 @@ armarx::ObstacleAvoidingPlatformUnit::onStartPlatformUnit()
ARMARX_DEBUG << "Started " << getName() << ".";
}
-
void
armarx::ObstacleAvoidingPlatformUnit::onExitPlatformUnit()
{
@@ -142,23 +135,19 @@ armarx::ObstacleAvoidingPlatformUnit::onExitPlatformUnit()
ARMARX_DEBUG << "Exited " << getName() << ".";
}
-
std::string
-armarx::ObstacleAvoidingPlatformUnit::getDefaultName()
-const
+armarx::ObstacleAvoidingPlatformUnit::getDefaultName() const
{
return default_name;
}
-
void
-armarx::ObstacleAvoidingPlatformUnit::moveTo(
- const float target_pos_x,
- const float target_pos_y,
- const float target_ori,
- const float pos_reached_threshold,
- const float ori_reached_threshold,
- const Ice::Current&)
+armarx::ObstacleAvoidingPlatformUnit::moveTo(const float target_pos_x,
+ const float target_pos_y,
+ const float target_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current&)
{
using namespace simox::math;
@@ -178,13 +167,11 @@ armarx::ObstacleAvoidingPlatformUnit::moveTo(
schedule_high_level_control_loop(control_mode::position);
}
-
void
-armarx::ObstacleAvoidingPlatformUnit::move(
- const float target_vel_x,
- const float target_vel_y,
- const float target_rot_vel,
- const Ice::Current&)
+armarx::ObstacleAvoidingPlatformUnit::move(const float target_vel_x,
+ const float target_vel_y,
+ const float target_rot_vel,
+ const Ice::Current&)
{
using namespace simox::math;
@@ -202,15 +189,13 @@ armarx::ObstacleAvoidingPlatformUnit::move(
schedule_high_level_control_loop(control_mode::velocity);
}
-
void
-armarx::ObstacleAvoidingPlatformUnit::moveRelative(
- const float target_pos_delta_x,
- const float target_pos_delta_y,
- const float target_delta_ori,
- const float pos_reached_threshold,
- const float ori_reached_threshold,
- const Ice::Current& current)
+armarx::ObstacleAvoidingPlatformUnit::moveRelative(const float target_pos_delta_x,
+ const float target_pos_delta_y,
+ const float target_delta_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current& current)
{
using namespace simox::math;
@@ -222,21 +207,18 @@ armarx::ObstacleAvoidingPlatformUnit::moveRelative(
lock.unlock();
// Use moveTo.
- moveTo(
- agent_pos.x() + target_pos_delta_x,
- agent_pos.y() + target_pos_delta_y,
- agent_ori + target_delta_ori,
- pos_reached_threshold,
- ori_reached_threshold,
- current);
+ moveTo(agent_pos.x() + target_pos_delta_x,
+ agent_pos.y() + target_pos_delta_y,
+ agent_ori + target_delta_ori,
+ pos_reached_threshold,
+ ori_reached_threshold,
+ current);
}
-
void
-armarx::ObstacleAvoidingPlatformUnit::setMaxVelocities(
- const float max_pos_vel,
- const float max_rot_vel,
- const Ice::Current&)
+armarx::ObstacleAvoidingPlatformUnit::setMaxVelocities(const float max_pos_vel,
+ const float max_rot_vel,
+ const Ice::Current&)
{
std::scoped_lock l{m_control_data.mutex};
m_control_data.max_vel = max_pos_vel;
@@ -244,7 +226,6 @@ armarx::ObstacleAvoidingPlatformUnit::setMaxVelocities(
m_platform->setMaxVelocities(max_pos_vel, max_rot_vel);
}
-
void
armarx::ObstacleAvoidingPlatformUnit::stopPlatform(const Ice::Current&)
{
@@ -252,7 +233,6 @@ armarx::ObstacleAvoidingPlatformUnit::stopPlatform(const Ice::Current&)
m_platform->stopPlatform();
}
-
void
armarx::ObstacleAvoidingPlatformUnit::schedule_high_level_control_loop(control_mode mode)
{
@@ -283,12 +263,10 @@ armarx::ObstacleAvoidingPlatformUnit::schedule_high_level_control_loop(control_m
ARMARX_VERBOSE << "Starting " << ::to_string(mode) << " control.";
m_control_loop.mode = mode;
m_control_loop.task = new RunningTask<ObstacleAvoidingPlatformUnit>(
- this,
- &ObstacleAvoidingPlatformUnit::high_level_control_loop);
+ this, &ObstacleAvoidingPlatformUnit::high_level_control_loop);
m_control_loop.task->start();
}
-
void
armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop()
{
@@ -333,8 +311,7 @@ armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop()
}
catch (const std::exception& e)
{
- ARMARX_ERROR << "Error occured while running control loop.\n"
- << e.what();
+ ARMARX_ERROR << "Error occured while running control loop.\n" << e.what();
}
catch (...)
{
@@ -353,7 +330,6 @@ armarx::ObstacleAvoidingPlatformUnit::high_level_control_loop()
ARMARX_VERBOSE << "Stopped " << ::to_string(mode) << " control.";
}
-
armarx::ObstacleAvoidingPlatformUnit::velocities
armarx::ObstacleAvoidingPlatformUnit::get_velocities()
{
@@ -382,7 +358,8 @@ armarx::ObstacleAvoidingPlatformUnit::get_velocities()
ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
- ARMARX_DEBUG << "Target velocity: " << target_vel.transpose() << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
+ ARMARX_DEBUG << "Target velocity: " << target_vel.transpose()
+ << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose() << modulated_vel.norm();
const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
@@ -399,10 +376,8 @@ armarx::ObstacleAvoidingPlatformUnit::get_velocities()
return vels;
}
-
Eigen::Vector2f
-armarx::ObstacleAvoidingPlatformUnit::get_target_velocity()
-const
+armarx::ObstacleAvoidingPlatformUnit::get_target_velocity() const
{
using namespace simox::math;
@@ -423,10 +398,8 @@ const
return norm_max(uncapped_target_vel, m_control_data.max_vel);
}
-
float
-armarx::ObstacleAvoidingPlatformUnit::get_target_rotational_velocity()
-const
+armarx::ObstacleAvoidingPlatformUnit::get_target_rotational_velocity() const
{
using namespace simox::math;
@@ -448,7 +421,6 @@ const
uncapped_target_rot_vel);
}
-
void
armarx::ObstacleAvoidingPlatformUnit::update_agent_dependent_values()
{
@@ -470,11 +442,9 @@ armarx::ObstacleAvoidingPlatformUnit::update_agent_dependent_values()
ARMARX_CHECK(m_control_data.target_pos.allFinite());
ARMARX_CHECK(std::isfinite(m_control_data.target_ori));
- m_control_data.target_dist =
- (m_control_data.target_pos - m_control_data.agent_pos).norm();
- m_control_data.target_angular_dist =
- periodic_clamp<float>(m_control_data.target_ori - m_control_data.agent_ori,
- -M_PI, M_PI);
+ m_control_data.target_dist = (m_control_data.target_pos - m_control_data.agent_pos).norm();
+ m_control_data.target_angular_dist = periodic_clamp<float>(
+ m_control_data.target_ori - m_control_data.agent_ori, -M_PI, M_PI);
ARMARX_CHECK_GREATER(m_control_data.target_angular_dist, -M_PI);
ARMARX_CHECK_LESS(m_control_data.target_angular_dist, M_PI);
@@ -490,10 +460,8 @@ armarx::ObstacleAvoidingPlatformUnit::update_agent_dependent_values()
}
}
-
bool
-armarx::ObstacleAvoidingPlatformUnit::target_position_reached()
-const
+armarx::ObstacleAvoidingPlatformUnit::target_position_reached() const
{
if (m_control_loop.mode == control_mode::position)
{
@@ -504,10 +472,8 @@ const
return false;
}
-
bool
-armarx::ObstacleAvoidingPlatformUnit::target_orientation_reached()
-const
+armarx::ObstacleAvoidingPlatformUnit::target_orientation_reached() const
{
if (m_control_loop.mode == control_mode::position)
{
@@ -518,10 +484,8 @@ const
return false;
}
-
bool
-armarx::ObstacleAvoidingPlatformUnit::target_reached()
-const
+armarx::ObstacleAvoidingPlatformUnit::target_reached() const
{
if (m_control_loop.mode == control_mode::position)
{
@@ -531,11 +495,9 @@ const
return false;
}
-
Eigen::Vector2f
-armarx::ObstacleAvoidingPlatformUnit::post_process_vel(
- const Eigen::Vector2f& target_vel,
- const Eigen::Vector2f& modulated_vel)
+armarx::ObstacleAvoidingPlatformUnit::post_process_vel(const Eigen::Vector2f& target_vel,
+ const Eigen::Vector2f& modulated_vel)
{
ARMARX_CHECK(target_vel.allFinite());
ARMARX_CHECK(modulated_vel.allFinite());
@@ -555,7 +517,8 @@ armarx::ObstacleAvoidingPlatformUnit::post_process_vel(
const bool is_near_target = this->is_near_target(mean_modulated_vel);
// Min velocity is determined through distance to target.
- float min_vel = is_near_target ? m_control_data.min_vel_near_target : m_control_data.min_vel_general;
+ float min_vel =
+ is_near_target ? m_control_data.min_vel_near_target : m_control_data.min_vel_general;
if (target_vel.norm() < min_vel)
{
min_vel = target_vel.norm();
@@ -575,12 +538,14 @@ armarx::ObstacleAvoidingPlatformUnit::post_process_vel(
return simox::math::norm_clamp(mean_modulated_vel, min_vel, max_vel);
}
-
-bool armarx::ObstacleAvoidingPlatformUnit::is_near_target(const Eigen::Vector2f& control_velocity) const noexcept
+bool
+armarx::ObstacleAvoidingPlatformUnit::is_near_target(
+ const Eigen::Vector2f& control_velocity) const noexcept
{
if (m_control_data.target_dist < m_control_data.pos_near_threshold)
{
- const Eigen::Vector2f target_direction = m_control_data.target_pos - m_control_data.agent_pos;
+ const Eigen::Vector2f target_direction =
+ m_control_data.target_pos - m_control_data.agent_pos;
const Eigen::Vector2f control_direction = control_velocity / control_velocity.norm();
const float sim = simox::math::cosine_similarity(target_direction, control_direction);
@@ -595,10 +560,8 @@ bool armarx::ObstacleAvoidingPlatformUnit::is_near_target(const Eigen::Vector2f&
return false;
}
-
Eigen::Vector2f
-armarx::ObstacleAvoidingPlatformUnit::calculate_mean_modulated_vel()
-const
+armarx::ObstacleAvoidingPlatformUnit::calculate_mean_modulated_vel() const
{
const unsigned adaptive_buffer_size = calculate_adaptive_smoothing_buffer_size();
const unsigned elements =
@@ -607,24 +570,16 @@ const
std::advance(end, elements);
const auto transform =
- [elements](
- const Eigen::Vector2f & vel,
- const std::tuple<Eigen::Vector2f, Eigen::Vector2f>& vels)
- -> Eigen::Vector2f
- {
- return vel + std::get<1>(vels) * (1. / elements);
- };
-
- return std::accumulate(m_control_data.vel_history.begin(),
- end,
- Eigen::Vector2f{0, 0},
- transform);
-}
+ [elements](const Eigen::Vector2f& vel,
+ const std::tuple<Eigen::Vector2f, Eigen::Vector2f>& vels) -> Eigen::Vector2f
+ { return vel + std::get<1>(vels) * (1. / elements); };
+ return std::accumulate(
+ m_control_data.vel_history.begin(), end, Eigen::Vector2f{0, 0}, transform);
+}
unsigned
-armarx::ObstacleAvoidingPlatformUnit::calculate_adaptive_smoothing_buffer_size()
-const
+armarx::ObstacleAvoidingPlatformUnit::calculate_adaptive_smoothing_buffer_size() const
{
// buffer_size(min_buffer_size_dist) = min_buffer_size
const float min_buffer_size_dist = 200;
@@ -638,16 +593,14 @@ const
if (m_control_loop.mode == control_mode::position)
{
// Given the two points, calculate m and b in: f(x) = m * x + b
- const float m = (max_buffer_size - min_buffer_size) /
- (max_buffer_size_dist - min_buffer_size_dist);
+ const float m =
+ (max_buffer_size - min_buffer_size) / (max_buffer_size_dist - min_buffer_size_dist);
const float b = min_buffer_size - (m * min_buffer_size_dist);
-
-
// Calculate buffer size and clamp value if need be.
- return static_cast<unsigned>(std::clamp(m * m_control_data.target_dist + b,
- min_buffer_size, max_buffer_size));
+ return static_cast<unsigned>(
+ std::clamp(m * m_control_data.target_dist + b, min_buffer_size, max_buffer_size));
}
else
{
@@ -656,10 +609,8 @@ const
}
}
-
float
-armarx::ObstacleAvoidingPlatformUnit::calculate_adaptive_max_vel()
-const
+armarx::ObstacleAvoidingPlatformUnit::calculate_adaptive_max_vel() const
{
using namespace simox::math;
@@ -701,12 +652,10 @@ const
std::back_inserter(angular_similarities),
transform);
- const float mean_angular_similarity = std::accumulate(angular_similarities.begin(),
- angular_similarities.end(),
- 0.f,
- std::plus<float>());
- const float max_vel_factor = std::pow(mean_angular_similarity,
- m_control_data.adaptive_max_vel_exp);
+ const float mean_angular_similarity = std::accumulate(
+ angular_similarities.begin(), angular_similarities.end(), 0.f, std::plus<float>());
+ const float max_vel_factor =
+ std::pow(mean_angular_similarity, m_control_data.adaptive_max_vel_exp);
return max_vel_factor * m_control_data.max_vel;
}
@@ -716,7 +665,6 @@ const
}
}
-
void
armarx::ObstacleAvoidingPlatformUnit::visualize()
{
@@ -731,7 +679,6 @@ armarx::ObstacleAvoidingPlatformUnit::visualize()
visualize(vels);
}
-
void
armarx::ObstacleAvoidingPlatformUnit::visualize(const velocities& vels)
{
@@ -769,29 +716,26 @@ armarx::ObstacleAvoidingPlatformUnit::visualize(const velocities& vels)
if (not target_reached())
{
// Start.
- l_prog.add(Sphere{"start"}
- .position(m_viz.start)
- .color(Color::cyan(255, 64))
- .radius(40));
+ l_prog.add(
+ Sphere{"start"}.position(m_viz.start).color(Color::cyan(255, 64)).radius(40));
// Path.
for (unsigned i = 0; i < m_viz.path.size(); ++i)
{
l_prog.add(Sphere{"path_" + std::to_string(i + 1)}
- .position(m_viz.path[i])
- .color(Color::magenta(255, 128))
- .radius(20));
+ .position(m_viz.path[i])
+ .color(Color::magenta(255, 128))
+ .radius(20));
}
// Goal.
- const Eigen::Vector3f target{m_control_data.target_pos.x(),
- m_control_data.target_pos.y(),
- 0};
- l_prog.add(Arrow{"goal"}
- .fromTo(target + Eigen::Vector3f{0, 0, 220},
- target + Eigen::Vector3f{0, 0, 40})
- .color(Color::red(255, 128))
- .width(20));
+ const Eigen::Vector3f target{
+ m_control_data.target_pos.x(), m_control_data.target_pos.y(), 0};
+ l_prog.add(
+ Arrow{"goal"}
+ .fromTo(target + Eigen::Vector3f{0, 0, 220}, target + Eigen::Vector3f{0, 0, 40})
+ .color(Color::red(255, 128))
+ .width(20));
}
else
{
@@ -813,17 +757,17 @@ armarx::ObstacleAvoidingPlatformUnit::visualize(const velocities& vels)
if (original.norm() > min_velocity)
{
l_vels.add(Arrow{"original"}
- .fromTo(from1, from1 + original * 5)
- .color(Color::green(255, 128))
- .width(25));
+ .fromTo(from1, from1 + original * 5)
+ .color(Color::green(255, 128))
+ .width(25));
}
if (modulated.norm() > min_velocity)
{
l_vels.add(Arrow{"modulated"}
- .fromTo(from2, from2 + modulated * 5)
- .color(Color::cyan(255, 128))
- .width(25));
+ .fromTo(from2, from2 + modulated * 5)
+ .color(Color::cyan(255, 128))
+ .width(25));
}
}
@@ -831,26 +775,22 @@ armarx::ObstacleAvoidingPlatformUnit::visualize(const velocities& vels)
Layer l_agnt = arviz.layer("agent");
if (m_control_loop.mode != control_mode::none)
{
- l_agnt.add(Sphere{"agent"}
- .position(agent_pos)
- .color(Color::red(255, 128))
- .radius(50));
+ l_agnt.add(Sphere{"agent"}.position(agent_pos).color(Color::red(255, 128)).radius(50));
// Agent safety margin.
if (m_control_data.agent_safety_margin > 0)
{
l_agnt.add(Cylinder{"agent_safety_margin"}
- .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
- .color(Color::red(255, 64))
- .radius(m_control_data.agent_safety_margin)
- .height(2));
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .color(Color::red(255, 64))
+ .radius(m_control_data.agent_safety_margin)
+ .height(2));
}
}
arviz.commit({l_prog, l_vels, l_agnt});
}
-
armarx::PropertyDefinitionsPtr
armarx::ObstacleAvoidingPlatformUnit::createPropertyDefinitions()
{
@@ -860,15 +800,22 @@ armarx::ObstacleAvoidingPlatformUnit::createPropertyDefinitions()
def->component(m_obstacle_avoidance, "PlatformObstacleAvoidance");
// Settings.
- def->optional(m_control_data.adaptive_max_vel_exp, "adaptive_max_vel_exponent",
+ def->optional(m_control_data.adaptive_max_vel_exp,
+ "adaptive_max_vel_exponent",
"Adaptive max vel exponent. This throttles the max_vel adaptively "
"depending on the angle between target velocity and modulated velocity. "
"0 = disable.");
- def->optional(m_control_data.min_vel_near_target, "min_vel_near_target", "Velocity in [mm/s] "
+ def->optional(m_control_data.min_vel_near_target,
+ "min_vel_near_target",
+ "Velocity in [mm/s] "
"the robot should at least set when near the target");
- def->optional(m_control_data.min_vel_general, "min_vel_general", "Velocity in [mm/s] the robot "
+ def->optional(m_control_data.min_vel_general,
+ "min_vel_general",
+ "Velocity in [mm/s] the robot "
"should at least set on general");
- def->optional(m_control_data.pos_near_threshold, "pos_near_threshold", "Distance in [mm] after "
+ def->optional(m_control_data.pos_near_threshold,
+ "pos_near_threshold",
+ "Distance in [mm] after "
"which the robot is considered to be near the target for min velocity, "
"smoothing, etc.");
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
index 1535fdc74c809fd43518095997a51e64aa31cc23..24720fc86f299a0a53e39c93d30058b0b4443edc 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h
@@ -26,9 +26,9 @@
// STD/STL
#include <deque>
+#include <mutex>
#include <string>
#include <tuple>
-#include <mutex>
#include <vector>
// Eigen
@@ -48,10 +48,9 @@
// RobotAPI
#include <RobotAPI/components/units/PlatformUnit.h>
#include <RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.h>
-#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-
+#include <RobotAPI/libraries/core/PIDController.h>
namespace armarx
{
@@ -64,7 +63,6 @@ namespace armarx
{
public:
-
enum class control_mode
{
position,
@@ -73,7 +71,6 @@ namespace armarx
};
private:
-
struct control_loop
{
std::mutex mutex;
@@ -127,150 +124,84 @@ namespace armarx
};
public:
-
ObstacleAvoidingPlatformUnit();
- virtual
- ~ObstacleAvoidingPlatformUnit()
- override;
-
- virtual
- std::string
- getDefaultName()
- const override;
-
- virtual
- void
- moveTo(
- float target_pos_x,
- float target_pos_y,
- float target_ori,
- float pos_reached_threshold,
- float ori_reached_threshold,
- const Ice::Current& = Ice::Current{})
- override;
-
- virtual
- void
- move(
- float target_vel_x,
- float target_vel_y,
- float target_rot_vel,
- const Ice::Current& = Ice::Current{})
- override;
-
- virtual
- void
- moveRelative(
- float target_pos_delta_x,
- float target_pos_delta_y,
- float target_delta_ori,
- float pos_reached_threshold,
- float ori_reached_threshold,
- const Ice::Current& = Ice::Current{})
- override;
-
- virtual
- void
- setMaxVelocities(
- float max_pos_vel,
- float max_rot_vel,
- const Ice::Current& = Ice::Current{})
- override;
-
- virtual
- void
- stopPlatform(const Ice::Current& = Ice::Current{})
- override;
+ virtual ~ObstacleAvoidingPlatformUnit() override;
- protected:
+ virtual std::string getDefaultName() const override;
- virtual
- void
- onInitPlatformUnit()
- override;
+ virtual void moveTo(float target_pos_x,
+ float target_pos_y,
+ float target_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{}) override;
- virtual
- void
- onStartPlatformUnit()
- override;
+ virtual void move(float target_vel_x,
+ float target_vel_y,
+ float target_rot_vel,
+ const Ice::Current& = Ice::Current{}) override;
- virtual
- void
- onExitPlatformUnit()
- override;
+ virtual void moveRelative(float target_pos_delta_x,
+ float target_pos_delta_y,
+ float target_delta_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{}) override;
- virtual
- PropertyDefinitionsPtr
- createPropertyDefinitions()
- override;
+ virtual void setMaxVelocities(float max_pos_vel,
+ float max_rot_vel,
+ const Ice::Current& = Ice::Current{}) override;
- private:
+ virtual void stopPlatform(const Ice::Current& = Ice::Current{}) override;
- void
- schedule_high_level_control_loop(control_mode mode);
+ protected:
+ virtual void onInitPlatformUnit() override;
- void
- high_level_control_loop();
+ virtual void onStartPlatformUnit() override;
- velocities
- get_velocities();
+ virtual void onExitPlatformUnit() override;
- void
- update_agent_dependent_values();
+ virtual PropertyDefinitionsPtr createPropertyDefinitions() override;
- Eigen::Vector2f
- get_target_velocity()
- const;
+ private:
+ void schedule_high_level_control_loop(control_mode mode);
- float
- get_target_rotational_velocity()
- const;
+ void high_level_control_loop();
- bool
- target_position_reached()
- const;
+ velocities get_velocities();
- bool
- target_orientation_reached()
- const;
+ void update_agent_dependent_values();
- bool
- target_reached()
- const;
+ Eigen::Vector2f get_target_velocity() const;
- Eigen::Vector2f
- post_process_vel(const Eigen::Vector2f& target_vel, const Eigen::Vector2f& modulated_vel);
+ float get_target_rotational_velocity() const;
- Eigen::Vector2f
- calculate_mean_modulated_vel()
- const;
+ bool target_position_reached() const;
- unsigned
- calculate_adaptive_smoothing_buffer_size()
- const;
+ bool target_orientation_reached() const;
- float
- calculate_adaptive_max_vel()
- const;
+ bool target_reached() const;
- void
- visualize();
+ Eigen::Vector2f post_process_vel(const Eigen::Vector2f& target_vel,
+ const Eigen::Vector2f& modulated_vel);
- void
- visualize(const velocities& vels);
+ Eigen::Vector2f calculate_mean_modulated_vel() const;
- bool
- is_near_target(const Eigen::Vector2f& control_velocity)
- const
- noexcept;
+ unsigned calculate_adaptive_smoothing_buffer_size() const;
- public:
+ float calculate_adaptive_max_vel() const;
+
+ void visualize();
+
+ void visualize(const velocities& vels);
+ bool is_near_target(const Eigen::Vector2f& control_velocity) const noexcept;
+
+ public:
static const std::string default_name;
private:
-
PlatformUnitInterfacePrx m_platform;
ObstacleAvoidanceInterfacePrx m_obstacle_avoidance;
VirtualRobot::RobotPtr m_robot;
@@ -281,7 +212,6 @@ namespace armarx
mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0};
visualization m_viz;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/main.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/main.cpp
index 8dd9c74ae1fb88b27bfc9e0aab17e2be12bb451c..1715a595aa7e300f8e4e53b59439c426ea1b2e56 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/main.cpp
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/main.cpp
@@ -25,14 +25,14 @@
#include <string>
// ArmarX
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
// RobotAPI
#include <RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h>
-
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
using namespace armarx;
const std::string name = ObstacleAvoidingPlatformUnit::default_name;
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
index 5f51021681e02c7162cdc194372f199ac6e6408d..57efc2b7a33bbb8c78b4edcad69622897d4fb81f 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -41,7 +41,6 @@
// ArmarX
#include <ArmarXCore/observers/variant/Variant.h>
-
namespace
{
@@ -51,7 +50,6 @@ namespace
v = std::numeric_limits<float>::infinity();
}
-
void
invalidate(Eigen::Vector2f& v)
{
@@ -59,7 +57,6 @@ namespace
v.y() = std::numeric_limits<float>::infinity();
}
-
void
invalidate(Eigen::Vector3f& v)
{
@@ -68,13 +65,13 @@ namespace
v.z() = std::numeric_limits<float>::infinity();
}
- template<typename T>
- void invalidate(std::deque<T>& d)
+ template <typename T>
+ void
+ invalidate(std::deque<T>& d)
{
d.clear();
}
-
std::string
to_string(armarx::ObstacleAwarePlatformUnit::control_mode mode)
{
@@ -90,11 +87,9 @@ namespace
}
}
-}
-
+} // namespace
-const std::string
-armarx::ObstacleAwarePlatformUnit::default_name = "ObstacleAwarePlatformUnit";
+const std::string armarx::ObstacleAwarePlatformUnit::default_name = "ObstacleAwarePlatformUnit";
armarx::ObstacleAwarePlatformUnit::ObstacleAwarePlatformUnit() = default;
@@ -102,7 +97,6 @@ armarx::ObstacleAwarePlatformUnit::ObstacleAwarePlatformUnit() = default;
armarx::ObstacleAwarePlatformUnit::~ObstacleAwarePlatformUnit() = default;
-
void
armarx::ObstacleAwarePlatformUnit::onInitPlatformUnit()
{
@@ -111,7 +105,6 @@ armarx::ObstacleAwarePlatformUnit::onInitPlatformUnit()
ARMARX_DEBUG << "Initialized " << getName() << ".";
}
-
void
armarx::ObstacleAwarePlatformUnit::onStartPlatformUnit()
{
@@ -131,7 +124,6 @@ armarx::ObstacleAwarePlatformUnit::onStartPlatformUnit()
ARMARX_DEBUG << "Started " << getName() << ".";
}
-
void
armarx::ObstacleAwarePlatformUnit::onExitPlatformUnit()
{
@@ -142,23 +134,19 @@ armarx::ObstacleAwarePlatformUnit::onExitPlatformUnit()
ARMARX_DEBUG << "Exited " << getName() << ".";
}
-
std::string
-armarx::ObstacleAwarePlatformUnit::getDefaultName()
-const
+armarx::ObstacleAwarePlatformUnit::getDefaultName() const
{
return default_name;
}
-
void
-armarx::ObstacleAwarePlatformUnit::moveTo(
- const float target_pos_x,
- const float target_pos_y,
- const float target_ori,
- const float pos_reached_threshold,
- const float ori_reached_threshold,
- const Ice::Current&)
+armarx::ObstacleAwarePlatformUnit::moveTo(const float target_pos_x,
+ const float target_pos_y,
+ const float target_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current&)
{
using namespace simox::math;
@@ -177,13 +165,11 @@ armarx::ObstacleAwarePlatformUnit::moveTo(
schedule_high_level_control_loop(control_mode::position);
}
-
void
-armarx::ObstacleAwarePlatformUnit::move(
- const float target_vel_x,
- const float target_vel_y,
- const float target_rot_vel,
- const Ice::Current&)
+armarx::ObstacleAwarePlatformUnit::move(const float target_vel_x,
+ const float target_vel_y,
+ const float target_rot_vel,
+ const Ice::Current&)
{
using namespace simox::math;
@@ -201,15 +187,13 @@ armarx::ObstacleAwarePlatformUnit::move(
schedule_high_level_control_loop(control_mode::velocity);
}
-
void
-armarx::ObstacleAwarePlatformUnit::moveRelative(
- const float target_pos_delta_x,
- const float target_pos_delta_y,
- const float target_delta_ori,
- const float pos_reached_threshold,
- const float ori_reached_threshold,
- const Ice::Current& current)
+armarx::ObstacleAwarePlatformUnit::moveRelative(const float target_pos_delta_x,
+ const float target_pos_delta_y,
+ const float target_delta_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current& current)
{
using namespace simox::math;
@@ -221,21 +205,18 @@ armarx::ObstacleAwarePlatformUnit::moveRelative(
lock.unlock();
// Use moveTo.
- moveTo(
- agent_pos.x() + target_pos_delta_x,
- agent_pos.y() + target_pos_delta_y,
- agent_ori + target_delta_ori,
- pos_reached_threshold,
- ori_reached_threshold,
- current);
+ moveTo(agent_pos.x() + target_pos_delta_x,
+ agent_pos.y() + target_pos_delta_y,
+ agent_ori + target_delta_ori,
+ pos_reached_threshold,
+ ori_reached_threshold,
+ current);
}
-
void
-armarx::ObstacleAwarePlatformUnit::setMaxVelocities(
- float max_pos_vel,
- float max_rot_vel,
- const Ice::Current&)
+armarx::ObstacleAwarePlatformUnit::setMaxVelocities(float max_pos_vel,
+ float max_rot_vel,
+ const Ice::Current&)
{
// ensure positiveness
max_pos_vel = std::max(max_pos_vel, 0.F);
@@ -247,7 +228,6 @@ armarx::ObstacleAwarePlatformUnit::setMaxVelocities(
m_platform->setMaxVelocities(max_pos_vel, max_rot_vel);
}
-
void
armarx::ObstacleAwarePlatformUnit::stopPlatform(const Ice::Current&)
{
@@ -255,7 +235,6 @@ armarx::ObstacleAwarePlatformUnit::stopPlatform(const Ice::Current&)
m_platform->stopPlatform();
}
-
void
armarx::ObstacleAwarePlatformUnit::schedule_high_level_control_loop(control_mode mode)
{
@@ -286,12 +265,10 @@ armarx::ObstacleAwarePlatformUnit::schedule_high_level_control_loop(control_mode
ARMARX_VERBOSE << "Starting " << ::to_string(mode) << " control.";
m_control_loop.mode = mode;
m_control_loop.task = new RunningTask<ObstacleAwarePlatformUnit>(
- this,
- &ObstacleAwarePlatformUnit::high_level_control_loop);
+ this, &ObstacleAwarePlatformUnit::high_level_control_loop);
m_control_loop.task->start();
}
-
void
armarx::ObstacleAwarePlatformUnit::high_level_control_loop()
{
@@ -327,17 +304,17 @@ armarx::ObstacleAwarePlatformUnit::high_level_control_loop()
m["modulated_local_y"] = new Variant{vels.modulated_local.y()};
m["modulated_local_abs"] = new Variant{vels.modulated_local.norm()};
-// m["max_rot_vel"] = new Variant{m_control_data.max_rot_vel};
-// m["max_lin_vel"] = new Variant{m_control_data.max_vel};
+ // m["max_rot_vel"] = new Variant{m_control_data.max_rot_vel};
+ // m["max_lin_vel"] = new Variant{m_control_data.max_vel};
-// m["rot_desired"] = new Variant{m_control_data.target_ori};
-// m["rot_measured"] = new Variant{m_control_data.agent_ori};
+ // m["rot_desired"] = new Variant{m_control_data.target_ori};
+ // m["rot_measured"] = new Variant{m_control_data.agent_ori};
-// m["pos_x_desired"] = new Variant{m_control_data.target_pos.x()};
-// m["pos_x_measured"] = new Variant{m_control_data.agent_pos.y()};
+ // m["pos_x_desired"] = new Variant{m_control_data.target_pos.x()};
+ // m["pos_x_measured"] = new Variant{m_control_data.agent_pos.y()};
-// m["pos_y_desired"] = new Variant{m_control_data.target_pos.x()};
-// m["pos_y_measured"] = new Variant{m_control_data.agent_pos.y()};
+ // m["pos_y_desired"] = new Variant{m_control_data.target_pos.x()};
+ // m["pos_y_measured"] = new Variant{m_control_data.agent_pos.y()};
setDebugObserverChannel("ObstacleAwarePlatformCtrl", m);
@@ -348,8 +325,7 @@ armarx::ObstacleAwarePlatformUnit::high_level_control_loop()
}
catch (const std::exception& e)
{
- ARMARX_ERROR << "Error occured while running control loop.\n"
- << e.what();
+ ARMARX_ERROR << "Error occured while running control loop.\n" << e.what();
}
catch (...)
{
@@ -365,7 +341,6 @@ armarx::ObstacleAwarePlatformUnit::high_level_control_loop()
ARMARX_VERBOSE << "Stopped " << ::to_string(mode) << " control.";
}
-
armarx::ObstacleAwarePlatformUnit::velocities
armarx::ObstacleAwarePlatformUnit::get_velocities()
{
@@ -378,7 +353,10 @@ armarx::ObstacleAwarePlatformUnit::get_velocities()
const Eigen::Vector2f target_vel = get_target_velocity();
const float target_rot_vel = get_target_rotational_velocity();
- const Eigen::Vector2f extents = Eigen::Affine3f(m_robot->getRobotNode("PlatformCornerFrontLeft")->getPoseInRootFrame()).translation().head<2>();
+ const Eigen::Vector2f extents =
+ Eigen::Affine3f(m_robot->getRobotNode("PlatformCornerFrontLeft")->getPoseInRootFrame())
+ .translation()
+ .head<2>();
// Apply obstacle avoidance and apply post processing to get the modulated velocity.
const Eigen::Vector2f modulated_vel = [this, &target_vel, &extents]
@@ -386,7 +364,8 @@ armarx::ObstacleAwarePlatformUnit::get_velocities()
const VirtualRobot::Circle circle = VirtualRobot::projectedBoundingCircle(*m_robot);
ARMARX_DEBUG << "Circle approximation: " << circle.radius;
m_viz.boundingCircle = circle;
- const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos /*circle.center*/, circle.radius, m_control_data.target_pos);
+ const float distance = m_obsman->distanceToObstacle(
+ m_control_data.agent_pos /*circle.center*/, circle.radius, m_control_data.target_pos);
ARMARX_DEBUG << "Distance to obstacle: " << distance;
@@ -418,10 +397,8 @@ armarx::ObstacleAwarePlatformUnit::get_velocities()
return vels;
}
-
Eigen::Vector2f
-armarx::ObstacleAwarePlatformUnit::get_target_velocity()
-const
+armarx::ObstacleAwarePlatformUnit::get_target_velocity() const
{
using namespace simox::math;
@@ -442,10 +419,8 @@ const
return norm_max(uncapped_target_vel, m_control_data.max_vel);
}
-
float
-armarx::ObstacleAwarePlatformUnit::get_target_rotational_velocity()
-const
+armarx::ObstacleAwarePlatformUnit::get_target_rotational_velocity() const
{
using namespace simox::math;
@@ -463,13 +438,13 @@ const
ARMARX_CHECK(std::isfinite(uncapped_target_rot_vel));
- return std::clamp(uncapped_target_rot_vel, -m_control_data.max_rot_vel, m_control_data.max_rot_vel);
+ return std::clamp(
+ uncapped_target_rot_vel, -m_control_data.max_rot_vel, m_control_data.max_rot_vel);
// return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
// uncapped_target_rot_vel);
}
-
void
armarx::ObstacleAwarePlatformUnit::update_agent_dependent_values()
{
@@ -491,11 +466,9 @@ armarx::ObstacleAwarePlatformUnit::update_agent_dependent_values()
ARMARX_CHECK(m_control_data.target_pos.allFinite());
ARMARX_CHECK(std::isfinite(m_control_data.target_ori));
- m_control_data.target_dist =
- (m_control_data.target_pos - m_control_data.agent_pos).norm();
- m_control_data.target_angular_dist =
- periodic_clamp<float>(m_control_data.target_ori - m_control_data.agent_ori,
- -M_PI, M_PI);
+ m_control_data.target_dist = (m_control_data.target_pos - m_control_data.agent_pos).norm();
+ m_control_data.target_angular_dist = periodic_clamp<float>(
+ m_control_data.target_ori - m_control_data.agent_ori, -M_PI, M_PI);
ARMARX_CHECK_GREATER(m_control_data.target_angular_dist, -M_PI);
ARMARX_CHECK_LESS(m_control_data.target_angular_dist, M_PI);
@@ -511,10 +484,8 @@ armarx::ObstacleAwarePlatformUnit::update_agent_dependent_values()
}
}
-
bool
-armarx::ObstacleAwarePlatformUnit::target_position_reached()
-const
+armarx::ObstacleAwarePlatformUnit::target_position_reached() const
{
if (m_control_loop.mode == control_mode::position)
{
@@ -525,10 +496,8 @@ const
return false;
}
-
bool
-armarx::ObstacleAwarePlatformUnit::target_orientation_reached()
-const
+armarx::ObstacleAwarePlatformUnit::target_orientation_reached() const
{
if (m_control_loop.mode == control_mode::position)
{
@@ -539,10 +508,8 @@ const
return false;
}
-
bool
-armarx::ObstacleAwarePlatformUnit::target_reached()
-const
+armarx::ObstacleAwarePlatformUnit::target_reached() const
{
if (m_control_loop.mode == control_mode::position)
{
@@ -552,14 +519,14 @@ const
return false;
}
-
bool
-armarx::ObstacleAwarePlatformUnit::is_near_target(const Eigen::Vector2f& control_velocity)
-const noexcept
+armarx::ObstacleAwarePlatformUnit::is_near_target(
+ const Eigen::Vector2f& control_velocity) const noexcept
{
if (m_control_data.target_dist < m_control_data.pos_near_threshold)
{
- const Eigen::Vector2f target_direction = m_control_data.target_pos - m_control_data.agent_pos;
+ const Eigen::Vector2f target_direction =
+ m_control_data.target_pos - m_control_data.agent_pos;
const Eigen::Vector2f control_direction = control_velocity / control_velocity.norm();
const float sim = simox::math::cosine_similarity(target_direction, control_direction);
@@ -574,7 +541,6 @@ const noexcept
return false;
}
-
void
armarx::ObstacleAwarePlatformUnit::visualize()
{
@@ -589,7 +555,6 @@ armarx::ObstacleAwarePlatformUnit::visualize()
visualize(vels);
}
-
void
armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
{
@@ -610,11 +575,12 @@ armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
{
l_prog.add(Cylinder{"boundingCylinder"}
- .position(agent_pos)
- .color(Color::cyan(255, 64))
- .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0) * Eigen::Isometry3f(Eigen::Translation3f(0, -1000, 0)).matrix())
- .radius(m_viz.boundingCircle.radius)
- .height(2000));
+ .position(agent_pos)
+ .color(Color::cyan(255, 64))
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0) *
+ Eigen::Isometry3f(Eigen::Translation3f(0, -1000, 0)).matrix())
+ .radius(m_viz.boundingCircle.radius)
+ .height(2000));
}
// If no start is set, use current agent pos.
@@ -636,29 +602,26 @@ armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
if (not target_reached())
{
// Start.
- l_prog.add(Sphere{"start"}
- .position(m_viz.start)
- .color(Color::cyan(255, 64))
- .radius(40));
+ l_prog.add(
+ Sphere{"start"}.position(m_viz.start).color(Color::cyan(255, 64)).radius(40));
// Path.
for (unsigned i = 0; i < m_viz.path.size(); ++i)
{
l_prog.add(Sphere{"path_" + std::to_string(i + 1)}
- .position(m_viz.path[i])
- .color(Color::magenta(255, 128))
- .radius(20));
+ .position(m_viz.path[i])
+ .color(Color::magenta(255, 128))
+ .radius(20));
}
// Goal.
- const Eigen::Vector3f target{m_control_data.target_pos.x(),
- m_control_data.target_pos.y(),
- 0};
- l_prog.add(Arrow{"goal"}
- .fromTo(target + Eigen::Vector3f{0, 0, 220},
- target + Eigen::Vector3f{0, 0, 40})
- .color(Color::red(255, 128))
- .width(20));
+ const Eigen::Vector3f target{
+ m_control_data.target_pos.x(), m_control_data.target_pos.y(), 0};
+ l_prog.add(
+ Arrow{"goal"}
+ .fromTo(target + Eigen::Vector3f{0, 0, 220}, target + Eigen::Vector3f{0, 0, 40})
+ .color(Color::red(255, 128))
+ .width(20));
}
else
{
@@ -680,17 +643,17 @@ armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
if (original.norm() > min_velocity)
{
l_vels.add(Arrow{"original"}
- .fromTo(from1, from1 + original * 5)
- .color(Color::green(255, 128))
- .width(25));
+ .fromTo(from1, from1 + original * 5)
+ .color(Color::green(255, 128))
+ .width(25));
}
if (modulated.norm() > min_velocity)
{
l_vels.add(Arrow{"modulated"}
- .fromTo(from2, from2 + modulated * 5)
- .color(Color::cyan(255, 128))
- .width(25));
+ .fromTo(from2, from2 + modulated * 5)
+ .color(Color::cyan(255, 128))
+ .width(25));
}
// TODO rotation arrow
@@ -700,26 +663,22 @@ armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
Layer l_agnt = arviz.layer("agent");
if (m_control_loop.mode != control_mode::none)
{
- l_agnt.add(Sphere{"agent"}
- .position(agent_pos)
- .color(Color::red(255, 128))
- .radius(50));
+ l_agnt.add(Sphere{"agent"}.position(agent_pos).color(Color::red(255, 128)).radius(50));
// Agent safety margin.
if (m_control_data.agent_safety_margin > 0)
{
l_agnt.add(Cylinder{"agent_safety_margin"}
- .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
- .color(Color::red(255, 64))
- .radius(m_control_data.agent_safety_margin)
- .height(2));
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .color(Color::red(255, 64))
+ .radius(m_control_data.agent_safety_margin)
+ .height(2));
}
}
arviz.commit({l_prog, l_vels, l_agnt});
}
-
armarx::PropertyDefinitionsPtr
armarx::ObstacleAwarePlatformUnit::createPropertyDefinitions()
{
@@ -729,11 +688,17 @@ armarx::ObstacleAwarePlatformUnit::createPropertyDefinitions()
def->component(m_obsman, "ObstacleAvoidingPlatformUnit");
// Settings.
- def->optional(m_control_data.min_vel_near_target, "min_vel_near_target", "Velocity in [mm/s] "
+ def->optional(m_control_data.min_vel_near_target,
+ "min_vel_near_target",
+ "Velocity in [mm/s] "
"the robot should at least set when near the target");
- def->optional(m_control_data.min_vel_general, "min_vel_general", "Velocity in [mm/s] the robot "
+ def->optional(m_control_data.min_vel_general,
+ "min_vel_general",
+ "Velocity in [mm/s] the robot "
"should at least set on general");
- def->optional(m_control_data.pos_near_threshold, "pos_near_threshold", "Distance in [mm] after "
+ def->optional(m_control_data.pos_near_threshold,
+ "pos_near_threshold",
+ "Distance in [mm] after "
"which the robot is considered to be near the target for min velocity, "
"smoothing, etc.");
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
index 7ddf5b4a09a50db16e0c7ee68ab191d6738d9bb7..7c3ed39123ff1806e0e648ae90040ac9c269c875 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
@@ -25,8 +25,8 @@
// STD/STL
-#include <string>
#include <mutex>
+#include <string>
#include <vector>
// Eigen
@@ -36,8 +36,8 @@
#include <IceUtil/Time.h>
// Simox
-#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Safety.h>
+#include <VirtualRobot/VirtualRobot.h>
// ArmarX
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
@@ -47,10 +47,9 @@
// RobotAPI
#include <RobotAPI/components/units/PlatformUnit.h>
#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
-#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-
+#include <RobotAPI/libraries/core/PIDController.h>
namespace armarx
{
@@ -63,7 +62,6 @@ namespace armarx
{
public:
-
enum class control_mode
{
position,
@@ -72,7 +70,6 @@ namespace armarx
};
private:
-
struct control_loop
{
std::mutex mutex;
@@ -123,124 +120,75 @@ namespace armarx
};
public:
-
ObstacleAwarePlatformUnit();
- ~ObstacleAwarePlatformUnit()
- override;
-
- std::string
- getDefaultName()
- const override;
-
- void
- moveTo(
- float target_pos_x,
- float target_pos_y,
- float target_ori,
- float pos_reached_threshold,
- float ori_reached_threshold,
- const Ice::Current& = Ice::Current{})
- override;
-
- void
- move(
- float target_vel_x,
- float target_vel_y,
- float target_rot_vel,
- const Ice::Current& = Ice::Current{})
- override;
-
- void
- moveRelative(
- float target_pos_delta_x,
- float target_pos_delta_y,
- float target_delta_ori,
- float pos_reached_threshold,
- float ori_reached_threshold,
- const Ice::Current& = Ice::Current{})
- override;
-
- void
- setMaxVelocities(
- float max_pos_vel,
- float max_rot_vel,
- const Ice::Current& = Ice::Current{})
- override;
-
- void
- stopPlatform(const Ice::Current& = Ice::Current{})
- override;
+ ~ObstacleAwarePlatformUnit() override;
- protected:
+ std::string getDefaultName() const override;
- void
- onInitPlatformUnit()
- override;
+ void moveTo(float target_pos_x,
+ float target_pos_y,
+ float target_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{}) override;
- void
- onStartPlatformUnit()
- override;
+ void move(float target_vel_x,
+ float target_vel_y,
+ float target_rot_vel,
+ const Ice::Current& = Ice::Current{}) override;
- void
- onExitPlatformUnit()
- override;
+ void moveRelative(float target_pos_delta_x,
+ float target_pos_delta_y,
+ float target_delta_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{}) override;
- PropertyDefinitionsPtr
- createPropertyDefinitions()
- override;
+ void setMaxVelocities(float max_pos_vel,
+ float max_rot_vel,
+ const Ice::Current& = Ice::Current{}) override;
- private:
+ void stopPlatform(const Ice::Current& = Ice::Current{}) override;
- void
- schedule_high_level_control_loop(control_mode mode);
+ protected:
+ void onInitPlatformUnit() override;
- void
- high_level_control_loop();
+ void onStartPlatformUnit() override;
- velocities
- get_velocities();
+ void onExitPlatformUnit() override;
- void
- update_agent_dependent_values();
+ PropertyDefinitionsPtr createPropertyDefinitions() override;
- Eigen::Vector2f
- get_target_velocity()
- const;
+ private:
+ void schedule_high_level_control_loop(control_mode mode);
- float
- get_target_rotational_velocity()
- const;
+ void high_level_control_loop();
- bool
- target_position_reached()
- const;
+ velocities get_velocities();
- bool
- target_orientation_reached()
- const;
+ void update_agent_dependent_values();
- bool
- target_reached()
- const;
+ Eigen::Vector2f get_target_velocity() const;
- void
- visualize();
+ float get_target_rotational_velocity() const;
- void
- visualize(const velocities& vels);
+ bool target_position_reached() const;
- bool
- is_near_target(const Eigen::Vector2f& control_velocity)
- const
- noexcept;
+ bool target_orientation_reached() const;
- public:
+ bool target_reached() const;
+
+ void visualize();
+ void visualize(const velocities& vels);
+
+ bool is_near_target(const Eigen::Vector2f& control_velocity) const noexcept;
+
+ public:
static const std::string default_name;
private:
-
PlatformUnitInterfacePrx m_platform;
VirtualRobot::RobotPtr m_robot;
DynamicObstacleManagerInterfacePrx m_obsman;
@@ -248,10 +196,14 @@ namespace armarx
ObstacleAwarePlatformUnit::control_loop m_control_loop;
ObstacleAwarePlatformUnit::control_data m_control_data;
- mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), true};
+ mutable PIDController m_rot_pid_controller{1.0,
+ 0.00009,
+ 0.0,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ true};
visualization m_viz;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
index f168208f6734ea228aa848dcf0c5d0976370bae7..4f0ae0c29366e581d87565c91d91040b523eb702 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
@@ -25,14 +25,14 @@
#include <string>
// ArmarX
-#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/application/Application.h>
// RobotAPI
#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
-
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
using namespace armarx;
const std::string name = ObstacleAwarePlatformUnit::default_name;
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
index f87a5b56edb87cc841b71da64a5018d42ace1274..442e85687b33c228d0f8acc88699d5d36b9782dc 100644
--- a/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.cpp
@@ -23,21 +23,18 @@
*/
#include "OptoForceUnitObserver.h"
-
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
-
-#include <RobotAPI/libraries/core/Pose.h>
-
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx
{
- void OptoForceUnitObserver::onInitObserver()
+ void
+ OptoForceUnitObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("OptoForceTopicName").getValue());
@@ -49,21 +46,28 @@ namespace armarx
offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
}
-
-
- void OptoForceUnitObserver::onConnectObserver()
+ void
+ OptoForceUnitObserver::onConnectObserver()
{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(
+ getProperty<std::string>("DebugDrawerTopic").getValue());
}
-
- void OptoForceUnitObserver::onExitObserver()
+ void
+ OptoForceUnitObserver::onExitObserver()
{
//debugDrawerPrx->removePoseVisu("IMU", "orientation");
//debugDrawerPrx->removeLineVisu("IMU", "acceleration");
}
- void OptoForceUnitObserver::reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c)
+ void
+ OptoForceUnitObserver::reportSensorValues(const std::string& device,
+ const std::string& name,
+ float fx,
+ float fy,
+ float fz,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
@@ -129,18 +133,19 @@ namespace armarx
debugDrawerPrx->setBoxDebugLayerVisu("floor", new Pose(), new Vector3(5000, 5000, 1), DrawColor {0.7f, 0.7f, 0.7f, 1.0f});
}*/
- void OptoForceUnitObserver::offerValue(std::string device, std::string fieldName, Vector3Ptr vec)
+ void
+ OptoForceUnitObserver::offerValue(std::string device, std::string fieldName, Vector3Ptr vec)
{
offerOrUpdateDataField(device, fieldName, vec, fieldName + " values");
offerOrUpdateDataField(device, fieldName + "_x", vec->x, fieldName + "_x value");
offerOrUpdateDataField(device, fieldName + "_y", vec->y, fieldName + "_y value");
offerOrUpdateDataField(device, fieldName + "_z", vec->z, fieldName + "_z value");
-
}
-
- PropertyDefinitionsPtr OptoForceUnitObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ OptoForceUnitObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new OptoForceUnitObserverPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new OptoForceUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/OptoForceUnitObserver.h b/source/RobotAPI/components/units/OptoForceUnitObserver.h
index 0d415cb2bc89396f9fc0d81669009f034b17c646..da62411ff971e85c420dc9b6e74d8f7547859d9c 100644
--- a/source/RobotAPI/components/units/OptoForceUnitObserver.h
+++ b/source/RobotAPI/components/units/OptoForceUnitObserver.h
@@ -24,32 +24,33 @@
#pragma once
-#include <RobotAPI/interface/units/OptoForceUnit.h>
+#include <mutex>
+
#include <ArmarXCore/observers/Observer.h>
+
+#include <RobotAPI/interface/units/OptoForceUnit.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <mutex>
-
namespace armarx
{
/**
* \class OptoForceUnitObserverPropertyDefinitions
* \brief
*/
- class OptoForceUnitObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class OptoForceUnitObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- OptoForceUnitObserverPropertyDefinitions(std::string prefix):
+ OptoForceUnitObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("OptoForceTopicName", "OptoForceValues", "Name of the OptoForce Topic");
- defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ defineOptionalProperty<std::string>(
+ "OptoForceTopicName", "OptoForceValues", "Name of the OptoForce Topic");
+ defineOptionalProperty<std::string>(
+ "DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
}
};
-
/**
* \class OptoForceUnitObserver
* \ingroup RobotAPI-SensorActorUnits-observers
@@ -63,17 +64,27 @@ namespace armarx
virtual public OptoForceUnitObserverInterface
{
public:
- OptoForceUnitObserver() {}
+ OptoForceUnitObserver()
+ {
+ }
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "OptoForceUnitObserver";
}
+
void onInitObserver() override;
void onConnectObserver() override;
void onExitObserver() override;
- void reportSensorValues(const std::string& device, const std::string& name, float fx, float fy, float fz, const TimestampBasePtr& timestamp, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportSensorValues(const std::string& device,
+ const std::string& name,
+ float fx,
+ float fy,
+ float fz,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/**
* @see PropertyUser::createPropertyDefinitions()
@@ -88,5 +99,4 @@ namespace armarx
void offerValue(std::string device, std::string fieldName, Vector3Ptr vec);
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
index 75640877635972e5cbf11f078580487834f64d4d..8f9dbfec7a804c84cae8fe78c26d94591e150c08 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
@@ -23,41 +23,52 @@
*/
#include "OrientedTactileSensorUnitObserver.h"
-
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
-
-#include <RobotAPI/libraries/core/Pose.h>
-
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx
{
- void OrientedTactileSensorUnitObserver::onInitObserver()
+ void
+ OrientedTactileSensorUnitObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("SensorTopicName").getValue());
offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
}
-
-
- void OrientedTactileSensorUnitObserver::onConnectObserver()
+ void
+ OrientedTactileSensorUnitObserver::onConnectObserver()
{
- debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(
+ getProperty<std::string>("DebugDrawerTopic").getValue());
}
-
- void OrientedTactileSensorUnitObserver::onExitObserver()
+ void
+ OrientedTactileSensorUnitObserver::onExitObserver()
{
//debugDrawerPrx->removePoseVisu("IMU", "orientation");
//debugDrawerPrx->removeLineVisu("IMU", "acceleration");
}
- void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, const TimestampBasePtr& timestamp, const Ice::Current&)
+ void
+ OrientedTactileSensorUnitObserver::reportSensorValues(int id,
+ float pressure,
+ float qw,
+ float qx,
+ float qy,
+ float qz,
+ float pressureRate,
+ float rotationRate,
+ float accelerationRate,
+ float accelx,
+ float accely,
+ float accelz,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
@@ -72,13 +83,21 @@ namespace armarx
}
offerOrUpdateDataField(channelName, "pressure", Variant(pressure), "current pressure");
- QuaternionPtr orientationQuaternion = new Quaternion(qw, qx, qy, qz);
- offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current orientation");
- offerOrUpdateDataField(channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
- offerOrUpdateDataField(channelName, "pressureRate", Variant(pressureRate), "current pressureRate");
- offerOrUpdateDataField(channelName, "accelerationRate", Variant(accelerationRate), "current accelerationRate");
- offerOrUpdateDataField(channelName, "linear acceleration", Variant(new Vector3(accelx, accely, accelz)), "current linear acceleration");
+ QuaternionPtr orientationQuaternion = new Quaternion(qw, qx, qy, qz);
+ offerOrUpdateDataField(
+ channelName, "orientation", orientationQuaternion, "current orientation");
+ offerOrUpdateDataField(
+ channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
+ offerOrUpdateDataField(
+ channelName, "pressureRate", Variant(pressureRate), "current pressureRate");
+ offerOrUpdateDataField(
+ channelName, "accelerationRate", Variant(accelerationRate), "current accelerationRate");
+ offerOrUpdateDataField(channelName,
+ "linear acceleration",
+ Variant(new Vector3(accelx, accely, accelz)),
+ "current linear acceleration");
}
+
/* TODO: for integration with debug drawer
updateChannel(device);
@@ -102,8 +121,10 @@ namespace armarx
*/
- PropertyDefinitionsPtr OrientedTactileSensorUnitObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ OrientedTactileSensorUnitObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new OrientedTactileSensorUnitObserverPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new OrientedTactileSensorUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
index 70916163f96bfa87944269e5bcf3f85a88bb5278..761d73c24314bf8b993f0dee7cb73fd4556465db 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
@@ -24,32 +24,33 @@
#pragma once
-#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
+#include <mutex>
+
#include <ArmarXCore/observers/Observer.h>
+
+#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <mutex>
-
namespace armarx
{
/**
* \class OrientedTactileSensorUnitObserverPropertyDefinitions
* \brief
*/
- class OrientedTactileSensorUnitObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class OrientedTactileSensorUnitObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- OrientedTactileSensorUnitObserverPropertyDefinitions(std::string prefix):
+ OrientedTactileSensorUnitObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("SensorTopicName", "OrientedTactileSensorValues", "Name of the Sensor Topic.");
- defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
+ defineOptionalProperty<std::string>(
+ "SensorTopicName", "OrientedTactileSensorValues", "Name of the Sensor Topic.");
+ defineOptionalProperty<std::string>(
+ "DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
}
};
-
/**
* \class OrientedTactileSensorUnitObserver
* \ingroup RobotAPI-SensorActorUnits-observers
@@ -62,17 +63,34 @@ namespace armarx
virtual public OrientedTactileSensorUnitObserverInterface
{
public:
- OrientedTactileSensorUnitObserver() {}
+ OrientedTactileSensorUnitObserver()
+ {
+ }
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "OrientedTactileSensorUnitObserver";
}
+
void onInitObserver() override;
void onConnectObserver() override;
void onExitObserver() override;
- void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, const TimestampBasePtr& timestamp, const Ice::Current&) override;
+ void reportSensorValues(int id,
+ float pressure,
+ float qw,
+ float qx,
+ float qy,
+ float qz,
+ float pressureRate,
+ float rotationRate,
+ float accelerationRate,
+ float accelx,
+ float accely,
+ float accelz,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current&) override;
/**
* @see PropertyUser::createPropertyDefinitions()
@@ -84,5 +102,4 @@ namespace armarx
std::mutex dataMutex;
DebugDrawerInterfacePrx debugDrawerPrx;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/PlatformUnit.cpp b/source/RobotAPI/components/units/PlatformUnit.cpp
index a5414a34e838b6fd2268f7bfb26ddc527530624f..1d6c2a024f0f89ddb28c80c3661b363387cdb567 100644
--- a/source/RobotAPI/components/units/PlatformUnit.cpp
+++ b/source/RobotAPI/components/units/PlatformUnit.cpp
@@ -25,16 +25,17 @@
#include "PlatformUnit.h"
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
namespace armarx
{
- PropertyDefinitionsPtr PlatformUnit::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ PlatformUnit::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr def = new PlatformUnitPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr def =
+ new PlatformUnitPropertyDefinitions(getConfigIdentifier());
def->topic(odometryPrx);
def->topic(globalPosePrx);
@@ -45,38 +46,44 @@ namespace armarx
return def;
}
-
- void PlatformUnit::onInitComponent()
+ void
+ PlatformUnit::onInitComponent()
{
std::string platformName = getProperty<std::string>("PlatformName").getValue();
this->onInitPlatformUnit();
}
-
- void PlatformUnit::onConnectComponent()
+ void
+ PlatformUnit::onConnectComponent()
{
this->onStartPlatformUnit();
}
-
- void PlatformUnit::onDisconnectComponent()
+ void
+ PlatformUnit::onDisconnectComponent()
{
this->onStopPlatformUnit();
}
-
- void PlatformUnit::onExitComponent()
+ void
+ PlatformUnit::onExitComponent()
{
this->onExitPlatformUnit();
}
-
- void PlatformUnit::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
+ void
+ PlatformUnit::moveTo(Ice::Float targetPlatformPositionX,
+ Ice::Float targetPlatformPositionY,
+ Ice::Float targetPlatformRotation,
+ Ice::Float positionalAccuracy,
+ Ice::Float orientationalAccuracy,
+ const Ice::Current& c)
{
}
- PlatformPose toPlatformPose(const TransformStamped& transformStamped)
+ PlatformPose
+ toPlatformPose(const TransformStamped& transformStamped)
{
const float yaw = simox::math::mat4f_to_rpy(transformStamped.transform).z();
const Eigen::Affine3f pose(transformStamped.transform);
diff --git a/source/RobotAPI/components/units/PlatformUnit.h b/source/RobotAPI/components/units/PlatformUnit.h
index ee3c2a6999c0ba4f1f9560106f818826162c432f..23f9a7856e29541a4b19f1b24aba2c7ef8a35f88 100644
--- a/source/RobotAPI/components/units/PlatformUnit.h
+++ b/source/RobotAPI/components/units/PlatformUnit.h
@@ -24,16 +24,15 @@
#pragma once
-#include <RobotAPI/components/units/SensorActorUnit.h>
+#include <vector>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
-#include <vector>
-
namespace armarx
{
@@ -41,18 +40,18 @@ namespace armarx
* \class PlatformUnitPropertyDefinitions
* \brief Defines all necessary properties for armarx::PlatformUnit
*/
- class PlatformUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class PlatformUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- PlatformUnitPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ PlatformUnitPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("PlatformName", "Platform", "Name of the platform (will publish values on PlatformName + 'State')");
+ defineOptionalProperty<std::string>(
+ "PlatformName",
+ "Platform",
+ "Name of the platform (will publish values on PlatformName + 'State')");
}
};
-
/**
* \defgroup Component-PlatformUnit PlatformUnit
* \ingroup RobotAPI-SensorActorUnits
@@ -67,13 +66,12 @@ namespace armarx
* @ingroup Component-PlatformUnit
* @brief The PlatformUnit class
*/
- class PlatformUnit :
- virtual public PlatformUnitInterface,
- virtual public SensorActorUnit
+ class PlatformUnit : virtual public PlatformUnitInterface, virtual public SensorActorUnit
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "PlatformUnit";
}
@@ -99,24 +97,36 @@ namespace armarx
virtual void onInitPlatformUnit() = 0;
virtual void onStartPlatformUnit() = 0;
- virtual void onStopPlatformUnit() {}
+
+ virtual void
+ onStopPlatformUnit()
+ {
+ }
+
virtual void onExitPlatformUnit() = 0;
/**
* Set a new target position and orientation for the platform.
* The platform will move until it reaches the specified target with the specified accuracy.
*/
- void moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation,
- Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = Ice::emptyCurrent) override;
+ void moveTo(Ice::Float targetPlatformPositionX,
+ Ice::Float targetPlatformPositionY,
+ Ice::Float targetPlatformRotation,
+ Ice::Float positionalAccuracy,
+ Ice::Float orientationalAccuracy,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+
+ void
+ stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override
+ {
+ }
- void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override {}
/**
* \see armarx::PropertyUser::createPropertyDefinitions()
*/
PropertyDefinitionsPtr createPropertyDefinitions() override;
protected:
-
// std::string listenerChannelName;
/**
* PlatformUnitListener proxy for publishing state updates
@@ -127,9 +137,7 @@ namespace armarx
OdometryListenerPrx odometryPrx;
RobotStateComponentInterfacePrx robotStateComponent;
-
-
};
PlatformPose toPlatformPose(const TransformStamped& transformStamped);
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.cpp b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
index e4b4d4de1139392d9a8e7cd868c58eacdd5a0314..64c8271344c54c404cf6d2557b8a281a6bcdd625 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
@@ -21,25 +21,26 @@
*/
#include "PlatformUnitObserver.h"
-#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
-#include "PlatformUnit.h"
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
-#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
+#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
#include <ArmarXCore/observers/checks/ConditionCheckValid.h>
-#include <ArmarXCore/observers/checks/ConditionCheckEqualsWithTolerance.h>
+#include "PlatformUnit.h"
namespace armarx
{
// ********************************************************************
// observer framework hooks
// ********************************************************************
- void PlatformUnitObserver::onInitObserver()
+ void
+ PlatformUnitObserver::onInitObserver()
{
// TODO: check if platformNodeName exists?
platformNodeName = getProperty<std::string>("PlatformName").getValue();
@@ -58,7 +59,8 @@ namespace armarx
usingTopic("GlobalRobotPoseLocalization");
}
- void PlatformUnitObserver::onConnectObserver()
+ void
+ PlatformUnitObserver::onConnectObserver()
{
// register all channels
offerChannel("platformPose", "Current Position of " + platformNodeName);
@@ -66,27 +68,55 @@ namespace armarx
offerChannel("platformOdometryPose", "Current Odometry Position of " + platformNodeName);
// register all data fields
- offerDataField("platformPose", "positionX", VariantType::Float, "Current X position of " + platformNodeName + " in mm");
- offerDataField("platformPose", "positionY", VariantType::Float, "Current Y position of " + platformNodeName + " in mm");
- offerDataField("platformPose", "rotation", VariantType::Float, "Current Rotation of " + platformNodeName + " in radian");
-
- offerDataField("platformVelocity", "velocityX", VariantType::Float, "Current X velocity of " + platformNodeName + " in mm/s");
- offerDataField("platformVelocity", "velocityY", VariantType::Float, "Current Y velocity of " + platformNodeName + " in mm/s");
- offerDataField("platformVelocity", "velocityRotation", VariantType::Float, "Current Rotation velocity of " + platformNodeName + " in radian/s");
-
- offerDataField("platformOdometryPose", "positionX", VariantType::Float, "Current Odometry X position of " + platformNodeName + " in mm");
- offerDataField("platformOdometryPose", "positionY", VariantType::Float, "Current Odometry Y position of " + platformNodeName + " in mm");
- offerDataField("platformOdometryPose", "rotation", VariantType::Float, "Current Odometry Rotation of " + platformNodeName + " in radian");
+ offerDataField("platformPose",
+ "positionX",
+ VariantType::Float,
+ "Current X position of " + platformNodeName + " in mm");
+ offerDataField("platformPose",
+ "positionY",
+ VariantType::Float,
+ "Current Y position of " + platformNodeName + " in mm");
+ offerDataField("platformPose",
+ "rotation",
+ VariantType::Float,
+ "Current Rotation of " + platformNodeName + " in radian");
+
+ offerDataField("platformVelocity",
+ "velocityX",
+ VariantType::Float,
+ "Current X velocity of " + platformNodeName + " in mm/s");
+ offerDataField("platformVelocity",
+ "velocityY",
+ VariantType::Float,
+ "Current Y velocity of " + platformNodeName + " in mm/s");
+ offerDataField("platformVelocity",
+ "velocityRotation",
+ VariantType::Float,
+ "Current Rotation velocity of " + platformNodeName + " in radian/s");
+
+ offerDataField("platformOdometryPose",
+ "positionX",
+ VariantType::Float,
+ "Current Odometry X position of " + platformNodeName + " in mm");
+ offerDataField("platformOdometryPose",
+ "positionY",
+ VariantType::Float,
+ "Current Odometry Y position of " + platformNodeName + " in mm");
+ offerDataField("platformOdometryPose",
+ "rotation",
+ VariantType::Float,
+ "Current Odometry Rotation of " + platformNodeName + " in radian");
// odometry pose is always zero in the beginning - set it so that it can be queried
reportPlatformOdometryPose(0, 0, 0, Ice::emptyCurrent);
-
}
- void PlatformUnitObserver::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&)
+ void
+ PlatformUnitObserver::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped,
+ const ::Ice::Current&)
{
const Eigen::Isometry3f global_T_robot(transformStamped.transform);
-
+
const float x = global_T_robot.translation().x();
const float y = global_T_robot.translation().y();
const float rotationAroundZ = simox::math::mat3f_to_rpy(global_T_robot.linear()).z();
@@ -95,7 +125,11 @@ namespace armarx
updateChannel("platformPose");
}
- void PlatformUnitObserver::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
+ void
+ PlatformUnitObserver::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX,
+ ::Ice::Float currentPlatformVelocityY,
+ ::Ice::Float currentPlatformVelocityRotation,
+ const Ice::Current& c)
{
setDataField("platformVelocity", "velocityX", currentPlatformVelocityX);
setDataField("platformVelocity", "velocityY", currentPlatformVelocityY);
@@ -106,25 +140,33 @@ namespace armarx
// ********************************************************************
// private methods
// ********************************************************************
- void PlatformUnitObserver::nameValueMapToDataFields(std::string channelName, ::Ice::Float platformPositionX, ::Ice::Float platformPositionY, ::Ice::Float platformRotation)
+ void
+ PlatformUnitObserver::nameValueMapToDataFields(std::string channelName,
+ ::Ice::Float platformPositionX,
+ ::Ice::Float platformPositionY,
+ ::Ice::Float platformRotation)
{
setDataField(channelName, "positionX", platformPositionX);
setDataField(channelName, "positionY", platformPositionY);
setDataField(channelName, "rotation", platformRotation);
}
- PropertyDefinitionsPtr PlatformUnitObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ PlatformUnitObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new PlatformUnitObserverPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new PlatformUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
-
- void armarx::PlatformUnitObserver::reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&)
+ void
+ armarx::PlatformUnitObserver::reportPlatformOdometryPose(Ice::Float x,
+ Ice::Float y,
+ Ice::Float angle,
+ const Ice::Current&)
{
nameValueMapToDataFields("platformOdometryPose", x, y, angle);
updateChannel("platformOdometryPose");
}
-
-
-}
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.h b/source/RobotAPI/components/units/PlatformUnitObserver.h
index 20cdfefcd7c380eccf2e1144b3d4206b239cf088..5d48d884891fc6b92b0888dd43d6391dd1f5395c 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.h
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.h
@@ -22,18 +22,17 @@
#pragma once
-#include <ArmarXCore/core/system/ImportExport.h>
-#include <ArmarXCore/core/Component.h>
+#include <string>
-#include <RobotAPI/interface/core/RobotLocalization.h>
-#include <RobotAPI/interface/observers/PlatformUnitObserverInterface.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
-
#include <ArmarXCore/observers/ConditionCheck.h>
#include <ArmarXCore/observers/Observer.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <string>
+#include <RobotAPI/interface/core/RobotLocalization.h>
+#include <RobotAPI/interface/observers/PlatformUnitObserverInterface.h>
namespace armarx
{
@@ -45,14 +44,16 @@ namespace armarx
* \class PlatformUnitPropertyDefinitions
* \brief Defines all necessary properties for armarx::PlatformUnit
*/
- class PlatformUnitObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class PlatformUnitObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- PlatformUnitObserverPropertyDefinitions(std::string prefix):
+ PlatformUnitObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("PlatformName", "Platform", "Name of the platform (will publish values on PlatformName + 'State')");
+ defineOptionalProperty<std::string>(
+ "PlatformName",
+ "Platform",
+ "Name of the platform (will publish values on PlatformName + 'State')");
}
};
@@ -79,14 +80,21 @@ namespace armarx
void onConnectObserver() override;
// slice interface implementation
- void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&) override;
+ void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX,
+ ::Ice::Float currentPlatformVelocityY,
+ ::Ice::Float currentPlatformVelocityRotation,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportPlatformOdometryPose(Ice::Float x,
+ Ice::Float y,
+ Ice::Float angle,
+ const Ice::Current&) override;
// slice interface implementation
- void reportGlobalRobotPose(const ::armarx::TransformStamped&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
-
+ void reportGlobalRobotPose(const ::armarx::TransformStamped&,
+ const ::Ice::Current& = ::Ice::emptyCurrent) override;
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "PlatformUnitObserver";
}
@@ -97,11 +105,13 @@ namespace armarx
PropertyDefinitionsPtr createPropertyDefinitions() override;
protected:
- void nameValueMapToDataFields(std::string channelName, ::Ice::Float platformPositionX, ::Ice::Float platformPositionY, ::Ice::Float platformRotation);
+ void nameValueMapToDataFields(std::string channelName,
+ ::Ice::Float platformPositionX,
+ ::Ice::Float platformPositionY,
+ ::Ice::Float platformRotation);
private:
std::string platformNodeName;
-
};
/**
@@ -112,9 +122,9 @@ namespace armarx
class PlatformUnitDatafieldCreator
{
public:
- PlatformUnitDatafieldCreator(const std::string& channelName) :
- channelName(channelName)
- {}
+ PlatformUnitDatafieldCreator(const std::string& channelName) : channelName(channelName)
+ {
+ }
/**
* \brief Function to create a Channel Representation for a PlatformUnitObserver
@@ -123,7 +133,9 @@ namespace armarx
* in the simox-robot-xml-file
* \return returns a ChannelRepresentationPtr
*/
- DataFieldIdentifier getDatafield(const std::string& platformUnitObserverName, const std::string& platformName) const
+ DataFieldIdentifier
+ getDatafield(const std::string& platformUnitObserverName,
+ const std::string& platformName) const
{
if (platformUnitObserverName.empty())
{
@@ -141,10 +153,10 @@ namespace armarx
private:
std::string channelName;
};
-}
+} // namespace armarx
namespace armarx::channels::PlatformUnitObserver
{
const PlatformUnitDatafieldCreator platformPose("platformPose");
const PlatformUnitDatafieldCreator platformVelocity("platformVelocity");
-}
+} // namespace armarx::channels::PlatformUnitObserver
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 6abaa676025df57bbfbbde4f61ad147ac54e97eb..72cb6c5c9bd2243b15b137cd3ab4bfff95d06f6d 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -24,43 +24,61 @@
#include "PlatformUnitSimulation.h"
-#include <RobotAPI/components/units/PlatformUnit.h>
-#include <RobotAPI/interface/core/GeometryBase.h>
#include <cmath>
#include <Eigen/Geometry>
+#include <VirtualRobot/MathTools.h>
+
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/components/units/PlatformUnit.h>
+#include <RobotAPI/interface/core/GeometryBase.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <VirtualRobot/MathTools.h>
-
namespace armarx
{
- PropertyDefinitionsPtr PlatformUnitSimulation::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ PlatformUnitSimulation::createPropertyDefinitions()
{
auto def = PlatformUnit::createPropertyDefinitions();
- def->defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
- def->defineOptionalProperty<float>("InitialRotation", 0, "Initial rotation of the platform.");
- def->defineOptionalProperty<float>("InitialPosition.x", 0, "Initial x position of the platform.");
- def->defineOptionalProperty<float>("InitialPosition.y", 0, "Initial y position of the platform.");
- def->defineOptionalProperty<std::string>("ReferenceFrame", "Platform", "Reference frame in which the platform position is reported.");
- def->defineOptionalProperty<float>("LinearVelocity", 1000.0, "Linear velocity of the platform (default: 1000 mm/sec).");
- def->defineOptionalProperty<float>("MaxLinearAcceleration", 1000.0, "Linear acceleration of the platform (default: 1000 mm/sec).");
- def->defineOptionalProperty<float>("Kp_Position", 5.0, "P value of the PID position controller");
- def->defineOptionalProperty<float>("Kp_Velocity", 5.0, "P value of the PID velocity controller");
- def->defineOptionalProperty<float>("AngularVelocity", 1.5, "Angular velocity of the platform (default: 1.5 rad/sec).");
+ def->defineOptionalProperty<int>(
+ "IntervalMs",
+ 10,
+ "The time in milliseconds between two calls to the simulation method.");
+ def->defineOptionalProperty<float>(
+ "InitialRotation", 0, "Initial rotation of the platform.");
+ def->defineOptionalProperty<float>(
+ "InitialPosition.x", 0, "Initial x position of the platform.");
+ def->defineOptionalProperty<float>(
+ "InitialPosition.y", 0, "Initial y position of the platform.");
+ def->defineOptionalProperty<std::string>(
+ "ReferenceFrame",
+ "Platform",
+ "Reference frame in which the platform position is reported.");
+ def->defineOptionalProperty<float>(
+ "LinearVelocity", 1000.0, "Linear velocity of the platform (default: 1000 mm/sec).");
+ def->defineOptionalProperty<float>(
+ "MaxLinearAcceleration",
+ 1000.0,
+ "Linear acceleration of the platform (default: 1000 mm/sec).");
+ def->defineOptionalProperty<float>(
+ "Kp_Position", 5.0, "P value of the PID position controller");
+ def->defineOptionalProperty<float>(
+ "Kp_Velocity", 5.0, "P value of the PID velocity controller");
+ def->defineOptionalProperty<float>(
+ "AngularVelocity", 1.5, "Angular velocity of the platform (default: 1.5 rad/sec).");
def->component(robotStateComponent);
return def;
}
- void PlatformUnitSimulation::onInitPlatformUnit()
+ void
+ PlatformUnitSimulation::onInitPlatformUnit()
{
platformMode = eUndefined;
referenceFrame = getProperty<std::string>("ReferenceFrame").getValue();
@@ -72,24 +90,40 @@ namespace armarx
maxLinearVelocity = getProperty<float>("LinearVelocity").getValue();
maxAngularVelocity = getProperty<float>("AngularVelocity").getValue();
- velPID.reset(new MultiDimPIDController(getProperty<float>("Kp_Velocity").getValue(), 0, 0, getProperty<float>("MaxLinearAcceleration").getValue()));
- posPID.reset(new MultiDimPIDController(getProperty<float>("Kp_Position").getValue(), 0, 0, maxLinearVelocity, getProperty<float>("MaxLinearAcceleration").getValue()));
+ velPID.reset(
+ new MultiDimPIDController(getProperty<float>("Kp_Velocity").getValue(),
+ 0,
+ 0,
+ getProperty<float>("MaxLinearAcceleration").getValue()));
+ posPID.reset(
+ new MultiDimPIDController(getProperty<float>("Kp_Position").getValue(),
+ 0,
+ 0,
+ maxLinearVelocity,
+ getProperty<float>("MaxLinearAcceleration").getValue()));
positionalAccuracy = 0.01;
intervalMs = getProperty<int>("IntervalMs").getValue();
ARMARX_VERBOSE << "Starting platform unit simulation with " << intervalMs << " ms interval";
- simulationTask = new PeriodicTask<PlatformUnitSimulation>(this, &PlatformUnitSimulation::simulationFunction, intervalMs, false, "PlatformUnitSimulation");
-
+ simulationTask =
+ new PeriodicTask<PlatformUnitSimulation>(this,
+ &PlatformUnitSimulation::simulationFunction,
+ intervalMs,
+ false,
+ "PlatformUnitSimulation");
}
- Eigen::Matrix4f PlatformUnitSimulation::currentPlatformPose() const
+ Eigen::Matrix4f
+ PlatformUnitSimulation::currentPlatformPose() const
{
- return VirtualRobot::MathTools::posrpy2eigen4f(currentPositionX, currentPositionY, 0, 0, 0, currentRotation);
+ return VirtualRobot::MathTools::posrpy2eigen4f(
+ currentPositionX, currentPositionY, 0, 0, 0, currentRotation);
}
- void PlatformUnitSimulation::onStartPlatformUnit()
+ void
+ PlatformUnitSimulation::onStartPlatformUnit()
{
agentName = robotStateComponent->getRobotName();
robotRootFrame = robotStateComponent->getSynchronizedRobot()->getRootNode()->getName();
@@ -106,7 +140,8 @@ namespace armarx
simulationTask->start();
}
- void PlatformUnitSimulation::onStopPlatformUnit()
+ void
+ PlatformUnitSimulation::onStopPlatformUnit()
{
if (simulationTask)
{
@@ -114,8 +149,8 @@ namespace armarx
}
}
-
- void PlatformUnitSimulation::onExitPlatformUnit()
+ void
+ PlatformUnitSimulation::onExitPlatformUnit()
{
if (simulationTask)
{
@@ -123,7 +158,8 @@ namespace armarx
}
}
- void PlatformUnitSimulation::simulationFunction()
+ void
+ PlatformUnitSimulation::simulationFunction()
{
// the time it took until this method was called again
auto now = TimeUtil::GetTime();
@@ -150,7 +186,8 @@ namespace armarx
if (diff > orientationalAccuracy)
{
int sign = copysignf(1.0f, (targetRotation - currentRotation));
- currentRotation += sign * std::min<float>(maxAngularVelocity * timeDeltaInSeconds, diff);
+ currentRotation +=
+ sign * std::min<float>(maxAngularVelocity * timeDeltaInSeconds, diff);
// stay between +/- M_2_PI
if (currentRotation > 2 * M_PI)
@@ -164,7 +201,6 @@ namespace armarx
}
vel[2] = angularVelocity;
-
}
}
break;
@@ -206,14 +242,16 @@ namespace armarx
odomVelocityHeader.parentFrame = robotRootFrame;
odomVelocityHeader.frame = robotRootFrame;
odomVelocityHeader.agent = agentName;
- odomVelocityHeader.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();;
+ odomVelocityHeader.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ ;
// odom pose is in odom frame
FrameHeader odomPoseHeader;
odomPoseHeader.parentFrame = OdometryFrame;
odomPoseHeader.frame = robotRootFrame;
odomPoseHeader.agent = agentName;
- odomPoseHeader.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();;
+ odomPoseHeader.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ ;
TransformStamped platformPose;
platformPose.header = odomPoseHeader;
@@ -234,7 +272,7 @@ namespace armarx
currentPose.header.agent = agentName;
currentPose.header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
currentPose.transform = currentPlatformPose();
-
+
globalPosePrx->reportGlobalRobotPose(currentPose);
}
@@ -244,9 +282,16 @@ namespace armarx
listenerPrx->reportPlatformVelocity(velo.linear.x(), velo.linear.y(), velo.angular.z());
}
- void PlatformUnitSimulation::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
+ void
+ PlatformUnitSimulation::moveTo(Ice::Float targetPlatformPositionX,
+ Ice::Float targetPlatformPositionY,
+ Ice::Float targetPlatformRotation,
+ Ice::Float positionalAccuracy,
+ Ice::Float orientationalAccuracy,
+ const Ice::Current& c)
{
- ARMARX_INFO << "new target pose : " << targetPlatformPositionX << ", " << targetPlatformPositionY << " with angle " << targetPlatformRotation;
+ ARMARX_INFO << "new target pose : " << targetPlatformPositionX << ", "
+ << targetPlatformPositionY << " with angle " << targetPlatformRotation;
{
std::unique_lock lock(currentPoseMutex);
platformMode = ePositionControl;
@@ -265,21 +310,33 @@ namespace armarx
TransformStamped targetPose;
targetPose.header = header;
- targetPose.transform = VirtualRobot::MathTools::posrpy2eigen4f(targetPositionX, targetPositionY, 0, 0, 0, targetRotation);
+ targetPose.transform = VirtualRobot::MathTools::posrpy2eigen4f(
+ targetPositionX, targetPositionY, 0, 0, 0, targetRotation);
}
- void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
+ void
+ armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX,
+ float targetPlatformVelocityY,
+ float targetPlatformVelocityRotation,
+ const Ice::Current& c)
{
- ARMARX_INFO << deactivateSpam(1) << "New velocity: " << targetPlatformVelocityX << ", " << targetPlatformVelocityY << " with angular velocity: " << targetPlatformVelocityRotation;
+ ARMARX_INFO << deactivateSpam(1) << "New velocity: " << targetPlatformVelocityX << ", "
+ << targetPlatformVelocityY
+ << " with angular velocity: " << targetPlatformVelocityRotation;
std::unique_lock lock(currentPoseMutex);
platformMode = eVelocityControl;
linearVelocityX = std::min(maxLinearVelocity, targetPlatformVelocityX);
linearVelocityY = std::min(maxLinearVelocity, targetPlatformVelocityY);
angularVelocity = std::min(maxAngularVelocity, targetPlatformVelocityRotation);
-
}
- void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c)
+ void
+ PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX,
+ float targetPlatformOffsetY,
+ float targetPlatformOffsetRotation,
+ float positionalAccuracy,
+ float orientationalAccuracy,
+ const Ice::Current& c)
{
float targetPositionX, targetPositionY, targetRotation;
{
@@ -288,18 +345,26 @@ namespace armarx
targetPositionY = currentPositionY + targetPlatformOffsetY;
targetRotation = currentRotation + targetPlatformOffsetRotation;
}
- moveTo(targetPositionX, targetPositionY, targetRotation, positionalAccuracy, orientationalAccuracy);
+ moveTo(targetPositionX,
+ targetPositionY,
+ targetRotation,
+ positionalAccuracy,
+ orientationalAccuracy);
}
- void PlatformUnitSimulation::setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c)
+ void
+ PlatformUnitSimulation::setMaxVelocities(float positionalVelocity,
+ float orientaionalVelocity,
+ const Ice::Current& c)
{
std::unique_lock lock(currentPoseMutex);
maxLinearVelocity = positionalVelocity;
maxAngularVelocity = orientaionalVelocity;
}
- void PlatformUnitSimulation::stopPlatform(const Ice::Current& c)
+ void
+ PlatformUnitSimulation::stopPlatform(const Ice::Current& c)
{
move(0, 0, 0);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index 391331d4c45c4058ffbd16b8f1eb4637313b04cc..37568dc33cd217b421d09afd9af3d4111435bb39 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -24,16 +24,17 @@
#pragma once
-#include "PlatformUnit.h"
+#include <string>
+
+#include <IceUtil/Time.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/interface/core/RobotState.h>
-#include <IceUtil/Time.h>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/core/PIDController.h>
-#include <string>
+#include "PlatformUnit.h"
namespace armarx
{
@@ -42,12 +43,12 @@ namespace armarx
* \brief Simulates a robot platform.
* \ingroup RobotAPI-SensorActorUnits-simulation
*/
- class PlatformUnitSimulation :
- virtual public PlatformUnit
+ class PlatformUnitSimulation : virtual public PlatformUnit
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "PlatformUnitSimulation";
}
@@ -60,15 +61,30 @@ namespace armarx
void simulationFunction();
// proxy implementation
- void moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = Ice::emptyCurrent) override;
+ void moveTo(Ice::Float targetPlatformPositionX,
+ Ice::Float targetPlatformPositionY,
+ Ice::Float targetPlatformRotation,
+ Ice::Float positionalAccuracy,
+ Ice::Float orientationalAccuracy,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \warning Not yet implemented!
*/
- void move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
-
- void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c = Ice::emptyCurrent) override;
- void setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c = Ice::emptyCurrent) override;
+ void move(float targetPlatformVelocityX,
+ float targetPlatformVelocityY,
+ float targetPlatformVelocityRotation,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+
+ void moveRelative(float targetPlatformOffsetX,
+ float targetPlatformOffsetY,
+ float targetPlatformOffsetRotation,
+ float positionalAccuracy,
+ float orientationalAccuracy,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void setMaxVelocities(float positionalVelocity,
+ float orientaionalVelocity,
+ const Ice::Current& c = Ice::emptyCurrent) override;
void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \see PropertyUser::createPropertyDefinitions()
@@ -85,8 +101,7 @@ namespace armarx
eUndefined,
ePositionControl,
eVelocityControl
- }
- platformMode;
+ } platformMode;
::Ice::Float targetPositionX;
::Ice::Float targetPositionY;
@@ -118,8 +133,5 @@ namespace armarx
std::string agentName;
std::string robotRootFrame;
-
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotPoseUnit.cpp b/source/RobotAPI/components/units/RobotPoseUnit.cpp
index 85bb9fbdca72534c8369720d1ae19cd73b29a6f2..ab4ef463b8648a0121404bc922fa5810590d644e 100644
--- a/source/RobotAPI/components/units/RobotPoseUnit.cpp
+++ b/source/RobotAPI/components/units/RobotPoseUnit.cpp
@@ -25,10 +25,10 @@
#include "RobotPoseUnit.h"
-
namespace armarx
{
- void RobotPoseUnit::onInitComponent()
+ void
+ RobotPoseUnit::onInitComponent()
{
std::string RobotPoseName = getProperty<std::string>("RobotName").getValue();
@@ -39,8 +39,8 @@ namespace armarx
this->onInitRobotPoseUnit();
}
-
- void RobotPoseUnit::onConnectComponent()
+ void
+ RobotPoseUnit::onConnectComponent()
{
ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
listenerPrx = getTopic<RobotPoseUnitListenerPrx>(listenerChannelName);
@@ -48,29 +48,31 @@ namespace armarx
this->onStartRobotPoseUnit();
}
-
- void RobotPoseUnit::onDisconnectComponent()
+ void
+ RobotPoseUnit::onDisconnectComponent()
{
listenerPrx = NULL;
this->onStopRobotPoseUnit();
}
-
- void RobotPoseUnit::onExitComponent()
+ void
+ RobotPoseUnit::onExitComponent()
{
this->onExitRobotPoseUnit();
}
-
- void RobotPoseUnit:: moveTo(PoseBasePtr targetPose, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
+ void
+ RobotPoseUnit::moveTo(PoseBasePtr targetPose,
+ Ice::Float positionalAccuracy,
+ Ice::Float orientationalAccuracy,
+ const Ice::Current& c)
{
}
-
- PropertyDefinitionsPtr RobotPoseUnit::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ RobotPoseUnit::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new RobotPoseUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(new RobotPoseUnitPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotPoseUnit.h b/source/RobotAPI/components/units/RobotPoseUnit.h
index fb8e12eaf71b99496001d626a461750e8fba2763..9a3ee1399274c0c172a080d8d28249ce18a9f846 100644
--- a/source/RobotAPI/components/units/RobotPoseUnit.h
+++ b/source/RobotAPI/components/units/RobotPoseUnit.h
@@ -24,33 +24,32 @@
#pragma once
-#include <RobotAPI/components/units/SensorActorUnit.h>
+#include <vector>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/units/RobotPoseUnitInterface.h>
-#include <vector>
-
namespace armarx
{
/**
* \class RobotPoseUnitPropertyDefinitions
* \brief Defines all necessary properties for armarx::RobotPoseUnit
*/
- class RobotPoseUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class RobotPoseUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- RobotPoseUnitPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ RobotPoseUnitPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("RobotName", "Robot", "Name of the Robot (will publish values on RobotName + '6DPoseState')");
+ defineOptionalProperty<std::string>(
+ "RobotName",
+ "Robot",
+ "Name of the Robot (will publish values on RobotName + '6DPoseState')");
}
};
-
/**
* \defgroup Component-RobotPoseUnit RobotPoseUnit
* \ingroup RobotAPI-SensorActorUnits
@@ -66,13 +65,12 @@ namespace armarx
* @ingroup Component-RobotPoseUnit
* @brief The RobotPoseUnit class
*/
- class RobotPoseUnit :
- virtual public RobotPoseUnitInterface,
- virtual public SensorActorUnit
+ class RobotPoseUnit : virtual public RobotPoseUnitInterface, virtual public SensorActorUnit
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotPoseUnit";
}
@@ -98,7 +96,12 @@ namespace armarx
virtual void onInitRobotPoseUnit() = 0;
virtual void onStartRobotPoseUnit() = 0;
- virtual void onStopRobotPoseUnit() {}
+
+ virtual void
+ onStopRobotPoseUnit()
+ {
+ }
+
virtual void onExitRobotPoseUnit() = 0;
/**
@@ -107,9 +110,16 @@ namespace armarx
* @param postionalAccuracy Robot stops translating if distance to target position gets lower than this threshhold.
* @param orientationalAccuracy Robot stops rotating if distance from current to target orientation gets lower than this threshhold.
**/
- virtual void moveTo(PoseBasePtr targetPose, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c = Ice::emptyCurrent);
+ virtual void moveTo(PoseBasePtr targetPose,
+ Ice::Float positionalAccuracy,
+ Ice::Float orientationalAccuracy,
+ const Ice::Current& c = Ice::emptyCurrent);
+
+ void
+ stopMovement(const Ice::Current& c = Ice::emptyCurrent) override
+ {
+ }
- void stopMovement(const Ice::Current& c = Ice::emptyCurrent) override {}
/**
* \see armarx::PropertyUser::createPropertyDefinitions()
*/
@@ -125,5 +135,4 @@ namespace armarx
*/
std::string listenerChannelName;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
index ca3b048750edfaa65e0737f9d7d2ec4bda740821..ca938c13f1935a1e9bc5c6b236e53c35f9dce982 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
@@ -29,8 +29,8 @@
#include <Ice/Handle.h>
#include <IceUtil/Time.h>
-#include <RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h>
#include <RobotAPI/components/units/RobotUnit/ControlModes.h>
+#include <RobotAPI/components/units/RobotUnit/util/HeterogenousContinuousContainerMacros.h>
#include <RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h>
namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
index 8042b243406d074c7055b33502c6d022fee5deb0..85851f93a6457e233919a89d8b979a67af2f879b 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.cpp
@@ -6,14 +6,12 @@
#include <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
-#include <VirtualRobot/MathTools.h>
-
-
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/IK/IKSolver.h>
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
index 46704a4816bcca9a5265143d13ef3eda16fb66e9..517c501a624818d6d3b5b456e8e6fcacd79f4010 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControllerParts/CartesianImpedanceController.h
@@ -4,7 +4,6 @@
#include <VirtualRobot/VirtualRobot.h>
-
namespace armarx
{
class SensorValue1DoFActuatorTorque;
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
index e09d06c58578b367472dd7ffca03566ce7faea45..ba124143c7797d3a951e53202314a084a7c1cabd 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
@@ -45,7 +45,11 @@ namespace armarx
/// @return Whether the device has the given tag
bool hasTag(const std::string& tag) const;
- virtual bool hasError(){return false;}
+ virtual bool
+ hasError()
+ {
+ return false;
+ }
protected:
/// @brief adds the given tag to the Device
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
index 26fa0111d4f48e868573fbffc3590da3e7233fd6..bac6312bfbdb306d42bc293b7a4625cb186b33a0 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
@@ -23,9 +23,9 @@
*/
#include "NJointCartesianTorqueController.h"
-#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
index b02c292f635089f09c5553d56a6cf3aa5f3db985..c0a3e14b73b4163d5c856a468db810ba9ed42503 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
@@ -25,7 +25,6 @@
#include <VirtualRobot/VirtualRobot.h>
-
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianTorqueController.h>
#include "../ControlTargets/ControlTarget1DoFActuator.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
index 165da2d63db3e5e085eb1c02eb8b229b81a0fd66..82ab8615bc4b0d76f4ee47dac06d1a36a0f23851 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
@@ -24,13 +24,13 @@
#include "NJointCartesianVelocityController.h"
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#define DEFAULT_TCP_STRING "default TCP"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index ea5ab7c70dbd998b8b0b4e3c549c2712f4649ea1..9b0a92f4a1124a30e7b21811d842f960f9819661 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -23,9 +23,9 @@
*/
#include "NJointCartesianVelocityControllerWithRamp.h"
-#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 943f878897d4eb3e0e08b1fa8a23ae7b686397d3..1e287fbec35580fd04316b18f81cb455e51b1b59 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -1,6 +1,7 @@
#include "NJointCartesianWaypointController.h"
#include <iomanip>
+
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index f13f66fe7770cb8f3b7f77791e6a73e696edf5ee..aa4470eba55caf225de75d33c3d3bffb059fc208 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -26,7 +26,6 @@
#include <atomic>
-
#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index 1e827dcc163c5417515ebb006038f025950e9a51..a2387115d1c7c60806da0ef1ab9597beab465f9a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -25,10 +25,10 @@
#include <Eigen/Geometry>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
-
#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformRelativePositionController>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index c692e00dea00d7c5d3044054991ba6aaffd9ae39..9e845583fe8935079399ff70c66ef134fd024325 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -24,7 +24,6 @@
#include <VirtualRobot/VirtualRobot.h>
-
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
index e903a1a5d9e606b62b3f8ffba8f11d1fe9886d42..45c1bdbb19576767da2637b7556b49f610bc97af 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
@@ -29,6 +29,7 @@
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h"
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
index 1c3a68e75ac1131f46372a5bdaa68442b849788a..031936da89fcb5abb0079aaef8e7170eb1452659 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -23,9 +23,9 @@
*/
#include "NJointTCPController.h"
-#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
index 311df4838b1194c954f1ac1e23d4e754ff56434c..0e038c78890e0be5c990e85d85e81a1e908a7605 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -23,8 +23,8 @@
*/
#pragma once
-#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/IK/IKSolver.h>
+#include <VirtualRobot/VirtualRobot.h>
#include "../ControlTargets/ControlTarget1DoFActuator.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index 069b296afe87fddbe6493b91007089b66fdac58a..4ea5ce61fb65a2cbcef467d280bbc2bc1e227d5f 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -24,7 +24,6 @@
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
-
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index e4a068265734fffb5255660be5302f1e935ada3b..30fe0e1591772f13509b2553580d7cd5cdd28e2e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -1,8 +1,8 @@
#include "NJointTrajectoryController.h"
#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
index d9e4084164fb2144b5d4bd2c823c11b6177154ae..65afc57a1ef2b3cbcd99988d13c509ab1cf85a3d 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.cpp
@@ -22,10 +22,10 @@
#include "RobotUnitModuleControlThreadDataBuffer.h"
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
-
#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+
#include "RobotUnitModuleControllerManagement.h"
#include "RobotUnitModuleDevices.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
index d9ed47b7b6775296ec52cefebb77b32aff989bad..2d275a31bf0f331c3acdaf8e850c7eb426166f26 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h
@@ -24,7 +24,6 @@
#include <VirtualRobot/VirtualRobot.h>
-
#include <ArmarXCore/core/util/TripleBuffer.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
index 4702668c566ab4413b621f66807209bcb38607d6..28f698623e437e0338d6d400fe9baa7f6b290916 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h
@@ -24,9 +24,9 @@
#include <mutex>
-#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
@@ -264,7 +264,7 @@ namespace armarx::RobotUnitModule
* @param sensors The \ref SensorValue "SensorValues"
*/
- // TODO use base type for 'sensors' element
+ // TODO use base type for 'sensors' element
template <class PtrT>
void
updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot,
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index 033d869628eb78f86dd723a8671feb26063df5a1..5ee80162ba3c217ca8f3e6f6343b37e9e222c26a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -23,10 +23,10 @@
#include "RobotUnitModuleRobotData.h"
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index 1ebf5d476627e4503c355298b96d655ce11a19f0..b0a5f27914125dd02072e890945b3e87233651cf 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -21,6 +21,7 @@
*/
#include "RobotUnitModuleSelfCollisionChecker.h"
+
#include <algorithm>
#include <cstddef>
#include <string>
@@ -30,8 +31,8 @@
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Obstacle.h>
-#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/Metronome.h"
@@ -232,11 +233,13 @@ namespace armarx::RobotUnitModule
{
ARMARX_VERBOSE << "Removing ignored collision pairs";
// introduce vector to remove elements "in-place" via remove-erase-if idiom (not possible for sets)
- std::vector<std::pair<std::string, std::string>> validNamePairsToCheck(namePairsToCheck.begin(), namePairsToCheck.end());
+ std::vector<std::pair<std::string, std::string>> validNamePairsToCheck(
+ namePairsToCheck.begin(), namePairsToCheck.end());
- const auto isCollisionIgnored = [this](const std::string& a, const std::string& b) -> bool {
-
- if(a == FLOOR_OBJ_STR or b == FLOOR_OBJ_STR)
+ const auto isCollisionIgnored = [this](const std::string& a,
+ const std::string& b) -> bool
+ {
+ if (a == FLOOR_OBJ_STR or b == FLOOR_OBJ_STR)
{
return false;
}
@@ -244,7 +247,7 @@ namespace armarx::RobotUnitModule
const auto nodeA = selfCollisionAvoidanceRobot->getRobotNode(a);
const auto nodeB = selfCollisionAvoidanceRobot->getRobotNode(b);
- if(nodeA == nullptr or nodeB == nullptr)
+ if (nodeA == nullptr or nodeB == nullptr)
{
return false;
}
@@ -252,28 +255,34 @@ namespace armarx::RobotUnitModule
const std::vector<std::string> nodesIgnoredByA = nodeA->getIgnoredCollisionModels();
const std::vector<std::string> nodesIgnoredByB = nodeB->getIgnoredCollisionModels();
- if(std::find(nodesIgnoredByA.begin(), nodesIgnoredByA.end(), b) != nodesIgnoredByA.end())
+ if (std::find(nodesIgnoredByA.begin(), nodesIgnoredByA.end(), b) !=
+ nodesIgnoredByA.end())
{
ARMARX_VERBOSE << "Ignoring collision between nodes: " << a << " -- " << b;
return true;
}
- if(std::find(nodesIgnoredByB.begin(), nodesIgnoredByB.end(), a) != nodesIgnoredByB.end())
+ if (std::find(nodesIgnoredByB.begin(), nodesIgnoredByB.end(), a) !=
+ nodesIgnoredByB.end())
{
ARMARX_VERBOSE << "Ignoring collision between nodes: " << b << " -- " << a;
return true;
}
return false;
-
};
- validNamePairsToCheck.erase(std::remove_if(validNamePairsToCheck.begin(), validNamePairsToCheck.end(), [&isCollisionIgnored](const auto& p) -> bool {
- const auto& [a, b] = p;
- return isCollisionIgnored(a, b);
- }), validNamePairsToCheck.end());
+ validNamePairsToCheck.erase(std::remove_if(validNamePairsToCheck.begin(),
+ validNamePairsToCheck.end(),
+ [&isCollisionIgnored](const auto& p) -> bool
+ {
+ const auto& [a, b] = p;
+ return isCollisionIgnored(a, b);
+ }),
+ validNamePairsToCheck.end());
- ARMARX_VERBOSE << "Removed " << (namePairsToCheck.size() - validNamePairsToCheck.size()) << " collision pairs.";
+ ARMARX_VERBOSE << "Removed " << (namePairsToCheck.size() - validNamePairsToCheck.size())
+ << " collision pairs.";
// copy over name pairs which should not be ignored
namePairsToCheck = std::set(validNamePairsToCheck.begin(), validNamePairsToCheck.end());
@@ -381,7 +390,7 @@ namespace armarx::RobotUnitModule
if (inEmergencyStop || freq == 0)
{
ARMARX_VERBOSE << deactivateSpam() << "Self collision checker: skipping check "
- << VAROUT(freq) << " " << VAROUT(inEmergencyStop);
+ << VAROUT(freq) << " " << VAROUT(inEmergencyStop);
//currently wait
std::this_thread::sleep_for(std::chrono::microseconds{1'000});
continue;
@@ -445,11 +454,12 @@ namespace armarx::RobotUnitModule
//sleep remaining
const auto duration = metronome.waitForNextTick();
- if(not duration.isPositive())
+ if (not duration.isPositive())
{
- ARMARX_WARNING << deactivateSpam(10) <<
- "Self collision checking took too long. "
- "Exceeding time budget by " << duration.toMilliSecondsDouble() << "ms.";
+ ARMARX_WARNING << deactivateSpam(10)
+ << "Self collision checking took too long. "
+ "Exceeding time budget by "
+ << duration.toMilliSecondsDouble() << "ms.";
}
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
index 707800ad73868e83a5b49a0d159121beb8eb20c7..cd70f586411febfa7d1d031490abde52f0013211 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
@@ -21,10 +21,10 @@
*/
#pragma once
-#include <RobotAPI/libraries/core/Pose.h>
-
#include <Eigen/Core>
+#include <RobotAPI/libraries/core/Pose.h>
+
#include "SensorValueBase.h"
namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
index bc679526a8af3080a27de60f98f6ae39b4e0dac4..e22ae9d7b2682350b3fb3adeb7f57e853dbac719 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
@@ -26,7 +26,6 @@
#include <Eigen/Core>
-
#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/units/LocalizationUnitInterface.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
index f02cd9172e2d368cf3877d92e60cdb736000ba3a..82e09c9e8e2c321398f38f7f3b6b778b648640c1 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
@@ -23,6 +23,8 @@
#define BOOST_TEST_MODULE RobotAPI::BasicControllers::Test
#define ARMARX_BOOST_TEST
#define CREATE_LOGFILES
+#include <RobotAPI/Test.h>
+
#include <filesystem>
#include <iostream>
#include <random>
@@ -32,8 +34,6 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/algorithm.h>
-#include <RobotAPI/Test.h>
-
#include "../BasicControllers.h"
#include "../util/CtrlUtil.h"
using namespace armarx;
diff --git a/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
index c34b618afcd0cf9b03c4fae663866a52a1a11752..2863db26368e2ee0882dd093bf9c3cd7ea6450c1 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/test/SingleBasicControllerTest.cpp
@@ -23,6 +23,8 @@
#define BOOST_TEST_MODULE RobotAPI::BasicControllers::Test
#define ARMARX_BOOST_TEST
#define CREATE_LOGFILES
+#include <RobotAPI/Test.h>
+
#include <iostream>
#include <random>
@@ -31,8 +33,6 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/algorithm.h>
-#include <RobotAPI/Test.h>
-
#include "../BasicControllers.h"
using namespace armarx;
//params for random tests
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index cb47d82b18e8c30d5b0f82c7923ae8f2328bf734..29c5115399f7d5216ba3994e79b0849137b914a9 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -24,8 +24,8 @@
#include <vector>
-#include <ArmarXCore/core/logging/LoggingUtil.h> // THIS NEEDS TO BE INCLUDED BEFORE EXPRESSION EXCEPTION
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LoggingUtil.h> // THIS NEEDS TO BE INCLUDED BEFORE EXPRESSION EXCEPTION
#include <ArmarXCore/core/util/PropagateConst.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
@@ -396,8 +396,8 @@ namespace armarx::detail
entries(numEntries, nullptr)
{
ARMARX_DEBUG << "RtMessageLogBuffer created with bufferSize=" << bufferSize
- << " and numEntries=" << numEntries << " and bufferMaxSize=" << bufferMaxSize
- << " and bufferMaxNumberEntries " << bufferMaxNumberEntries;
+ << " and numEntries=" << numEntries << " and bufferMaxSize=" << bufferMaxSize
+ << " and bufferMaxNumberEntries " << bufferMaxNumberEntries;
}
inline RtMessageLogBuffer::~RtMessageLogBuffer()
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
index bc22d972c5bf6eb3830fc5f3a1b7e8ef12949dfc..6fcb426c8bed135936a1dd6b6d7ff3d3d087758f 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
@@ -23,15 +23,14 @@
*/
#include "DynamicsHelper.h"
+#include <VirtualRobot/Dynamics/Dynamics.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
-
-#include <VirtualRobot/Dynamics/Dynamics.h>
-#include <VirtualRobot/RobotNodeSet.h>
-
namespace armarx
{
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
index e49736123c26cf94e184d78f0445e4f46cf23754..f78705da723703a67e8bb92e56862c7d27a32854 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h
@@ -24,8 +24,8 @@
#pragma once
#include <Eigen/Core>
-#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Dynamics/Dynamics.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
diff --git a/source/RobotAPI/components/units/SensorActorUnit.cpp b/source/RobotAPI/components/units/SensorActorUnit.cpp
index 45efd98c586e5bfa15086ddc08a80d65f7c3497a..5f470411699da6838c2a0eb973061706259164ff 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.cpp
+++ b/source/RobotAPI/components/units/SensorActorUnit.cpp
@@ -21,8 +21,9 @@
*/
#include "SensorActorUnit.h"
-#include <Ice/ObjectAdapter.h>
+
#include <Ice/Initialize.h>
+#include <Ice/ObjectAdapter.h>
using namespace armarx;
@@ -35,10 +36,10 @@ SensorActorUnit::SensorActorUnit()
SensorActorUnit::~SensorActorUnit()
{
-
}
-void SensorActorUnit::init(const Ice::Current& c)
+void
+SensorActorUnit::init(const Ice::Current& c)
{
std::string currentName = c.adapter->getName();
Ice::Identity currentId = Ice::stringToIdentity(currentName);
@@ -59,11 +60,13 @@ void SensorActorUnit::init(const Ice::Current& c)
}
else
{
- ARMARX_WARNING << "Unit " << getName() << " can not be initialized while unrequested." << armarx::flush;
+ ARMARX_WARNING << "Unit " << getName() << " can not be initialized while unrequested."
+ << armarx::flush;
}
}
-void SensorActorUnit::start(const Ice::Current& c)
+void
+SensorActorUnit::start(const Ice::Current& c)
{
std::string currentName = c.adapter->getName();
Ice::Identity currentId = Ice::stringToIdentity(currentName);
@@ -84,11 +87,13 @@ void SensorActorUnit::start(const Ice::Current& c)
}
else
{
- ARMARX_WARNING << "Unit " << getName() << " can not be started while unrequested." << armarx::flush;
+ ARMARX_WARNING << "Unit " << getName() << " can not be started while unrequested."
+ << armarx::flush;
}
}
-void SensorActorUnit::stop(const Ice::Current& c)
+void
+SensorActorUnit::stop(const Ice::Current& c)
{
std::string currentName = c.adapter->getName();
Ice::Identity currentId = Ice::stringToIdentity(currentName);
@@ -109,18 +114,20 @@ void SensorActorUnit::stop(const Ice::Current& c)
}
else
{
- ARMARX_WARNING << "Unit " << getName() << " can not be stopped while unrequested. " << armarx::flush;
+ ARMARX_WARNING << "Unit " << getName() << " can not be stopped while unrequested. "
+ << armarx::flush;
}
}
-UnitExecutionState SensorActorUnit::getExecutionState(const Ice::Current& c)
+UnitExecutionState
+SensorActorUnit::getExecutionState(const Ice::Current& c)
{
CALLINFO
return executionState;
}
-
-void SensorActorUnit::request(const Ice::Current& c)
+void
+SensorActorUnit::request(const Ice::Current& c)
{
CALLINFO
@@ -138,7 +145,8 @@ void SensorActorUnit::request(const Ice::Current& c)
ARMARX_INFO << "unit requested by " << ownerName << flush;
}
-void SensorActorUnit::release(const Ice::Current& c)
+void
+SensorActorUnit::release(const Ice::Current& c)
{
CALLINFO
// retrieve owner id from current connection
@@ -158,7 +166,8 @@ void SensorActorUnit::release(const Ice::Current& c)
ARMARX_INFO << "unit released" << flush;
}
-void SensorActorUnit::onExitComponent()
+void
+SensorActorUnit::onExitComponent()
{
if (unitMutex.try_lock()) // try to lock, to ensure that it is locked on unlock call
{
diff --git a/source/RobotAPI/components/units/SensorActorUnit.h b/source/RobotAPI/components/units/SensorActorUnit.h
index 2c18c79e6bd840ed60f5660a858e104faecc3de6..fb93deea73661b04d2b80181931a5edef42852e6 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.h
+++ b/source/RobotAPI/components/units/SensorActorUnit.h
@@ -22,12 +22,13 @@
#pragma once
-#include <ArmarXCore/core/system/ImportExport.h>
+#include <mutex>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/Exception.h>
-#include <RobotAPI/interface/units/UnitInterface.h>
+#include <ArmarXCore/core/system/ImportExport.h>
-#include <mutex>
+#include <RobotAPI/interface/units/UnitInterface.h>
namespace armarx
{
@@ -113,21 +114,20 @@ namespace armarx
/**
* callback onInit for subclass hook. See init().
*/
- virtual void onInit() {};
+ virtual void onInit(){};
/**
* callback onStart for subclass hook. See start().
*/
- virtual void onStart() {};
+ virtual void onStart(){};
/**
* callback onStop for subclass hook. See stop().
*/
- virtual void onStop() {};
+ virtual void onStop(){};
std::mutex unitMutex;
Ice::Identity ownerId;
- UnitExecutionState executionState;
+ UnitExecutionState executionState;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/SpeechObserver.cpp b/source/RobotAPI/components/units/SpeechObserver.cpp
index 475c331a92701ff3e326ed498de7ff0ffe9a7d47..a44ebd7ce56251bff03137bea23a7252def7b9b0 100644
--- a/source/RobotAPI/components/units/SpeechObserver.cpp
+++ b/source/RobotAPI/components/units/SpeechObserver.cpp
@@ -22,39 +22,52 @@
*/
#include "SpeechObserver.h"
-#include <ArmarXCore/util/json/JSONObject.h>
#include <Ice/ObjectAdapter.h>
+#include <ArmarXCore/util/json/JSONObject.h>
+
using namespace armarx;
SpeechObserver::SpeechObserver()
{
}
-void SpeechObserver::onInitObserver()
+void
+SpeechObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("TextToSpeechTopicName").getValue());
usingTopic(getProperty<std::string>("TextToSpeechStateTopicName").getValue());
}
-void SpeechObserver::onConnectObserver()
+void
+SpeechObserver::onConnectObserver()
{
offerChannel("TextToSpeech", "TextToSpeech channel");
offerDataFieldWithDefault("TextToSpeech", "Text", Variant(""), "Current text");
- offerDataFieldWithDefault("TextToSpeech", "TextChangeCounter", Variant(reportTextCounter), "Counter for text updates");
+ offerDataFieldWithDefault("TextToSpeech",
+ "TextChangeCounter",
+ Variant(reportTextCounter),
+ "Counter for text updates");
offerDataFieldWithDefault("TextToSpeech", "State", Variant(""), "Current TTS state");
- offerDataFieldWithDefault("TextToSpeech", "StateChangeCounter", Variant(reportStateCounter), "Counter for state updates");
+ offerDataFieldWithDefault("TextToSpeech",
+ "StateChangeCounter",
+ Variant(reportStateCounter),
+ "Counter for state updates");
offerChannel("SpeechToText", "SpeechToText channel");
- offerDataFieldWithDefault("SpeechToText", "RecognizedText", Variant(std::string()), "Text recognized by the SpeechRecogntion");
+ offerDataFieldWithDefault("SpeechToText",
+ "RecognizedText",
+ Variant(std::string()),
+ "Text recognized by the SpeechRecogntion");
auto proxy = getObjectAdapter()->addWithUUID(new SpeechListenerImpl(this));
getIceManager()->subscribeTopic(proxy, "Speech_Commands");
}
-std::string SpeechObserver::SpeechStateToString(TextToSpeechStateType state)
+std::string
+SpeechObserver::SpeechStateToString(TextToSpeechStateType state)
{
switch (state)
{
@@ -70,7 +83,8 @@ std::string SpeechObserver::SpeechStateToString(TextToSpeechStateType state)
}
}
-void SpeechObserver::reportState(TextToSpeechStateType state, const Ice::Current&)
+void
+SpeechObserver::reportState(TextToSpeechStateType state, const Ice::Current&)
{
std::unique_lock lock(dataMutex);
reportStateCounter++;
@@ -79,7 +93,8 @@ void SpeechObserver::reportState(TextToSpeechStateType state, const Ice::Current
updateChannel("TextToSpeech");
}
-void SpeechObserver::reportText(const std::string& text, const Ice::Current& c)
+void
+SpeechObserver::reportText(const std::string& text, const Ice::Current& c)
{
std::unique_lock lock(dataMutex);
reportTextCounter++;
@@ -88,20 +103,21 @@ void SpeechObserver::reportText(const std::string& text, const Ice::Current& c)
updateChannel("TextToSpeech");
}
-void SpeechObserver::reportTextWithParams(const std::string& text, const Ice::StringSeq& params, const Ice::Current&)
+void
+SpeechObserver::reportTextWithParams(const std::string& text,
+ const Ice::StringSeq& params,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
ARMARX_WARNING << "reportTextWithParams is not implemented";
}
-SpeechListenerImpl::SpeechListenerImpl(SpeechObserver* obs) :
- obs(obs)
+SpeechListenerImpl::SpeechListenerImpl(SpeechObserver* obs) : obs(obs)
{
-
}
-
-void armarx::SpeechListenerImpl::reportText(const std::string& text, const Ice::Current&)
+void
+armarx::SpeechListenerImpl::reportText(const std::string& text, const Ice::Current&)
{
std::unique_lock lock(dataMutex);
JSONObject json;
@@ -109,7 +125,10 @@ void armarx::SpeechListenerImpl::reportText(const std::string& text, const Ice::
obs->setDataField("SpeechToText", "RecognizedText", Variant(json.getString("text")));
}
-void armarx::SpeechListenerImpl::reportTextWithParams(const std::string&, const Ice::StringSeq&, const Ice::Current&)
+void
+armarx::SpeechListenerImpl::reportTextWithParams(const std::string&,
+ const Ice::StringSeq&,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
ARMARX_WARNING << "reportTextWithParams is not implemented";
diff --git a/source/RobotAPI/components/units/SpeechObserver.h b/source/RobotAPI/components/units/SpeechObserver.h
index e7524e17c03bb9bff43719201922b3508db3f66b..755fa4aa371da3f88ccb1f077870b03268e86300 100644
--- a/source/RobotAPI/components/units/SpeechObserver.h
+++ b/source/RobotAPI/components/units/SpeechObserver.h
@@ -23,65 +23,79 @@
#pragma once
+#include <mutex>
+
#include <ArmarXCore/observers/Observer.h>
+
#include <RobotAPI/interface/observers/SpeechObserverInterface.h>
-#include <mutex>
namespace armarx
{
- class SpeechObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class SpeechObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- SpeechObserverPropertyDefinitions(std::string prefix):
- ObserverPropertyDefinitions(prefix)
+ SpeechObserverPropertyDefinitions(std::string prefix) : ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("TextToSpeechTopicName", "TextToSpeech", "Name of the TextToSpeechTopic");
- defineOptionalProperty<std::string>("TextToSpeechStateTopicName", "TextToSpeechState", "Name of the TextToSpeechStateTopic");
+ defineOptionalProperty<std::string>(
+ "TextToSpeechTopicName", "TextToSpeech", "Name of the TextToSpeechTopic");
+ defineOptionalProperty<std::string>("TextToSpeechStateTopicName",
+ "TextToSpeechState",
+ "Name of the TextToSpeechStateTopic");
}
};
class SpeechObserver;
+
class SpeechListenerImpl : public TextListenerInterface
{
public:
SpeechListenerImpl(SpeechObserver* obs);
+
protected:
SpeechObserver* obs;
std::mutex dataMutex;
// TextListenerInterface interface
public:
void reportText(const std::string&, const Ice::Current&) override;
- void reportTextWithParams(const std::string&, const Ice::StringSeq&, const Ice::Current&) override;
+ void reportTextWithParams(const std::string&,
+ const Ice::StringSeq&,
+ const Ice::Current&) override;
};
- class SpeechObserver :
- virtual public Observer,
- virtual public SpeechObserverInterface
+ class SpeechObserver : virtual public Observer, virtual public SpeechObserverInterface
{
friend class SpeechListenerImpl;
+
public:
SpeechObserver();
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "SpeechObserver";
}
- PropertyDefinitionsPtr createPropertyDefinitions() override
+ PropertyDefinitionsPtr
+ createPropertyDefinitions() override
{
- return PropertyDefinitionsPtr(new SpeechObserverPropertyDefinitions(getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new SpeechObserverPropertyDefinitions(getConfigIdentifier()));
}
void onInitObserver() override;
void onConnectObserver() override;
- virtual void reportState(armarx::TextToSpeechStateType state, const Ice::Current& = Ice::emptyCurrent) override;
- virtual void reportText(const std::string& text, const Ice::Current& = Ice::emptyCurrent) override;
- virtual void reportTextWithParams(const std::string& text, const Ice::StringSeq& params, const Ice::Current& = Ice::emptyCurrent) override;
+ virtual void reportState(armarx::TextToSpeechStateType state,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ virtual void reportText(const std::string& text,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ virtual void reportTextWithParams(const std::string& text,
+ const Ice::StringSeq& params,
+ const Ice::Current& = Ice::emptyCurrent) override;
static std::string SpeechStateToString(TextToSpeechStateType state);
+
private:
std::mutex dataMutex;
int reportTextCounter = 0;
int reportStateCounter = 0;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 08557d8a0649565caf404908ed2fb979ac095d9f..17c6079cd57aafbf67c9a3e8c84631d1f0fe6923 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -21,24 +21,24 @@
*/
#include "TCPControlUnit.h"
-#include <RobotAPI/libraries/core/LinkedPose.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <cfloat>
+#include <memory>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <VirtualRobot/RobotConfig.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/MathTools.h>
+#include <Eigen/Core>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/XML/RobotIO.h>
-#include <Eigen/Core>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
-#include <memory>
-#include <cfloat>
+#include <RobotAPI/libraries/core/LinkedPose.h>
using namespace VirtualRobot;
using namespace Eigen;
@@ -51,10 +51,10 @@ namespace armarx
requested(false),
calculationRunning(false)
{
-
}
- void TCPControlUnit::onInitComponent()
+ void
+ TCPControlUnit::onInitComponent()
{
topicName = getName() + "State";
usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
@@ -67,20 +67,27 @@ namespace armarx
maxJointVelocity = getProperty<float>("MaxJointVelocity").getValue();
}
-
- void TCPControlUnit::onConnectComponent()
+ void
+ TCPControlUnit::onConnectComponent()
{
std::unique_lock lock(dataMutex);
- debugObs = getTopic<DebugObserverInterfacePrx>("DebugObserver");
- kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
- robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
+ debugObs = getTopic<DebugObserverInterfacePrx>("DebugObserver");
+ kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(
+ getProperty<std::string>("KinematicUnitName").getValue());
+ robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(
+ getProperty<std::string>("RobotStateComponentName").getValue());
//remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
robotName = robotStateComponentPrx->getRobotName();
- localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx, robotStateComponentPrx->getRobotFilename(), robotStateComponentPrx->getArmarXPackages());
- jointExistenceCheckRobot = RemoteRobot::createLocalClone(robotStateComponentPrx, robotStateComponentPrx->getRobotFilename(), robotStateComponentPrx->getArmarXPackages());
+ localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx,
+ robotStateComponentPrx->getRobotFilename(),
+ robotStateComponentPrx->getArmarXPackages());
+ jointExistenceCheckRobot =
+ RemoteRobot::createLocalClone(robotStateComponentPrx,
+ robotStateComponentPrx->getRobotFilename(),
+ robotStateComponentPrx->getArmarXPackages());
localReportRobot = localRobot->clone(localRobot->getName());
@@ -128,7 +135,6 @@ namespace armarx
}
-
listener = getTopic<TCPControlUnitListenerPrx>(topicName);
requested = false;
@@ -138,13 +144,14 @@ namespace armarx
execTask->stop();
}
- execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
+ execTask = new PeriodicTask<TCPControlUnit>(
+ this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
execTask->start();
execTask->setDelayWarningTolerance(100);
-
}
- void TCPControlUnit::onDisconnectComponent()
+ void
+ TCPControlUnit::onDisconnectComponent()
{
try
@@ -162,13 +169,13 @@ namespace armarx
}
}
- void TCPControlUnit::onExitComponent()
+ void
+ TCPControlUnit::onExitComponent()
{
}
-
-
- void TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
+ void
+ TCPControlUnit::setCycleTime(Ice::Int milliseconds, const Ice::Current& c)
{
std::unique_lock lock(taskMutex);
cycleTime = milliseconds;
@@ -179,26 +186,36 @@ namespace armarx
}
}
- void TCPControlUnit::setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const FramedDirectionBasePtr& translationVelocity, const FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c)
+ void
+ TCPControlUnit::setTCPVelocity(const std::string& nodeSetName,
+ const std::string& tcpName,
+ const FramedDirectionBasePtr& translationVelocity,
+ const FramedDirectionBasePtr& orientationVelocityRPY,
+ const Ice::Current& c)
{
if (!isRequested())
{
- ARMARX_WARNING << "Implicitly requesting TCPControlUnit! Please call request before setting TCPVelocities!";
+ ARMARX_WARNING << "Implicitly requesting TCPControlUnit! Please call request before "
+ "setting TCPVelocities!";
request();
}
std::unique_lock lock(dataMutex);
- ARMARX_CHECK_EXPRESSION(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName)) << "The robot does not have the node set: " + nodeSetName;
+ ARMARX_CHECK_EXPRESSION(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName))
+ << "The robot does not have the node set: " + nodeSetName;
if (translationVelocity)
{
- ARMARX_DEBUG << "Setting new Velocity for " << nodeSetName << " in frame " << translationVelocity->frame << ":\n" << FramedDirectionPtr::dynamicCast(translationVelocity)->toEigen();
+ ARMARX_DEBUG << "Setting new Velocity for " << nodeSetName << " in frame "
+ << translationVelocity->frame << ":\n"
+ << FramedDirectionPtr::dynamicCast(translationVelocity)->toEigen();
}
if (orientationVelocityRPY)
{
- ARMARX_DEBUG << "Orientation Velo in frame " << orientationVelocityRPY->frame << ": \n" << FramedDirectionPtr::dynamicCast(orientationVelocityRPY)->toEigen();
+ ARMARX_DEBUG << "Orientation Velo in frame " << orientationVelocityRPY->frame << ": \n"
+ << FramedDirectionPtr::dynamicCast(orientationVelocityRPY)->toEigen();
}
TCPVelocityData data;
@@ -208,11 +225,13 @@ namespace armarx
if (tcpName.empty())
{
- data.tcpName = jointExistenceCheckRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
+ data.tcpName =
+ jointExistenceCheckRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
}
else
{
- ARMARX_CHECK_EXPRESSION(jointExistenceCheckRobot->hasRobotNode(tcpName)) << "The robot does not have the node: " + tcpName;
+ ARMARX_CHECK_EXPRESSION(jointExistenceCheckRobot->hasRobotNode(tcpName))
+ << "The robot does not have the node: " + tcpName;
data.tcpName = tcpName;
}
@@ -220,20 +239,23 @@ namespace armarx
tcpVelocitiesMap[data.tcpName] = data;
}
-
- void TCPControlUnit::init(const Ice::Current& c)
+ void
+ TCPControlUnit::init(const Ice::Current& c)
{
}
- void TCPControlUnit::start(const Ice::Current& c)
+ void
+ TCPControlUnit::start(const Ice::Current& c)
{
}
- void TCPControlUnit::stop(const Ice::Current& c)
+ void
+ TCPControlUnit::stop(const Ice::Current& c)
{
}
- UnitExecutionState TCPControlUnit::getExecutionState(const Ice::Current& c)
+ UnitExecutionState
+ TCPControlUnit::getExecutionState(const Ice::Current& c)
{
switch (getState())
{
@@ -253,7 +275,8 @@ namespace armarx
}
}
- void TCPControlUnit::request(const Ice::Current& c)
+ void
+ TCPControlUnit::request(const Ice::Current& c)
{
if (execTask)
{
@@ -269,13 +292,15 @@ namespace armarx
std::unique_lock lock(dataMutex);
requested = true;
- execTask = new PeriodicTask<TCPControlUnit>(this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
+ execTask = new PeriodicTask<TCPControlUnit>(
+ this, &TCPControlUnit::periodicExec, cycleTime, false, "TCPVelocityCalculator");
execTask->start();
execTask->setDelayWarningTolerance(100);
ARMARX_IMPORTANT << "Requested TCPControlUnit";
}
- void TCPControlUnit::release(const Ice::Current& c)
+ void
+ TCPControlUnit::release(const Ice::Current& c)
{
ARMARX_IMPORTANT << "Releasing TCPControlUnit";
std::unique_lock lock(dataMutex);
@@ -290,17 +315,17 @@ namespace armarx
requested = false;
//kinematicUnitPrx->stop();
ARMARX_IMPORTANT << "Released TCPControlUnit";
-
}
- bool TCPControlUnit::isRequested(const Ice::Current& c)
+ bool
+ TCPControlUnit::isRequested(const Ice::Current& c)
{
// no lock needed to read a single bool value.
return requested;
}
-
- void TCPControlUnit::periodicExec()
+ void
+ TCPControlUnit::periodicExec()
{
std::unique_lock lock(dataMutex, std::defer_lock);
@@ -337,9 +362,8 @@ namespace armarx
}
}
-
-
- void TCPControlUnit::calcAndSetVelocities()
+ void
+ TCPControlUnit::calcAndSetVelocities()
{
TCPVelocityDataMap::iterator it = localTcpVelocitiesMap.begin();
@@ -352,7 +376,8 @@ namespace armarx
if (itDIK == localDIKMap.end())
{
- VirtualRobot::DifferentialIKPtr dIk(new EDifferentialIK(nodeSet, localRobot->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ VirtualRobot::DifferentialIKPtr dIk(new EDifferentialIK(
+ nodeSet, localRobot->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
float lambda = getProperty<float>("LambdaDampedSVD").getValue();
dIk->setDampedSvdLambda(lambda);
localDIKMap[data.nodeSetName] = dIk;
@@ -400,9 +425,13 @@ namespace armarx
if (data.orientationVelocity)
{
Eigen::Matrix3f rotInOriginalFrame, rotInRefFrame;
- rotInOriginalFrame = Eigen::AngleAxisf(data.orientationVelocity->z * cycleTime * 0.001, Eigen::Vector3f::UnitZ())
- * Eigen::AngleAxisf(data.orientationVelocity->y * cycleTime * 0.001, Eigen::Vector3f::UnitY())
- * Eigen::AngleAxisf(data.orientationVelocity->x * cycleTime * 0.001, Eigen::Vector3f::UnitX());
+ rotInOriginalFrame =
+ Eigen::AngleAxisf(data.orientationVelocity->z * cycleTime * 0.001,
+ Eigen::Vector3f::UnitZ()) *
+ Eigen::AngleAxisf(data.orientationVelocity->y * cycleTime * 0.001,
+ Eigen::Vector3f::UnitY()) *
+ Eigen::AngleAxisf(data.orientationVelocity->x * cycleTime * 0.001,
+ Eigen::Vector3f::UnitX());
if (data.orientationVelocity->frame != refFrame)
{
@@ -410,7 +439,9 @@ namespace armarx
if (data.orientationVelocity->frame != GlobalFrame)
{
- trafoOriginalFrameToGlobal = localRobot->getRobotNode(data.orientationVelocity->frame)->getGlobalPose();
+ trafoOriginalFrameToGlobal =
+ localRobot->getRobotNode(data.orientationVelocity->frame)
+ ->getGlobalPose();
}
Eigen::Matrix4f trafoRefFrameToGlobal = Eigen::Matrix4f::Identity();
@@ -420,8 +451,10 @@ namespace armarx
trafoRefFrameToGlobal = localRobot->getRobotNode(refFrame)->getGlobalPose();
}
- Eigen::Matrix4f trafoOriginalToRef = trafoRefFrameToGlobal.inverse() * trafoOriginalFrameToGlobal;
- rotInRefFrame = trafoOriginalToRef.block<3, 3>(0, 0) * rotInOriginalFrame * trafoOriginalToRef.block<3, 3>(0, 0).inverse();
+ Eigen::Matrix4f trafoOriginalToRef =
+ trafoRefFrameToGlobal.inverse() * trafoOriginalFrameToGlobal;
+ rotInRefFrame = trafoOriginalToRef.block<3, 3>(0, 0) * rotInOriginalFrame *
+ trafoOriginalToRef.block<3, 3>(0, 0).inverse();
}
else
{
@@ -438,9 +471,12 @@ namespace armarx
if (data.translationVelocity)
{
- data.translationVelocity = FramedDirection::ChangeFrame(localRobot, *data.translationVelocity, refFrame);
- ARMARX_INFO << deactivateSpam(1) << "Translation in " << refFrame << ": " << data.translationVelocity->toEigen();
- m.block(0, 3, 3, 1) = tcpNode->getGlobalPose().block(0, 3, 3, 1) + data.translationVelocity->toEigen() * cycleTime * 0.001;
+ data.translationVelocity =
+ FramedDirection::ChangeFrame(localRobot, *data.translationVelocity, refFrame);
+ ARMARX_INFO << deactivateSpam(1) << "Translation in " << refFrame << ": "
+ << data.translationVelocity->toEigen();
+ m.block(0, 3, 3, 1) = tcpNode->getGlobalPose().block(0, 3, 3, 1) +
+ data.translationVelocity->toEigen() * cycleTime * 0.001;
}
@@ -448,7 +484,8 @@ namespace armarx
if (!dIK)
{
- ARMARX_WARNING << deactivateSpam(1) << "DiffIK is NULL for robot node set: " << data.nodeSetName;
+ ARMARX_WARNING << deactivateSpam(1)
+ << "DiffIK is NULL for robot node set: " << data.nodeSetName;
continue;
}
@@ -480,7 +517,8 @@ namespace armarx
// ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
// dIK->setVerbose(true);
Eigen::VectorXf jointDelta;
- dIK->computeSteps(jointDelta, 0.8f, 0.001, 1, &EDifferentialIK::computeStep); // 1.0, 0.00001, 50
+ dIK->computeSteps(
+ jointDelta, 0.8f, 0.001, 1, &EDifferentialIK::computeStep); // 1.0, 0.00001, 50
// ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << robotNodeSet->getTCP()->getGlobalPose();
jointDelta /= (cycleTime * 0.001);
@@ -491,15 +529,17 @@ namespace armarx
// build name value map
- const std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
- std::vector< VirtualRobot::RobotNodePtr >::const_iterator iter = nodes.begin();
+ const std::vector<VirtualRobot::RobotNodePtr> nodes = robotNodeSet->getAllRobotNodes();
+ std::vector<VirtualRobot::RobotNodePtr>::const_iterator iter = nodes.begin();
int i = 0;
while (iter != nodes.end() && i < jointDelta.rows())
{
if (targetVelocities.find((*iter)->getName()) != targetVelocities.end())
{
- ARMARX_WARNING << deactivateSpam(2) << (*iter)->getName() << " is set from two joint delta calculations - overwriting first value";
+ ARMARX_WARNING
+ << deactivateSpam(2) << (*iter)->getName()
+ << " is set from two joint delta calculations - overwriting first value";
}
targetVelocities.insert(std::make_pair((*iter)->getName(), jointDelta(i)));
@@ -515,9 +555,10 @@ namespace armarx
kinematicUnitPrx->setJointVelocities(targetVelocities);
}
-
-
- void TCPControlUnit::ContinuousAngles(const Eigen::AngleAxisf& oldAngle, Eigen::AngleAxisf& newAngle, double& offset)
+ void
+ TCPControlUnit::ContinuousAngles(const Eigen::AngleAxisf& oldAngle,
+ Eigen::AngleAxisf& newAngle,
+ double& offset)
{
// if(oldAngle.axis().isApprox(newAngle.axis()*-1))
const Eigen::Vector3f& v1 = oldAngle.axis();
@@ -537,11 +578,13 @@ namespace armarx
// else newAngle = newAngle;
- if (fabs(newAngle.angle() + offset - oldAngle.angle()) > fabs(newAngle.angle() + offset - (oldAngle.angle() + M_PI * 2)))
+ if (fabs(newAngle.angle() + offset - oldAngle.angle()) >
+ fabs(newAngle.angle() + offset - (oldAngle.angle() + M_PI * 2)))
{
offset -= M_PI * 2;
}
- else if (fabs(newAngle.angle() + offset - oldAngle.angle()) > fabs((newAngle.angle() + M_PI * 2 + offset) - oldAngle.angle()))
+ else if (fabs(newAngle.angle() + offset - oldAngle.angle()) >
+ fabs((newAngle.angle() + M_PI * 2 + offset) - oldAngle.angle()))
{
offset += M_PI * 2;
}
@@ -549,10 +592,13 @@ namespace armarx
newAngle.angle() += offset;
}
-
- Eigen::VectorXf TCPControlUnit::CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse, Eigen::VectorXf desiredJointValues)
+ Eigen::VectorXf
+ TCPControlUnit::CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet,
+ const Eigen::MatrixXf& jacobian,
+ const Eigen::MatrixXf& jacobianInverse,
+ Eigen::VectorXf desiredJointValues)
{
- std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
+ std::vector<VirtualRobot::RobotNodePtr> nodes = robotNodeSet->getAllRobotNodes();
Eigen::VectorXf actualJointValues(nodes.size());
if (desiredJointValues.rows() == 0)
@@ -563,10 +609,10 @@ namespace armarx
for (unsigned int i = 0; i < nodes.size(); i++)
{
VirtualRobot::RobotNodePtr node = nodes.at(i);
- desiredJointValues(i) = (node->getJointLimitHigh() - node->getJointLimitLow()) * 0.5f + node->getJointLimitLow();
+ desiredJointValues(i) =
+ (node->getJointLimitHigh() - node->getJointLimitLow()) * 0.5f +
+ node->getJointLimitLow();
}
-
-
}
// ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "desiredJointValues: " << desiredJointValues;
@@ -576,8 +622,10 @@ namespace armarx
{
VirtualRobot::RobotNodePtr node = nodes.at(i);
actualJointValues(i) = node->getJointValue();
- jointCompensationDeltas(i) = (node->getJointValue() - desiredJointValues(i)) / (node->getJointLimitHigh() - node->getJointLimitLow());
- jointCompensationDeltas(i) = -pow(jointCompensationDeltas(i), 3) * pow(nodes.size() - i, 2);
+ jointCompensationDeltas(i) = (node->getJointValue() - desiredJointValues(i)) /
+ (node->getJointLimitHigh() - node->getJointLimitLow());
+ jointCompensationDeltas(i) =
+ -pow(jointCompensationDeltas(i), 3) * pow(nodes.size() - i, 2);
}
// ARMARX_IMPORTANT << deactivateSpam(true, 0.5) << "actualJointValues: " << actualJointValues;
@@ -585,7 +633,10 @@ namespace armarx
return CalcNullspaceJointDeltas(jointCompensationDeltas, jacobian, jacobianInverse);
}
- Eigen::VectorXf TCPControlUnit::CalcNullspaceJointDeltas(const Eigen::VectorXf& desiredJointDeltas, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse)
+ Eigen::VectorXf
+ TCPControlUnit::CalcNullspaceJointDeltas(const Eigen::VectorXf& desiredJointDeltas,
+ const Eigen::MatrixXf& jacobian,
+ const Eigen::MatrixXf& jacobianInverse)
{
Eigen::MatrixXf I(jacobianInverse.rows(), jacobian.cols());
I.setIdentity();
@@ -596,41 +647,47 @@ namespace armarx
return delta;
}
- Eigen::VectorXf TCPControlUnit::applyMaxJointVelocity(const Eigen::VectorXf& jointVelocity, float maxJointVelocity)
+ Eigen::VectorXf
+ TCPControlUnit::applyMaxJointVelocity(const Eigen::VectorXf& jointVelocity,
+ float maxJointVelocity)
{
double currentMaxJointVel = std::numeric_limits<double>::min();
for (unsigned int i = 0; i < jointVelocity.rows(); i++)
{
- currentMaxJointVel = std::max(static_cast<double>(std::abs(jointVelocity(i))), currentMaxJointVel);
+ currentMaxJointVel =
+ std::max(static_cast<double>(std::abs(jointVelocity(i))), currentMaxJointVel);
}
if (currentMaxJointVel > maxJointVelocity)
{
- ARMARX_IMPORTANT << deactivateSpam(1) << "max joint velocity too high: " << currentMaxJointVel << " allowed: " << maxJointVelocity;
+ ARMARX_IMPORTANT << deactivateSpam(1)
+ << "max joint velocity too high: " << currentMaxJointVel
+ << " allowed: " << maxJointVelocity;
return jointVelocity * (maxJointVelocity / currentMaxJointVel);
}
else
{
return jointVelocity;
}
-
}
-
- PropertyDefinitionsPtr TCPControlUnit::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ TCPControlUnit::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new TCPControlUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(new TCPControlUnitPropertyDefinitions(getConfigIdentifier()));
}
- EDifferentialIK::EDifferentialIK(RobotNodeSetPtr rns, RobotNodePtr coordSystem, JacobiProvider::InverseJacobiMethod invJacMethod) :
+ EDifferentialIK::EDifferentialIK(RobotNodeSetPtr rns,
+ RobotNodePtr coordSystem,
+ JacobiProvider::InverseJacobiMethod invJacMethod) :
DifferentialIK(rns, coordSystem, invJacMethod)
{
}
- Eigen::MatrixXf EDifferentialIK::calcFullJacobian()
+ Eigen::MatrixXf
+ EDifferentialIK::calcFullJacobian()
{
if (nRows == 0)
{
@@ -656,16 +713,15 @@ namespace armarx
}
else
{
- VR_ERROR << "Internal error?!" << std::endl; // Error
+ VR_ERROR << "Internal error?!" << std::endl; // Error
}
-
-
}
return Jacobian;
}
- void EDifferentialIK::clearGoals()
+ void
+ EDifferentialIK::clearGoals()
{
targets.clear();
modes.clear();
@@ -675,39 +731,52 @@ namespace armarx
tcpWeights.clear();
}
- void EDifferentialIK::setRefFrame(RobotNodePtr coordSystem)
+ void
+ EDifferentialIK::setRefFrame(RobotNodePtr coordSystem)
{
this->coordSystem = coordSystem;
}
- void EDifferentialIK::setGoal(const Matrix4f& goal, RobotNodePtr tcp, IKSolver::CartesianSelection mode, float tolerancePosition, float toleranceRotation, VectorXf tcpWeight)
+ void
+ EDifferentialIK::setGoal(const Matrix4f& goal,
+ RobotNodePtr tcp,
+ IKSolver::CartesianSelection mode,
+ float tolerancePosition,
+ float toleranceRotation,
+ VectorXf tcpWeight)
{
- if (mode <= IKSolver::Z)
+ if (mode <= IKSolver::Z)
{
- ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 1) << "The tcpWeight vector must be of size 1";
+ ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 1)
+ << "The tcpWeight vector must be of size 1";
}
- else if (mode <= IKSolver::Orientation)
+ else if (mode <= IKSolver::Orientation)
{
- ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 3) << "The tcpWeight vector must be of size 3";
+ ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 3)
+ << "The tcpWeight vector must be of size 3";
}
- else if (mode == IKSolver::All)
+ else if (mode == IKSolver::All)
{
- ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 6) << "The tcpWeight vector must be of size 6";
+ ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 6)
+ << "The tcpWeight vector must be of size 6";
}
tcpWeights[tcp] = tcpWeight;
DifferentialIK::setGoal(goal, tcp, mode, tolerancePosition, toleranceRotation);
}
- void EDifferentialIK::setDOFWeights(Eigen::VectorXf dofWeights)
+ void
+ EDifferentialIK::setDOFWeights(Eigen::VectorXf dofWeights)
{
this->dofWeights = dofWeights;
}
- bool EDifferentialIK::computeSteps(float stepSize, float mininumChange, int maxNStep)
+ bool
+ EDifferentialIK::computeSteps(float stepSize, float mininumChange, int maxNStep)
{
VectorXf jointDelta;
- return computeSteps(jointDelta, stepSize, mininumChange, maxNStep, &DifferentialIK::computeStep);
+ return computeSteps(
+ jointDelta, stepSize, mininumChange, maxNStep, &DifferentialIK::computeStep);
}
// void EDifferentialIK::setTCPWeights(Eigen::VectorXf tcpWeights)
@@ -715,7 +784,12 @@ namespace armarx
// this->tcpWeights = tcpWeights;
// }
- bool EDifferentialIK::computeSteps(VectorXf& resultJointDelta, float stepSize, float mininumChange, int maxNStep, ComputeFunction computeFunction)
+ bool
+ EDifferentialIK::computeSteps(VectorXf& resultJointDelta,
+ float stepSize,
+ float mininumChange,
+ int maxNStep,
+ ComputeFunction computeFunction)
{
VR_ASSERT(rns);
VR_ASSERT(nodes.size() == rns->getSize());
@@ -727,7 +801,7 @@ namespace armarx
int step = 0;
checkTolerances();
- std::vector<std::pair<float, VectorXf> > jointDeltaIterations;
+ std::vector<std::pair<float, VectorXf>> jointDeltaIterations;
float lastDist = FLT_MAX;
VectorXf oldJointValues;
rns->getJointValues(oldJointValues);
@@ -746,7 +820,6 @@ namespace armarx
}
-
for (unsigned int i = 0; i < nodes.size(); i++)
{
ARMARX_DEBUG << VAROUT(nodes[i]->getJointValue()) << VAROUT(dTheta[i]);
@@ -789,14 +862,20 @@ namespace armarx
if (dTheta.norm() < mininumChange)
{
// if (verbose)
- ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << " Resulting error: pos " << posDist << " orientation: " << oriErr << std::endl;
+ ARMARX_INFO << deactivateSpam(1)
+ << "Could not improve result any more (dTheta.norm()=" << d
+ << "), loop:" << step << " Resulting error: pos " << posDist
+ << " orientation: " << oriErr << std::endl;
break;
}
if (checkImprovement && posDist > lastDist)
{
// if (verbose)
- ARMARX_INFO << deactivateSpam(1) << "Could not improve result any more (current position error=" << posDist << ", last loop's error:" << lastDist << "), loop:" << step << std::endl;
+ ARMARX_INFO << deactivateSpam(1)
+ << "Could not improve result any more (current position error="
+ << posDist << ", last loop's error:" << lastDist << "), loop:" << step
+ << std::endl;
robot->setJointValues(rns, jvBest);
break;
}
@@ -806,8 +885,7 @@ namespace armarx
step++;
jointDeltaIterations.push_back(std::make_pair(getWeightedError(), resultJointDelta));
- }
- while (step < maxNStep);
+ } while (step < maxNStep);
float bestJVError = std::numeric_limits<float>::max();
@@ -818,8 +896,6 @@ namespace armarx
{
bestJVError = jointDeltaIterations.at(i).first;
resultJointDelta = jointDeltaIterations.at(i).second;
-
-
}
}
@@ -833,18 +909,20 @@ namespace armarx
// ARMARX_IMPORTANT << "tcp delta:\n" << newPose.block(0,3,3,1) - oldPose.block(0,3,3,1);
dofWeights = tempDOFWeights;
- if (step >= maxNStep && verbose)
+ if (step >= maxNStep && verbose)
{
ARMARX_INFO << deactivateSpam(1) << "IK failed, loop:" << step << std::endl;
ARMARX_INFO << deactivateSpam(1) << "pos error:" << getMeanErrorPosition() << std::endl;
- ARMARX_INFO << deactivateSpam(1) << "rot error:" << getErrorRotation(rns->getTCP()) << std::endl;
+ ARMARX_INFO << deactivateSpam(1) << "rot error:" << getErrorRotation(rns->getTCP())
+ << std::endl;
return false;
}
return true;
}
- VectorXf EDifferentialIK::computeStep(float stepSize)
+ VectorXf
+ EDifferentialIK::computeStep(float stepSize)
{
if (nRows == 0)
@@ -897,14 +975,11 @@ namespace armarx
error.segment(index, 3) = delta.tail(3) * stepSize;
index += 3;
}
-
}
else
{
- VR_ERROR << "Internal error?!" << std::endl; // Error
+ VR_ERROR << "Internal error?!" << std::endl; // Error
}
-
-
}
// applyDOFWeightsToJacobian(Jacobian);
@@ -915,8 +990,8 @@ namespace armarx
return pseudo * error;
}
-
- VectorXf EDifferentialIK::computeStepIndependently(float stepSize)
+ VectorXf
+ EDifferentialIK::computeStepIndependently(float stepSize)
{
if (nRows == 0)
{
@@ -925,7 +1000,7 @@ namespace armarx
size_t nDoF = nodes.size();
- std::map<float, MatrixXf> partJacobians;
+ std::map<float, MatrixXf> partJacobians;
VectorXf thetaSum(nDoF);
thetaSum.setZero();
@@ -945,15 +1020,15 @@ namespace armarx
// applyTCPWeights(tcp, partJacobian);
int tcpDOF = 0;
- if (mode <= IKSolver::Z)
+ if (mode <= IKSolver::Z)
{
tcpDOF = 1;
}
- else if (mode <= IKSolver::Orientation)
+ else if (mode <= IKSolver::Orientation)
{
tcpDOF = 3;
}
- else if (mode == IKSolver::All)
+ else if (mode == IKSolver::All)
{
tcpDOF = 6;
}
@@ -989,7 +1064,8 @@ namespace armarx
}
- ARMARX_DEBUG << deactivateSpam(0.05) << "step size adjusted error to goal:\n" << partError;
+ ARMARX_DEBUG << deactivateSpam(0.05) << "step size adjusted error to goal:\n"
+ << partError;
applyDOFWeightsToJacobian(partJacobian);
// ARMARX_DEBUG << "Jac:\n" << partJacobian;
@@ -998,7 +1074,8 @@ namespace armarx
MatrixXf pseudo = computePseudoInverseJacobianMatrix(partJacobian);
Eigen::VectorXf tcpWeightVec;
- std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf>::iterator it = tcpWeights.find(tcp);
+ std::map<VirtualRobot::SceneObjectPtr, Eigen::VectorXf>::iterator it =
+ tcpWeights.find(tcp);
if (it != tcpWeights.end())
{
@@ -1009,15 +1086,15 @@ namespace armarx
IKSolver::CartesianSelection mode = modes[tcp_set.at(i)];
int size = 1;
- if (mode <= IKSolver::Z)
+ if (mode <= IKSolver::Z)
{
size = 1;
}
- else if (mode <= IKSolver::Orientation)
+ else if (mode <= IKSolver::Orientation)
{
size = 3;
}
- else if (mode == IKSolver::All)
+ else if (mode == IKSolver::All)
{
size = 6;
}
@@ -1025,7 +1102,6 @@ namespace armarx
Eigen::VectorXf tmptcpWeightVec(size);
tmptcpWeightVec.setOnes();
tcpWeightVec = tmptcpWeightVec;
-
}
@@ -1037,30 +1113,33 @@ namespace armarx
weightMat.diagonal() = tcpWeightVec;
// ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
// ARMARX_DEBUG << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
- pseudo = pseudo * weightMat;
+ pseudo = pseudo * weightMat;
// ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "InvJac after:\n" << pseudo;
}
else
{
- ARMARX_WARNING << deactivateSpam(3) << "Wrong dimension of tcp weights: " << tcpWeightVec.rows() << ", but should be: " << pseudo.cols();
+ ARMARX_WARNING << deactivateSpam(3)
+ << "Wrong dimension of tcp weights: " << tcpWeightVec.rows()
+ << ", but should be: " << pseudo.cols();
}
thetaSum += pseudo * partError;
-
}
else
{
- VR_ERROR << "Internal error?!" << std::endl; // Error
+ VR_ERROR << "Internal error?!" << std::endl; // Error
}
-
-
}
return thetaSum;
}
- bool EDifferentialIK::solveIK(float stepSize, float minChange, int maxSteps, bool useAlternativeOnFail)
+ bool
+ EDifferentialIK::solveIK(float stepSize,
+ float minChange,
+ int maxSteps,
+ bool useAlternativeOnFail)
{
Eigen::VectorXf jointValuesBefore;
rns->getJointValues(jointValuesBefore);
@@ -1071,7 +1150,11 @@ namespace armarx
if (useAlternativeOnFail && error > 5)
{
- result = computeSteps(jointDeltaAlternative, stepSize, minChange, maxSteps, &EDifferentialIK::computeStepIndependently);
+ result = computeSteps(jointDeltaAlternative,
+ stepSize,
+ minChange,
+ maxSteps,
+ &EDifferentialIK::computeStepIndependently);
}
if (getWeightedError() < error)
@@ -1082,7 +1165,8 @@ namespace armarx
return result;
}
- void EDifferentialIK::applyDOFWeightsToJacobian(MatrixXf& Jacobian)
+ void
+ EDifferentialIK::applyDOFWeightsToJacobian(MatrixXf& Jacobian)
{
if (dofWeights.rows() == Jacobian.cols())
{
@@ -1093,14 +1177,14 @@ namespace armarx
// ARMARX_DEBUG << deactivateSpam(1) << "Weights:\n" << weightMat;
Jacobian = Jacobian * weightMat;
// ARMARX_DEBUG << deactivateSpam(1) << "Jac after:\n" << Jacobian;
-
}
}
- void EDifferentialIK::applyTCPWeights(RobotNodePtr tcp, MatrixXf& partJacobian)
+ void
+ EDifferentialIK::applyTCPWeights(RobotNodePtr tcp, MatrixXf& partJacobian)
{
- std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf>::iterator it = tcpWeights.find(tcp);
+ std::map<VirtualRobot::SceneObjectPtr, Eigen::VectorXf>::iterator it = tcpWeights.find(tcp);
if (it != tcpWeights.end())
{
@@ -1118,17 +1202,21 @@ namespace armarx
}
else
{
- ARMARX_WARNING << deactivateSpam(3) << "Wrong dimension of tcp weights: " << tcpWeightVec.rows() << ", but should be: " << partJacobian.rows();
+ ARMARX_WARNING << deactivateSpam(3)
+ << "Wrong dimension of tcp weights: " << tcpWeightVec.rows()
+ << ", but should be: " << partJacobian.rows();
}
}
}
- void EDifferentialIK::applyTCPWeights(MatrixXf& invJacobian)
+ void
+ EDifferentialIK::applyTCPWeights(MatrixXf& invJacobian)
{
if (tcpWeightVec.rows() == 0)
for (size_t i = 0; i < tcp_set.size(); i++)
{
- std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf>::iterator it = tcpWeights.find(tcp_set.at(i));
+ std::map<VirtualRobot::SceneObjectPtr, Eigen::VectorXf>::iterator it =
+ tcpWeights.find(tcp_set.at(i));
if (it != tcpWeights.end())
{
@@ -1148,15 +1236,15 @@ namespace armarx
IKSolver::CartesianSelection mode = modes[tcp_set.at(i)];
int size = 1;
- if (mode <= IKSolver::Z)
+ if (mode <= IKSolver::Z)
{
size = 1;
}
- else if (mode <= IKSolver::Orientation)
+ else if (mode <= IKSolver::Orientation)
{
size = 3;
}
- else if (mode == IKSolver::All)
+ else if (mode == IKSolver::All)
{
size = 6;
}
@@ -1172,7 +1260,6 @@ namespace armarx
Eigen::VectorXf tmptcpWeightVec(size);
tmptcpWeightVec.setOnes();
tcpWeightVec.tail(size) = tmptcpWeightVec;
-
}
}
@@ -1184,17 +1271,19 @@ namespace armarx
weightMat.diagonal() = tcpWeightVec;
// ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "tcpWeightVec:\n" << tcpWeightVec;
// ARMARX_DEBUG << /*deactivateSpam(1) << */"InvJac Before:\n" << invJacobian;
- invJacobian = invJacobian * weightMat;
+ invJacobian = invJacobian * weightMat;
ARMARX_DEBUG << /*deactivateSpam(1) <<*/ "InvJac after:\n" << invJacobian;
}
else
{
- ARMARX_WARNING << deactivateSpam(3) << "Wrong dimension of tcp weights: " << tcpWeightVec.rows() << ", but should be: " << invJacobian.cols();
+ ARMARX_WARNING << deactivateSpam(3)
+ << "Wrong dimension of tcp weights: " << tcpWeightVec.rows()
+ << ", but should be: " << invJacobian.cols();
}
-
}
- float EDifferentialIK::getWeightedError()
+ float
+ EDifferentialIK::getWeightedError()
{
float result = 0.0f;
float positionOrientationRatio = 3.f / 180.f * M_PI;
@@ -1202,17 +1291,19 @@ namespace armarx
for (size_t i = 0; i < tcp_set.size(); i++)
{
SceneObjectPtr tcp = tcp_set[i];
- result += getWeightedErrorPosition(tcp) + getErrorRotation(tcp) * positionOrientationRatio;
+ result +=
+ getWeightedErrorPosition(tcp) + getErrorRotation(tcp) * positionOrientationRatio;
}
return result;
}
- float EDifferentialIK::getWeightedErrorPosition(SceneObjectPtr tcp)
+ float
+ EDifferentialIK::getWeightedErrorPosition(SceneObjectPtr tcp)
{
if (modes[tcp] == IKSolver::Orientation)
{
- return 0.0f; // ignoring position
+ return 0.0f; // ignoring position
}
if (!tcp)
@@ -1258,13 +1349,13 @@ namespace armarx
return sqrtf(result);
}
- bool EDifferentialIK::adjustDOFWeightsToJointLimits(const VectorXf& plannedJointDeltas)
+ bool
+ EDifferentialIK::adjustDOFWeightsToJointLimits(const VectorXf& plannedJointDeltas)
{
if (dofWeights.rows() != plannedJointDeltas.rows())
{
dofWeights.resize(plannedJointDeltas.rows());
dofWeights.setOnes();
-
}
bool result = false;
@@ -1275,31 +1366,36 @@ namespace armarx
if (angle > nodes[i]->getJointLimitHi() || angle < nodes[i]->getJointLimitLo())
{
- ARMARX_VERBOSE << deactivateSpam(3) << nodes[i]->getName() << " joint deactivated because of joint limit";
+ ARMARX_VERBOSE << deactivateSpam(3) << nodes[i]->getName()
+ << " joint deactivated because of joint limit";
dofWeights(i) = 0;
result = true;
}
}
return result;
-
}
+} // namespace armarx
-}
-
-
-
-void armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
std::unique_lock lock(reportMutex);
FramedPoseBaseMap tcpPoses;
- std::string rootFrame = localReportRobot->getRootNode()->getName();
+ std::string rootFrame = localReportRobot->getRootNode()->getName();
auto it = jointAngles.find("timestamp");
if (it == jointAngles.end())
{
@@ -1314,7 +1410,7 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles,
for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
RobotNodePtr& node = nodesToReport.at(i);
- const std::string& tcpName = node->getName();
+ const std::string& tcpName = node->getName();
const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robotName);
}
@@ -1322,7 +1418,11 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles,
listener->reportTCPPose(tcpPoses);
}
-void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
if ((TimeUtil::GetTime() - lastTopicReportTime).toMilliSeconds() < cycleTime)
{
@@ -1340,17 +1440,16 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
// ARMARX_DEBUG << jointVel; FramedPoseBaseMap tcpPoses;
FramedDirectionMap tcpTranslationVelocities;
FramedDirectionMap tcpOrientationVelocities;
- std::string rootFrame = localReportRobot->getRootNode()->getName();
+ std::string rootFrame = localReportRobot->getRootNode()->getName();
NameValueMap tempJointAngles = localReportRobot->getConfig()->getRobotNodeJointValueMap();
FramedPoseBaseMap tcpPoses;
for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
RobotNodePtr node = nodesToReport.at(i);
- const std::string& tcpName = node->getName();
+ const std::string& tcpName = node->getName();
const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robotName);
-
}
double tDelta = 0.001;
@@ -1373,13 +1472,16 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
RobotNodePtr node = localVelReportRobot->getRobotNode(nodesToReport.at(i)->getName());
- const std::string& tcpName = node->getName();
+ const std::string& tcpName = node->getName();
const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
- tcpTranslationVelocities[tcpName] = new FramedDirection((currentPose.block(0, 3, 3, 1) - lastPose->toEigen().block(0, 3, 3, 1)) / tDelta, rootFrame, localReportRobot->getName());
+ tcpTranslationVelocities[tcpName] = new FramedDirection(
+ (currentPose.block(0, 3, 3, 1) - lastPose->toEigen().block(0, 3, 3, 1)) / tDelta,
+ rootFrame,
+ localReportRobot->getName());
const Eigen::Matrix4f mat = currentPose * lastPose->toEigen().inverse();
// const Eigen::Matrix3f rot = currentPose.block(0,0,3,3) * lastPose->toEigen().block(0,0,3,3).inverse();
@@ -1388,30 +1490,47 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
// Eigen::AngleAxisf orient(rot.block<3,3>(0,0));
tcpOrientationVelocities[tcpName] = new FramedDirection(rpy / tDelta, rootFrame, robotName);
-
-
}
listener->reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
}
-void armarx::TCPControlUnit::reportJointTorques(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::TCPControlUnit::reportJointTorques(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointAccelerations(const armarx::NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
+void
+armarx::TCPControlUnit::reportJointAccelerations(const armarx::NameValueMap& jointAccelerations,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c)
{
-
}
-void armarx::TCPControlUnit::reportJointCurrents(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::TCPControlUnit::reportJointCurrents(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointMotorTemperatures(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::TCPControlUnit::reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointStatuses(const NameStatusMap&, Ice::Long timestamp, bool, const Ice::Current&)
+void
+armarx::TCPControlUnit::reportJointStatuses(const NameStatusMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&)
{
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index 8811a63dd792a7186c12cd8d2cd05d55220655ca..c514f465d65648d58fe0af0090f7582f82a4f9cb 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -22,18 +22,18 @@
#pragma once
-#include <RobotAPI/interface/units/TCPControlUnit.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <ArmarXCore/core/Component.h>
+#include <memory>
-#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/VirtualRobot.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <memory>
+#include <RobotAPI/interface/units/TCPControlUnit.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
namespace armarx
{
@@ -41,22 +41,33 @@ namespace armarx
* \class TCPControlUnitPropertyDefinitions
* \brief
*/
- class TCPControlUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class TCPControlUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- TCPControlUnitPropertyDefinitions(std::string prefix):
- ComponentPropertyDefinitions(prefix)
+ TCPControlUnitPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit Proxy");
- defineOptionalProperty<std::string>("RobotStateTopicName", "RobotState", "Name of the RobotComponent State topic.");
- defineOptionalProperty<float>("MaxJointVelocity", 30.f / 180 * 3.141, "Maximal joint velocity in rad/sec");
+ defineRequiredProperty<std::string>("KinematicUnitName",
+ "Name of the KinematicUnit Proxy");
+ defineOptionalProperty<std::string>(
+ "RobotStateTopicName", "RobotState", "Name of the RobotComponent State topic.");
+ defineOptionalProperty<float>(
+ "MaxJointVelocity", 30.f / 180 * 3.141, "Maximal joint velocity in rad/sec");
defineOptionalProperty<int>("CycleTime", 30, "Cycle time of the tcp control in ms");
// defineOptionalProperty<float>("MaximumCommandDelay", 20000, "Delay after which the TCP Control unit releases itself if no new velocity have been set.");
- defineOptionalProperty<std::string>("TCPsToReport", "", "comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none");
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
-
- defineOptionalProperty<float>("LambdaDampedSVD", 0.1f, "Parameter used for smoothing the differential IK near singularities.");
+ defineOptionalProperty<std::string>(
+ "TCPsToReport",
+ "",
+ "comma seperated list of nodesets' endeffectors, which poses and velocities that "
+ "should be reported. * for all, empty for none");
+ defineOptionalProperty<std::string>(
+ "RobotStateComponentName",
+ "RobotStateComponent",
+ "Name of the RobotStateComponent that should be used");
+
+ defineOptionalProperty<float>(
+ "LambdaDampedSVD",
+ 0.1f,
+ "Parameter used for smoothing the differential IK near singularities.");
}
};
@@ -83,9 +94,7 @@ namespace armarx
* @ingroup Component-TCPControlUnit
* @brief The TCPControlUnit class
*/
- class TCPControlUnit :
- virtual public Component,
- virtual public TCPControlUnitInterface
+ class TCPControlUnit : virtual public Component, virtual public TCPControlUnitInterface
{
public:
TCPControlUnit();
@@ -97,7 +106,8 @@ namespace armarx
* \param milliseconds New cycle time.
* \param c Ice Context, leave blank.
*/
- void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setCycleTime(Ice::Int milliseconds,
+ const Ice::Current& c = Ice::emptyCurrent) override;
/**
* \brief Sets the cartesian velocity of a node in a nodeset for translation and/or orientation.
@@ -114,7 +124,11 @@ namespace armarx
*
* \see request(), release()
*/
- void setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const::armarx::FramedDirectionBasePtr& translationVelocity, const::armarx::FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c = Ice::emptyCurrent) override;
+ void setTCPVelocity(const std::string& nodeSetName,
+ const std::string& tcpName,
+ const ::armarx::FramedDirectionBasePtr& translationVelocity,
+ const ::armarx::FramedDirectionBasePtr& orientationVelocityRPY,
+ const Ice::Current& c = Ice::emptyCurrent) override;
// UnitExecutionManagementInterface interface
/**
@@ -152,12 +166,13 @@ namespace armarx
bool isRequested(const Ice::Current& c = Ice::emptyCurrent) override;
protected:
-
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
void onExitComponent() override;
- std::string getDefaultName() const override
+
+ std::string
+ getDefaultName() const override
{
return "TCPControlUnit";
}
@@ -165,15 +180,28 @@ namespace armarx
// PropertyUser interface
PropertyDefinitionsPtr createPropertyDefinitions() override;
- static Eigen::VectorXf CalcNullspaceJointDeltas(const Eigen::VectorXf& desiredJointDeltas, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse);
- static Eigen::VectorXf CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::MatrixXf& jacobian, const Eigen::MatrixXf& jacobianInverse, Eigen::VectorXf desiredJointValues = Eigen::VectorXf());
+ static Eigen::VectorXf CalcNullspaceJointDeltas(const Eigen::VectorXf& desiredJointDeltas,
+ const Eigen::MatrixXf& jacobian,
+ const Eigen::MatrixXf& jacobianInverse);
+ static Eigen::VectorXf
+ CalcJointLimitAvoidanceDeltas(VirtualRobot::RobotNodeSetPtr robotNodeSet,
+ const Eigen::MatrixXf& jacobian,
+ const Eigen::MatrixXf& jacobianInverse,
+ Eigen::VectorXf desiredJointValues = Eigen::VectorXf());
void calcAndSetVelocities();
+
private:
- static void ContinuousAngles(const Eigen::AngleAxisf& oldAngle, Eigen::AngleAxisf& newAngle, double& offset);
+ static void ContinuousAngles(const Eigen::AngleAxisf& oldAngle,
+ Eigen::AngleAxisf& newAngle,
+ double& offset);
void periodicExec();
void periodicReport();
- Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, VirtualRobot::RobotNodePtr refFrame, VirtualRobot::IKSolver::CartesianSelection mode);
- Eigen::VectorXf applyMaxJointVelocity(const Eigen::VectorXf& jointVelocity, float maxJointVelocity);
+ Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet,
+ Eigen::VectorXf tcpDelta,
+ VirtualRobot::RobotNodePtr refFrame,
+ VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf applyMaxJointVelocity(const Eigen::VectorXf& jointVelocity,
+ float maxJointVelocity);
float maxJointVelocity;
int cycleTime;
@@ -210,7 +238,7 @@ namespace armarx
bool requested;
std::map<std::string, double> oriOffsets;
- std::vector<VirtualRobot::RobotNodePtr > nodesToReport;
+ std::vector<VirtualRobot::RobotNodePtr> nodesToReport;
FramedPoseBaseMap lastTCPPoses;
IceUtil::Time lastTopicReportTime;
@@ -223,32 +251,59 @@ namespace armarx
std::string topicName;
-
// KinematicUnitListener interface
protected:
- void reportControlModeChanged(const NameControlModeMap&, Ice::Long timestamp, bool, const Ice::Current&) override;
- void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool, const Ice::Current&) override;
- void reportJointVelocities(const NameValueMap& jointVel, Ice::Long timestamp, bool, const Ice::Current&) override;
- void reportJointTorques(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&) override;
- void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
- void reportJointCurrents(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&) override;
- void reportJointMotorTemperatures(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&) override;
- void reportJointStatuses(const NameStatusMap&, Ice::Long timestamp, bool, const Ice::Current&) override;
-
+ void reportControlModeChanged(const NameControlModeMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&) override;
+ void reportJointAngles(const NameValueMap& jointAngles,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&) override;
+ void reportJointVelocities(const NameValueMap& jointVel,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&) override;
+ void reportJointTorques(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&) override;
+ void reportJointAccelerations(const NameValueMap& jointAccelerations,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current& c) override;
+ void reportJointCurrents(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&) override;
+ void reportJointMotorTemperatures(const NameValueMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&) override;
+ void reportJointStatuses(const NameStatusMap&,
+ Ice::Long timestamp,
+ bool,
+ const Ice::Current&) override;
};
class EDifferentialIK : public VirtualRobot::DifferentialIK, virtual public Logging
{
public:
- typedef Eigen::VectorXf(EDifferentialIK::*ComputeFunction)(float);
+ typedef Eigen::VectorXf (EDifferentialIK::*ComputeFunction)(float);
- EDifferentialIK(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot:: RobotNodePtr coordSystem = VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = eSVD);
+ EDifferentialIK(VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr coordSystem = VirtualRobot::RobotNodePtr(),
+ VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = eSVD);
- VirtualRobot::RobotNodePtr getRefFrame()
+ VirtualRobot::RobotNodePtr
+ getRefFrame()
{
return coordSystem;
}
- int getJacobianRows()
+
+ int
+ getJacobianRows()
{
return nRows;
}
@@ -258,12 +313,22 @@ namespace armarx
void clearGoals();
void setRefFrame(VirtualRobot::RobotNodePtr coordSystem);
- void setGoal(const Eigen::Matrix4f& goal, VirtualRobot::RobotNodePtr tcp, VirtualRobot::IKSolver::CartesianSelection mode, float tolerancePosition, float toleranceRotation, Eigen::VectorXf tcpWeight);
+ void setGoal(const Eigen::Matrix4f& goal,
+ VirtualRobot::RobotNodePtr tcp,
+ VirtualRobot::IKSolver::CartesianSelection mode,
+ float tolerancePosition,
+ float toleranceRotation,
+ Eigen::VectorXf tcpWeight);
void setDOFWeights(Eigen::VectorXf dofWeights);
// void setTCPWeights(Eigen::VectorXf tcpWeights);
- bool computeSteps(float stepSize = 1.f, float mininumChange = 0.01f, int maxNStep = 50) override;
- bool computeSteps(Eigen::VectorXf& resultJointDelta, float stepSize = 1.f, float mininumChange = 0.01f, int maxNStep = 50, ComputeFunction computeFunction = &DifferentialIK::computeStep);
+ bool
+ computeSteps(float stepSize = 1.f, float mininumChange = 0.01f, int maxNStep = 50) override;
+ bool computeSteps(Eigen::VectorXf& resultJointDelta,
+ float stepSize = 1.f,
+ float mininumChange = 0.01f,
+ int maxNStep = 50,
+ ComputeFunction computeFunction = &DifferentialIK::computeStep);
Eigen::VectorXf computeStep(float stepSize) override;
void applyDOFWeightsToJacobian(Eigen::MatrixXf& Jacobian);
void applyTCPWeights(VirtualRobot::RobotNodePtr tcp, Eigen::MatrixXf& partJacobian);
@@ -271,14 +336,19 @@ namespace armarx
float getWeightedError();
float getWeightedErrorPosition(VirtualRobot::SceneObjectPtr tcp);
Eigen::VectorXf computeStepIndependently(float stepSize);
- bool solveIK(float stepSize = 1, float minChange = 0.01f, int maxSteps = 50, bool useAlternativeOnFail = false);
+ bool solveIK(float stepSize = 1,
+ float minChange = 0.01f,
+ int maxSteps = 50,
+ bool useAlternativeOnFail = false);
+
protected:
bool adjustDOFWeightsToJointLimits(const Eigen::VectorXf& plannedJointDeltas);
Eigen::VectorXf dofWeights;
- std::map<VirtualRobot:: SceneObjectPtr, Eigen::VectorXf> tcpWeights;
+ std::map<VirtualRobot::SceneObjectPtr, Eigen::VectorXf> tcpWeights;
Eigen::VectorXf tcpWeightVec;
};
+
using EDifferentialIKPtr = std::shared_ptr<EDifferentialIK>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.cpp b/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
index 8fda50da01097a9f7eb8ba77d056dfb19dda12e1..35ce23a9972728f1d037f4bc146028fb8b3c58bd 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.cpp
@@ -23,14 +23,15 @@
#include "TCPControlUnitObserver.h"
//#include <ArmarXCore/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
-#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
+#include <ArmarXCore/observers/checks/ConditionCheckUpdated.h>
+
#include <RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
#include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#define TCP_POSE_CHANNEL "TCPPose"
#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
@@ -42,7 +43,8 @@ namespace armarx
{
}
- void TCPControlUnitObserver::onInitObserver()
+ void
+ TCPControlUnitObserver::onInitObserver()
{
usingTopic(getProperty<std::string>("TCPControlUnitName").getValue() + "State");
@@ -54,25 +56,24 @@ namespace armarx
// offerConditionCheck("magnitudeinrange", new ConditionCheckInRange());
offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
offerConditionCheck("approx", new ConditionCheckApproxPose());
-
-
-
}
- void TCPControlUnitObserver::onConnectObserver()
+ void
+ TCPControlUnitObserver::onConnectObserver()
{
offerChannel(TCP_POSE_CHANNEL, "TCP poses of the robot.");
offerChannel(TCP_TRANS_VELOCITIES_CHANNEL, "TCP velocities of the robot.");
-
}
- PropertyDefinitionsPtr TCPControlUnitObserver::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ TCPControlUnitObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new TCPControlUnitObserverPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new TCPControlUnitObserverPropertyDefinitions(getConfigIdentifier()));
}
- void TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current&)
+ void
+ TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current&)
{
std::unique_lock lock(dataMutex);
// ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
@@ -86,7 +87,8 @@ namespace armarx
if (!existsDataField(TCP_POSE_CHANNEL, tcpName))
{
- offerDataFieldWithDefault(TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ offerDataFieldWithDefault(
+ TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
}
else
{
@@ -101,10 +103,14 @@ namespace armarx
offerDataFieldWithDefault(tcpName, "x", Variant(vec->position->x), "X axis");
offerDataFieldWithDefault(tcpName, "y", Variant(vec->position->y), "Y axis");
offerDataFieldWithDefault(tcpName, "z", Variant(vec->position->z), "Z axis");
- offerDataFieldWithDefault(tcpName, "qx", Variant(vec->orientation->qx), "Quaternion part x");
- offerDataFieldWithDefault(tcpName, "qy", Variant(vec->orientation->qy), "Quaternion part y");
- offerDataFieldWithDefault(tcpName, "qz", Variant(vec->orientation->qz), "Quaternion part z");
- offerDataFieldWithDefault(tcpName, "qw", Variant(vec->orientation->qw), "Quaternion part w");
+ offerDataFieldWithDefault(
+ tcpName, "qx", Variant(vec->orientation->qx), "Quaternion part x");
+ offerDataFieldWithDefault(
+ tcpName, "qy", Variant(vec->orientation->qy), "Quaternion part y");
+ offerDataFieldWithDefault(
+ tcpName, "qz", Variant(vec->orientation->qz), "Quaternion part z");
+ offerDataFieldWithDefault(
+ tcpName, "qw", Variant(vec->orientation->qw), "Quaternion part w");
offerDataFieldWithDefault(tcpName, "frame", Variant(vec->frame), "Reference Frame");
}
else
@@ -120,11 +126,13 @@ namespace armarx
}
updateChannel(tcpName);
-
}
}
- void TCPControlUnitObserver::reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current&)
+ void
+ TCPControlUnitObserver::reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities,
+ const FramedDirectionMap& tcpOrientationVelocities,
+ const Ice::Current&)
{
std::unique_lock lock(dataMutex);
FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();
@@ -147,7 +155,10 @@ namespace armarx
if (!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
{
- offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL,
+ tcpName,
+ Variant(it->second),
+ "Pose of " + tcpName);
}
else
{
@@ -166,7 +177,8 @@ namespace armarx
offerDataFieldWithDefault(channelName, "roll", Variant(vecOri->x), "Roll");
offerDataFieldWithDefault(channelName, "pitch", Variant(vecOri->y), "Pitch");
offerDataFieldWithDefault(channelName, "yaw", Variant(vecOri->z), "Yaw");
- offerDataFieldWithDefault(channelName, "frame", Variant(vecOri->frame), "Reference Frame");
+ offerDataFieldWithDefault(
+ channelName, "frame", Variant(vecOri->frame), "Reference Frame");
}
else
{
@@ -180,10 +192,7 @@ namespace armarx
}
updateChannel(channelName);
-
}
}
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/TCPControlUnitObserver.h b/source/RobotAPI/components/units/TCPControlUnitObserver.h
index 92859f6e597b85231d02a6fd1b00b92970f642fa..cb6d7e559adb4d9ca800e9a87b0b014f3c412603 100644
--- a/source/RobotAPI/components/units/TCPControlUnitObserver.h
+++ b/source/RobotAPI/components/units/TCPControlUnitObserver.h
@@ -21,10 +21,11 @@
*/
#pragma once
+#include <mutex>
+
#include <ArmarXCore/observers/Observer.h>
-#include <RobotAPI/interface/units/TCPControlUnit.h>
-#include <mutex>
+#include <RobotAPI/interface/units/TCPControlUnit.h>
namespace armarx
{
@@ -32,14 +33,14 @@ namespace armarx
* \class TCPControlUnitObserverPropertyDefinitions
* \brief
*/
- class TCPControlUnitObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class TCPControlUnitObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- TCPControlUnitObserverPropertyDefinitions(std::string prefix):
+ TCPControlUnitObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("TCPControlUnitName", "TCPControlUnit", "Name of the TCPControlUnit");
+ defineOptionalProperty<std::string>(
+ "TCPControlUnitName", "TCPControlUnit", "Name of the TCPControlUnit");
}
};
@@ -61,10 +62,12 @@ namespace armarx
TCPControlUnitObserver();
// framework hooks
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "TCPControlUnitObserver";
}
+
void onInitObserver() override;
void onConnectObserver() override;
@@ -75,11 +78,13 @@ namespace armarx
public:
// TCPControlUnitListener interface
- void reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current& c = Ice::emptyCurrent) override;
- void reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportTCPPose(const FramedPoseBaseMap& poseMap,
+ const Ice::Current& c = Ice::emptyCurrent) override;
+ void reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities,
+ const FramedDirectionMap& tcpOrientationVelocities,
+ const Ice::Current& c = Ice::emptyCurrent) override;
std::mutex dataMutex;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/CyberGloveUnit/CyberGloveInterface.ice b/source/RobotAPI/drivers/CyberGloveUnit/CyberGloveInterface.ice
index 1e4dd648305747cc2ecedf395f0fa2e3d98aacdb..e9b3235ec2c2238755e7be9826ec96ce8b99d0fc 100644
--- a/source/RobotAPI/drivers/CyberGloveUnit/CyberGloveInterface.ice
+++ b/source/RobotAPI/drivers/CyberGloveUnit/CyberGloveInterface.ice
@@ -63,7 +63,7 @@ module armarx
interface CyberGloveInterface
{
- string getTopicName();
+ string getTopicName();
};
};
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
index 14cc8fb49d139d177425040e4b69334f4e9e042a..5c8bd0e40f1d4e549d0a28adbe67f512fc753f72 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp
@@ -23,13 +23,15 @@
#include "GamepadUnit.h"
#include "ArmarXCore/core/logging/Logging.h"
-#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+
#include <linux/joystick.h>
using namespace armarx;
-void GamepadUnit::onInitComponent()
+void
+GamepadUnit::onInitComponent()
{
ARMARX_TRACE;
offeringTopic(getProperty<std::string>("GamepadTopicName").getValue());
@@ -38,58 +40,68 @@ void GamepadUnit::onInitComponent()
readTask = new RunningTask<GamepadUnit>(this, &GamepadUnit::run, "GamepadUnit");
}
-void GamepadUnit::onConnectComponent()
+void
+GamepadUnit::onConnectComponent()
{
ARMARX_TRACE;
- topicPrx = getTopic<GamepadUnitListenerPrx>(getProperty<std::string>("GamepadTopicName").getValue());
+ topicPrx =
+ getTopic<GamepadUnitListenerPrx>(getProperty<std::string>("GamepadTopicName").getValue());
ARMARX_TRACE;
- sendTask = new SimplePeriodicTask<>([&]
- {
- ARMARX_TRACE;
- std::unique_lock lock(mutex);
- if (!js.opened())
- {
- return;
- }
- ARMARX_TRACE;
- if (!dataTimestamp)
- {
- ARMARX_INFO << deactivateSpam(1) << "dataTimestamp is null, waiting for value";
- return;
- }
- ARMARX_CHECK_NOT_NULL(dataTimestamp);
- const IceUtil::Time age = IceUtil::Time::now() - dataTimestamp->toTime();
- if (age.toMilliSeconds() < getProperty<int>("PublishTimeout").getValue())
+ sendTask = new SimplePeriodicTask<>(
+ [&]
{
ARMARX_TRACE;
- ARMARX_CHECK_NOT_NULL(topicPrx) << "Topic proxy must not be null.";
- topicPrx->reportGamepadState(deviceName, js.name, data, dataTimestamp);
- }
- else
- {
+ std::unique_lock lock(mutex);
+ if (!js.opened())
+ {
+ return;
+ }
ARMARX_TRACE;
- ARMARX_INFO << deactivateSpam(100000, std::to_string(dataTimestamp->getTimestamp())) << "No new signal from gamepad for " << age.toMilliSecondsDouble() << " milliseconds. Not sending data. Timeout: " << getProperty<int>("PublishTimeout").getValue() << " ms";
- }
- }, 30);
-
+ if (!dataTimestamp)
+ {
+ ARMARX_INFO << deactivateSpam(1) << "dataTimestamp is null, waiting for value";
+ return;
+ }
+ ARMARX_CHECK_NOT_NULL(dataTimestamp);
+ const IceUtil::Time age = IceUtil::Time::now() - dataTimestamp->toTime();
+ if (age.toMilliSeconds() < getProperty<int>("PublishTimeout").getValue())
+ {
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(topicPrx) << "Topic proxy must not be null.";
+ topicPrx->reportGamepadState(deviceName, js.name, data, dataTimestamp);
+ }
+ else
+ {
+ ARMARX_TRACE;
+ ARMARX_INFO << deactivateSpam(100000, std::to_string(dataTimestamp->getTimestamp()))
+ << "No new signal from gamepad for " << age.toMilliSecondsDouble()
+ << " milliseconds. Not sending data. Timeout: "
+ << getProperty<int>("PublishTimeout").getValue() << " ms";
+ }
+ },
+ 30);
+
sendTask->start();
ARMARX_TRACE;
openGamepadConnection();
}
-void GamepadUnit::vibrate(const ::Ice::Current&)
+void
+GamepadUnit::vibrate(const ::Ice::Current&)
{
ARMARX_INFO << "vibration!";
js.executeEffect();
}
-bool GamepadUnit::openGamepadConnection()
+bool
+GamepadUnit::openGamepadConnection()
{
if (js.open(deviceName, deviceEventName))
{
ARMARX_TRACE;
- ARMARX_INFO << "opened a gamepad named " << js.name << " with " << js.numberOfAxis << " axis and " << js.numberOfButtons << " buttons.";
+ ARMARX_INFO << "opened a gamepad named " << js.name << " with " << js.numberOfAxis
+ << " axis and " << js.numberOfButtons << " buttons.";
if (js.numberOfAxis == 8 && js.numberOfButtons == 11)
{
ARMARX_TRACE;
@@ -113,7 +125,8 @@ bool GamepadUnit::openGamepadConnection()
return true;
}
-void GamepadUnit::run()
+void
+GamepadUnit::run()
{
ARMARX_TRACE;
while (readTask->isRunning())
@@ -162,14 +175,16 @@ void GamepadUnit::run()
data.rightStickButton = js.buttonsPressed[10];
ARMARX_TRACE;
- ARMARX_VERBOSE << "left x (integer): " << js.axis[0] << " left x (float): " << data.leftStickX << " right trigger: " << data.rightTrigger;
+ ARMARX_VERBOSE << "left x (integer): " << js.axis[0]
+ << " left x (float): " << data.leftStickX
+ << " right trigger: " << data.rightTrigger;
//usleep(1000); // 10ms
}
}
-
-void GamepadUnit::onDisconnectComponent()
+void
+GamepadUnit::onDisconnectComponent()
{
ARMARX_TRACE;
if (sendTask)
@@ -184,14 +199,14 @@ void GamepadUnit::onDisconnectComponent()
}
}
-
-void GamepadUnit::onExitComponent()
+void
+GamepadUnit::onExitComponent()
{
-
}
-armarx::PropertyDefinitionsPtr GamepadUnit::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+GamepadUnit::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new GamepadUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new GamepadUnitPropertyDefinitions(getConfigIdentifier()));
}
diff --git a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
index a2008600dc1f50432bbbf3f00c646153c7d7e83e..1c6d6cf279804e2acddb412d1f8a4b9f8390d20e 100644
--- a/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
+++ b/source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h
@@ -22,19 +22,18 @@
#pragma once
-#include<linux/joystick.h>
-#include<sys/stat.h>
-#include<fcntl.h>
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <fcntl.h>
+#include <sys/stat.h>
#include <ArmarXCore/core/Component.h>
-
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <RobotAPI/interface/units/GamepadUnit.h>
#include "Joystick.h"
+#include <linux/joystick.h>
namespace armarx
{
@@ -42,19 +41,28 @@ namespace armarx
* @class GamepadUnitPropertyDefinitions
* @brief
*/
- class GamepadUnitPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class GamepadUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- GamepadUnitPropertyDefinitions(std::string prefix):
+ GamepadUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
- defineOptionalProperty<std::string>("GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
- defineOptionalProperty<std::string>("GamepadDeviceName", "/dev/input/js2", "device that will be opened as a gamepad");
- defineOptionalProperty<std::string>("GamepadForceFeedbackName", "", "device that will be used for force feedback, leave empty to disable. See RobotAPI/source/RobotAPI/drivers/GamepadUnit/README.md for more details.");
- defineOptionalProperty<int>("PublishTimeout", 2000, "In Milliseconds. Timeout after which the gamepad data is not published after, if no new data was read from the gamepad");
+ defineOptionalProperty<std::string>(
+ "GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
+ defineOptionalProperty<std::string>(
+ "GamepadDeviceName", "/dev/input/js2", "device that will be opened as a gamepad");
+ defineOptionalProperty<std::string>(
+ "GamepadForceFeedbackName",
+ "",
+ "device that will be used for force feedback, leave empty to disable. See "
+ "RobotAPI/source/RobotAPI/drivers/GamepadUnit/README.md for more details.");
+ defineOptionalProperty<int>(
+ "PublishTimeout",
+ 2000,
+ "In Milliseconds. Timeout after which the gamepad data is not published after, if "
+ "no new data was read from the gamepad");
}
};
@@ -69,15 +77,14 @@ namespace armarx
*
* Detailed description of class GamepadUnit.
*/
- class GamepadUnit :
- virtual public armarx::Component,
- virtual public GamepadUnitInterface
+ class GamepadUnit : virtual public armarx::Component, virtual public GamepadUnitInterface
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "GamepadUnit";
}
@@ -125,4 +132,4 @@ namespace armarx
GamepadData data;
TimestampVariantPtr dataTimestamp;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/GamepadUnit/Joystick.h b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
index af7cc386dc35a8b1b8ec3122545fd0bb88fef4bd..27a1c747a0713d5bfe2c8b9c37214b92cae28f0a 100644
--- a/source/RobotAPI/drivers/GamepadUnit/Joystick.h
+++ b/source/RobotAPI/drivers/GamepadUnit/Joystick.h
@@ -63,7 +63,9 @@ namespace armarx
fdEvent = ::open(deviceEventName.c_str(), O_RDWR | O_CLOEXEC);
ARMARX_CHECK(fdEvent != -1);
- } else {
+ }
+ else
+ {
ARMARX_INFO << "Force feedback disabled";
}
@@ -122,7 +124,8 @@ namespace armarx
executeEffect(int gain = 100, const int nTimes = 1)
{
// this feature is disabled
- if(fdEvent < 0) return;
+ if (fdEvent < 0)
+ return;
// see https://docs.kernel.org/input/ff.html
@@ -194,7 +197,7 @@ namespace armarx
for (int i = 0; i < 5; i++)
{
-
+
input_event statusIe; // structure used to communicate with the driver
statusIe.type = EV_FF_STATUS;
statusIe.code = e.id;
@@ -220,7 +223,8 @@ namespace armarx
stop.type = EV_FF;
stop.code = e.id;
stop.value = 0;
- [[maybe_unused]] const int stopStatus = write(fdEvent, static_cast<const void*>(&stop), sizeof(stop));
+ [[maybe_unused]] const int stopStatus =
+ write(fdEvent, static_cast<const void*>(&stop), sizeof(stop));
ret = ioctl(fdEvent, EVIOCRMFF, e.id);
diff --git a/source/RobotAPI/drivers/GamepadUnit/test/GamepadUnitTest.cpp b/source/RobotAPI/drivers/GamepadUnit/test/GamepadUnitTest.cpp
index 488590d87748de3a5a0f2a548ac5a00c379d9d03..4af36becd977249d0b7d436907f36e065498e1da 100644
--- a/source/RobotAPI/drivers/GamepadUnit/test/GamepadUnitTest.cpp
+++ b/source/RobotAPI/drivers/GamepadUnit/test/GamepadUnitTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/drivers/GamepadUnit/GamepadUnit.h>
#include <iostream>
+#include <RobotAPI/drivers/GamepadUnit/GamepadUnit.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::GamepadUnit instance;
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index a89af6168a5f66f366a4df3a36b59c01906a0695..6f43269fe8bf347c6e1dc2335e70a715abb482b8 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -39,7 +39,6 @@ HokuyoLaserUnit::HokuyoLaserUnit()
addPlugin(heartbeat);
}
-
void
HokuyoLaserUnit::onInitComponent()
{
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
index 98da02fce21736694803272e4e76e046080d2cd4..d7e55105ca70d986640c47c768fefdf5c1946b5f 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -46,8 +46,9 @@ namespace armarx
HokuyoLaserUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>(
- "LaserScannerListenerName", "CartographerMappingAndLocalization", "Name of the laser scan listener.");
+ defineOptionalProperty<std::string>("LaserScannerListenerName",
+ "CartographerMappingAndLocalization",
+ "Name of the laser scan listener.");
defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans");
defineOptionalProperty<float>("AngleOffset",
-2.3561944902,
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp
index 581618323d926db09e1559e695df1345ed865190..46d49defb32fb0f5630feab78a0a88c67c0a11a9 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/test/HokuyoLaserUnitTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h>
#include <iostream>
+#include <RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::HokuyoLaserUnit instance;
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.cpp b/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.cpp
index 25b5f9972efd5decb236f520eca28920ac741d19..92fa7441dd460f4c2e68aa4bdcec7d78cb2cdb2e 100644
--- a/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.cpp
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.cpp
@@ -1,24 +1,30 @@
#include "KITProsthesisIceDriver.h"
+
#include <iostream>
#include <memory>
using namespace KITProsthesis;
-void KITProsthesisIceDriver::sendGrasp(Ice::Int n, const Ice::Current&)
+void
+KITProsthesisIceDriver::sendGrasp(Ice::Int n, const Ice::Current&)
{
iface.sendGrasp(n);
}
-void KITProsthesisIceDriver::sendThumbPWM(const ProsthesisMotorValues& motorValues, const Ice::Current&)
+
+void
+KITProsthesisIceDriver::sendThumbPWM(const ProsthesisMotorValues& motorValues, const Ice::Current&)
{
iface.sendThumbPWM(motorValues.v, motorValues.maxPWM, motorValues.pos);
}
-void KITProsthesisIceDriver::sendFingerPWM(const ProsthesisMotorValues& motorValues, const Ice::Current&)
+void
+KITProsthesisIceDriver::sendFingerPWM(const ProsthesisMotorValues& motorValues, const Ice::Current&)
{
iface.sendFingerPWM(motorValues.v, motorValues.maxPWM, motorValues.pos);
}
-ProsthesisSensorValues KITProsthesisIceDriver::getSensorValues(const Ice::Current&)
+ProsthesisSensorValues
+KITProsthesisIceDriver::getSensorValues(const Ice::Current&)
{
ProsthesisSensorValues value;
switch (iface.getState())
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.h b/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.h
index 3d8ec5dc028ef543d3f2aa8212c2f5620ab8c512..7310fcb9a6f3ab001ba144c372d0b71bc881b4d7 100644
--- a/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.h
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.h
@@ -22,31 +22,36 @@
#pragma once
-#include <RobotAPI/interface/units/KITProstheticHandInterface.h>
-#include <RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h>
-#include <Ice/Ice.h>
-#include <Ice/Object.h>
-#include <QCoreApplication>
#include <iostream>
#include <thread>
+#include <QCoreApplication>
+
+#include <Ice/Ice.h>
+#include <Ice/Object.h>
+
+#include <RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h>
+#include <RobotAPI/interface/units/KITProstheticHandInterface.h>
+
[[maybe_unused]] static constexpr auto prosthesis = "CB:43:34:8F:3C:0A";
[[maybe_unused]] static constexpr auto prosthesis_old = "DF:70:E8:81:DB:D6";
-class KITProsthesisIceDriver:
- virtual public KITProsthesis::KITProstheticHandInterface
+class KITProsthesisIceDriver : virtual public KITProsthesis::KITProstheticHandInterface
{
public:
KITProsthesisIceDriver(const std::string& mac = prosthesis_old) : iface{mac}
- {}
+ {
+ }
// sender interface
void sendGrasp(Ice::Int, const Ice::Current& = Ice::emptyCurrent) override;
- void sendThumbPWM(const KITProsthesis::ProsthesisMotorValues&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
- void sendFingerPWM(const KITProsthesis::ProsthesisMotorValues&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
+ void sendThumbPWM(const KITProsthesis::ProsthesisMotorValues&,
+ const ::Ice::Current& = ::Ice::emptyCurrent) override;
+ void sendFingerPWM(const KITProsthesis::ProsthesisMotorValues&,
+ const ::Ice::Current& = ::Ice::emptyCurrent) override;
KITProsthesis::ProsthesisSensorValues getSensorValues(const Ice::Current&) override;
+
private:
BLEProthesisInterface iface;
-
};
diff --git a/source/RobotAPI/drivers/KITProsthesisIceDriver/example/KITProsthesisIceDriverExample.cpp b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/KITProsthesisIceDriverExample.cpp
index 10928b88eb88b13a09239e5cda8226c2245c8d8b..963822d18bf0a6a966e866a39fe16bad6c08ac77 100644
--- a/source/RobotAPI/drivers/KITProsthesisIceDriver/example/KITProsthesisIceDriverExample.cpp
+++ b/source/RobotAPI/drivers/KITProsthesisIceDriver/example/KITProsthesisIceDriverExample.cpp
@@ -1,9 +1,14 @@
+#include <thread>
+
+#include <QCoreApplication>
+
#include <Ice/Ice.h>
+
#include <RobotAPI/drivers/KITProsthesisIceDriver/KITProsthesisIceDriver.h>
#include <RobotAPI/interface/units/KITProstheticHandInterface.h>
-#include <QCoreApplication>
-#include <thread>
-int main(int argc, char* argv[])
+
+int
+main(int argc, char* argv[])
{
for (int i = 0; i < argc; ++i)
{
@@ -19,7 +24,8 @@ int main(int argc, char* argv[])
try
{
Ice::CommunicatorHolder iceServer(argc, argv);
- Ice::ObjectAdapterPtr adapter = iceServer->createObjectAdapterWithEndpoints("KITProsthesisControlAdapter", "default -h localhost -p 10000");
+ Ice::ObjectAdapterPtr adapter = iceServer->createObjectAdapterWithEndpoints(
+ "KITProsthesisControlAdapter", "default -h localhost -p 10000");
KITProsthesis::KITProstheticHandInterfacePtr object = new KITProsthesisIceDriver;
Ice::ObjectPrx prx = adapter->add(object, Ice::stringToIdentity("KITProsthesisControl"));
std::cout << prx->ice_toString() << std::endl;
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
index fab724f4b1cc29732eed10bfa1c291237284c8ec..7f66db44b75b5befe302d713992e2ba4f1814726 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.cpp
@@ -1,11 +1,11 @@
+#include "BLEProthesisInterface.h"
+
#include <sstream>
-#include "BLEProthesisInterface.h"
#include "BLEProthesisInterfaceQtWorkerThread.h"
BLEProthesisInterface::BLEProthesisInterface(const std::string& mac, SensorValueProtocol protocol) :
- _worker{new BLEProthesisInterfaceQtWorkerThread{mac, *this}},
- _protocol{protocol}
+ _worker{new BLEProthesisInterfaceQtWorkerThread{mac, *this}}, _protocol{protocol}
{
_worker->start();
}
@@ -20,148 +20,130 @@ BLEProthesisInterface::~BLEProthesisInterface()
_worker->wait();
}
-std::int64_t BLEProthesisInterface::getThumbPWM() const
+std::int64_t
+BLEProthesisInterface::getThumbPWM() const
{
return _thumbPWM;
}
-std::int64_t BLEProthesisInterface::getThumbPos() const
+std::int64_t
+BLEProthesisInterface::getThumbPos() const
{
return _thumbPos;
}
-std::int64_t BLEProthesisInterface::getFingerPWM() const
+std::int64_t
+BLEProthesisInterface::getFingerPWM() const
{
return _fingerPWM;
}
-std::int64_t BLEProthesisInterface::getFingerPos() const
+std::int64_t
+BLEProthesisInterface::getFingerPos() const
{
return _fingerPos;
}
-BLEProthesisInterface::State BLEProthesisInterface::getState() const
+BLEProthesisInterface::State
+BLEProthesisInterface::getState() const
{
return _state;
}
-void BLEProthesisInterface::sendRaw(const std::string& cmd)
+void
+BLEProthesisInterface::sendRaw(const std::string& cmd)
{
if (_state != State::Running)
{
- throw std::invalid_argument
- {
- "BLEProthesisInterface::sendRaw( cmd = " + cmd +
- " ): The UART service is not connected!"
- };
+ throw std::invalid_argument{"BLEProthesisInterface::sendRaw( cmd = " + cmd +
+ " ): The UART service is not connected!"};
}
_worker->sendCommand(cmd);
}
-void BLEProthesisInterface::sendGrasp(std::uint64_t n)
+void
+BLEProthesisInterface::sendGrasp(std::uint64_t n)
{
if (n > getMaxG())
{
- throw std::invalid_argument
- {
- "BLEProthesisInterface::sendGrasp( n = " + std::to_string(n) +
- " ): the maximal value for n is " + std::to_string(getMaxG())
- };
+ throw std::invalid_argument{"BLEProthesisInterface::sendGrasp( n = " + std::to_string(n) +
+ " ): the maximal value for n is " + std::to_string(getMaxG())};
}
sendRaw("g" + std::to_string(n) + "\n");
}
-void BLEProthesisInterface::sendThumbPWM(uint64_t v, uint64_t mxPWM, uint64_t pos)
+void
+BLEProthesisInterface::sendThumbPWM(uint64_t v, uint64_t mxPWM, uint64_t pos)
{
if (v < getMinV() || v > getMaxV())
{
- throw std::invalid_argument
- {
- "sendThumbPWM( v = " + std::to_string(v) +
- " , mxPWM = " + std::to_string(mxPWM) +
- " , pos = " + std::to_string(pos) +
- " ): The interval for v is [" + std::to_string(getMinV()) +
- ", " + std::to_string(getMaxV()) + "]"
- };
+ throw std::invalid_argument{
+ "sendThumbPWM( v = " + std::to_string(v) + " , mxPWM = " + std::to_string(mxPWM) +
+ " , pos = " + std::to_string(pos) + " ): The interval for v is [" +
+ std::to_string(getMinV()) + ", " + std::to_string(getMaxV()) + "]"};
}
if (mxPWM > getMaxPWM())
{
- throw std::invalid_argument
- {
- "sendThumbPWM( v = " + std::to_string(v) +
- " , mxPWM = " + std::to_string(mxPWM) +
- " , pos = " + std::to_string(pos) +
- " ): The interval for mxPWM is [0, , " +
- std::to_string(getMaxPWM()) + "]"
- };
+ throw std::invalid_argument{
+ "sendThumbPWM( v = " + std::to_string(v) + " , mxPWM = " + std::to_string(mxPWM) +
+ " , pos = " + std::to_string(pos) + " ): The interval for mxPWM is [0, , " +
+ std::to_string(getMaxPWM()) + "]"};
}
if (pos > getMaxPosThumb())
{
- throw std::invalid_argument
- {
- "sendThumbPWM( v = " + std::to_string(v) +
- " , mxPWM = " + std::to_string(mxPWM) +
- " , pos = " + std::to_string(pos) +
- " ): The interval for pos is [0, " +
- std::to_string(getMaxPosThumb()) + "]"
- };
+ throw std::invalid_argument{
+ "sendThumbPWM( v = " + std::to_string(v) + " , mxPWM = " + std::to_string(mxPWM) +
+ " , pos = " + std::to_string(pos) + " ): The interval for pos is [0, " +
+ std::to_string(getMaxPosThumb()) + "]"};
}
std::stringstream str;
str << 'M' << 2 << ',' << v << ',' << mxPWM << ',' << pos << '\n';
sendRaw(str.str());
}
-void BLEProthesisInterface::sendFingerPWM(uint64_t v, uint64_t mxPWM, uint64_t pos)
+void
+BLEProthesisInterface::sendFingerPWM(uint64_t v, uint64_t mxPWM, uint64_t pos)
{
if (v < getMinV() || v > getMaxV())
{
- throw std::invalid_argument
- {
- "sendThumbPWM( v = " + std::to_string(v) +
- " , mxPWM = " + std::to_string(mxPWM) +
- " , pos = " + std::to_string(pos) +
- " ): The interval for v is [" + std::to_string(getMinV()) +
- ", " + std::to_string(getMaxV()) + "]"
- };
+ throw std::invalid_argument{
+ "sendThumbPWM( v = " + std::to_string(v) + " , mxPWM = " + std::to_string(mxPWM) +
+ " , pos = " + std::to_string(pos) + " ): The interval for v is [" +
+ std::to_string(getMinV()) + ", " + std::to_string(getMaxV()) + "]"};
}
if (mxPWM > getMaxPWM())
{
- throw std::invalid_argument
- {
- "sendThumbPWM( v = " + std::to_string(v) +
- " , mxPWM = " + std::to_string(mxPWM) +
- " , pos = " + std::to_string(pos) +
- " ): The interval for mxPWM is [0, , " +
- std::to_string(getMaxPWM()) + "]"
- };
+ throw std::invalid_argument{
+ "sendThumbPWM( v = " + std::to_string(v) + " , mxPWM = " + std::to_string(mxPWM) +
+ " , pos = " + std::to_string(pos) + " ): The interval for mxPWM is [0, , " +
+ std::to_string(getMaxPWM()) + "]"};
}
if (pos > getMaxPosFingers())
{
- throw std::invalid_argument
- {
- "sendThumbPWM( v = " + std::to_string(v) +
- " , mxPWM = " + std::to_string(mxPWM) +
- " , pos = " + std::to_string(pos) +
- " ): The interval for pos is [0, " +
- std::to_string(getMaxPosFingers()) + "]"
- };
+ throw std::invalid_argument{
+ "sendThumbPWM( v = " + std::to_string(v) + " , mxPWM = " + std::to_string(mxPWM) +
+ " , pos = " + std::to_string(pos) + " ): The interval for pos is [0, " +
+ std::to_string(getMaxPosFingers()) + "]"};
}
std::stringstream str;
str << 'M' << 3 << ',' << v << ',' << mxPWM << ',' << pos << '\n';
sendRaw(str.str());
}
-void BLEProthesisInterface::verboseReceive(bool b)
+void
+BLEProthesisInterface::verboseReceive(bool b)
{
_verboseReceive = b;
}
-void BLEProthesisInterface::verboseSend(bool b)
+void
+BLEProthesisInterface::verboseSend(bool b)
{
_verboseSend = b;
}
-std::string to_string(BLEProthesisInterface::State s)
+std::string
+to_string(BLEProthesisInterface::State s)
{
switch (s)
{
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
index 51797fe9b5b1e2ee3300b53221ab43a8c9675052..c958e9d3c63a9eaf78fa5614aa16a6b969d563a5 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h
@@ -1,8 +1,8 @@
#pragma once
#include <atomic>
-#include <string>
#include <memory>
+#include <string>
class BLEProthesisInterfaceQtWorkerThread;
class BLEProthesisInterfaceQtWorker;
@@ -29,8 +29,10 @@ public:
tpos_tpwm_fpos_fpwm,
mx_pos_pwm
};
+
public:
- BLEProthesisInterface(const std::string& mac, SensorValueProtocol protocol = SensorValueProtocol::mx_pos_pwm);
+ BLEProthesisInterface(const std::string& mac,
+ SensorValueProtocol protocol = SensorValueProtocol::mx_pos_pwm);
~BLEProthesisInterface();
std::int64_t getThumbPWM() const;
@@ -48,6 +50,7 @@ public:
void verboseReceive(bool b = true);
void verboseSend(bool b = true);
+
private:
friend class BLEProthesisInterfaceQtWorker;
//sensor values
@@ -62,28 +65,40 @@ private:
std::atomic_bool _verboseReceive{false};
std::atomic_bool _verboseSend{true};
SensorValueProtocol _protocol;
+
public:
- static constexpr std::uint64_t getMaxG()
+ static constexpr std::uint64_t
+ getMaxG()
{
return 8;
}
- static constexpr std::uint64_t getMinV()
+
+ static constexpr std::uint64_t
+ getMinV()
{
return 10;
}
- static constexpr std::uint64_t getMaxV()
+
+ static constexpr std::uint64_t
+ getMaxV()
{
return 200;
}
- static constexpr std::uint64_t getMaxPWM()
+
+ static constexpr std::uint64_t
+ getMaxPWM()
{
- return 2999;
+ return 2999;
}
- static constexpr std::uint64_t getMaxPosThumb()
+
+ static constexpr std::uint64_t
+ getMaxPosThumb()
{
return 100'000;
}
- static constexpr std::uint64_t getMaxPosFingers()
+
+ static constexpr std::uint64_t
+ getMaxPosFingers()
{
return 200'000;
}
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
index 0f5294d5f9edaf4b67834487a0fa62f1f044378a..78a51406a06a79fc70de153ef74ad0bb9c7b028e 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.cpp
@@ -6,9 +6,9 @@
#define RXUUID "6e400002-b5a3-f393-e0a9-e50e24dcca9e"
#define TXUUID "6e400003-b5a3-f393-e0a9-e50e24dcca9e"
-BLEProthesisInterfaceQtWorker::BLEProthesisInterfaceQtWorker(const QString& mac, BLEProthesisInterface& owner) :
- _owner{&owner},
- _mac{mac}
+BLEProthesisInterfaceQtWorker::BLEProthesisInterfaceQtWorker(const QString& mac,
+ BLEProthesisInterface& owner) :
+ _owner{&owner}, _mac{mac}
{
_timer = startTimer(std::chrono::milliseconds{10});
}
@@ -18,18 +18,21 @@ BLEProthesisInterfaceQtWorker::~BLEProthesisInterfaceQtWorker()
cleanup();
}
-void BLEProthesisInterfaceQtWorker::kill()
+void
+BLEProthesisInterfaceQtWorker::kill()
{
_killed = true;
}
-void BLEProthesisInterfaceQtWorker::sendCommand(const std::string& cmd)
+void
+BLEProthesisInterfaceQtWorker::sendCommand(const std::string& cmd)
{
std::lock_guard guard(_cmdMutex);
_cmd += QString::fromStdString(cmd);
}
-void BLEProthesisInterfaceQtWorker::cleanup()
+void
+BLEProthesisInterfaceQtWorker::cleanup()
{
//stop services etc
if (_timer != -1)
@@ -61,7 +64,8 @@ void BLEProthesisInterfaceQtWorker::cleanup()
}
}
-void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
+void
+BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
{
if (_killed)
{
@@ -74,10 +78,14 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
if (_owner->_state == State::Created)
{
_deviceDiscoveryAgent = new QBluetoothDeviceDiscoveryAgent;
- connect(_deviceDiscoveryAgent, SIGNAL(deviceDiscovered(const QBluetoothDeviceInfo&)),
- this, SLOT(deviceDiscovered(const QBluetoothDeviceInfo&)));
- connect(_deviceDiscoveryAgent, SIGNAL(error(QBluetoothDeviceDiscoveryAgent::Error)),
- this, SLOT(deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error)));
+ connect(_deviceDiscoveryAgent,
+ SIGNAL(deviceDiscovered(const QBluetoothDeviceInfo&)),
+ this,
+ SLOT(deviceDiscovered(const QBluetoothDeviceInfo&)));
+ connect(_deviceDiscoveryAgent,
+ SIGNAL(error(QBluetoothDeviceDiscoveryAgent::Error)),
+ this,
+ SLOT(deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error)));
connect(_deviceDiscoveryAgent, SIGNAL(finished()), this, SLOT(deviceDiscoverFinished()));
connect(_deviceDiscoveryAgent, SIGNAL(canceled()), this, SLOT(deviceDiscoverFinished()));
qDebug() << '[' << _mac << ']' << " State DiscoveringDevices";
@@ -109,18 +117,19 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
}
_serviceDiscovered = false;
_control = new QLowEnergyController(_currentDevice);
- _control ->setRemoteAddressType(QLowEnergyController::RandomAddress);
-
- connect(_control, SIGNAL(serviceDiscovered(QBluetoothUuid)),
- this, SLOT(serviceDiscovered(QBluetoothUuid)));
- connect(_control, SIGNAL(discoveryFinished()),
- this, SLOT(serviceScanDone()));
- connect(_control, SIGNAL(error(QLowEnergyController::Error)),
- this, SLOT(controllerError(QLowEnergyController::Error)));
- connect(_control, SIGNAL(connected()),
- this, SLOT(deviceConnected()));
- connect(_control, SIGNAL(disconnected()),
- this, SLOT(deviceDisconnected()));
+ _control->setRemoteAddressType(QLowEnergyController::RandomAddress);
+
+ connect(_control,
+ SIGNAL(serviceDiscovered(QBluetoothUuid)),
+ this,
+ SLOT(serviceDiscovered(QBluetoothUuid)));
+ connect(_control, SIGNAL(discoveryFinished()), this, SLOT(serviceScanDone()));
+ connect(_control,
+ SIGNAL(error(QLowEnergyController::Error)),
+ this,
+ SLOT(controllerError(QLowEnergyController::Error)));
+ connect(_control, SIGNAL(connected()), this, SLOT(deviceConnected()));
+ connect(_control, SIGNAL(disconnected()), this, SLOT(deviceDisconnected()));
_control->connectToDevice();
qDebug() << '[' << _mac << ']' << " State Connecting";
_owner->_state = State::Connecting;
@@ -141,12 +150,18 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
}
_service = _control->createServiceObject(QBluetoothUuid(QUuid(UARTSERVICEUUID)));
- connect(_service, SIGNAL(stateChanged(QLowEnergyService::ServiceState)),
- this, SLOT(serviceStateChanged(QLowEnergyService::ServiceState)));
- connect(_service, SIGNAL(characteristicChanged(QLowEnergyCharacteristic, QByteArray)),
- this, SLOT(readData(QLowEnergyCharacteristic, QByteArray)));
- connect(_service, SIGNAL(descriptorWritten(QLowEnergyDescriptor, QByteArray)),
- this, SLOT(receiveDeviceDisconnec(QLowEnergyDescriptor, QByteArray)));
+ connect(_service,
+ SIGNAL(stateChanged(QLowEnergyService::ServiceState)),
+ this,
+ SLOT(serviceStateChanged(QLowEnergyService::ServiceState)));
+ connect(_service,
+ SIGNAL(characteristicChanged(QLowEnergyCharacteristic, QByteArray)),
+ this,
+ SLOT(readData(QLowEnergyCharacteristic, QByteArray)));
+ connect(_service,
+ SIGNAL(descriptorWritten(QLowEnergyDescriptor, QByteArray)),
+ this,
+ SLOT(receiveDeviceDisconnec(QLowEnergyDescriptor, QByteArray)));
_service->discoverDetails();
@@ -158,7 +173,8 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
std::lock_guard g(_cmdMutex);
if (_service && _cmd.size() != 0)
{
- const QLowEnergyCharacteristic RxChar = _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
+ const QLowEnergyCharacteristic RxChar =
+ _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
QByteArray data;
data.append(_cmd);
_service->writeCharacteristic(RxChar, data, QLowEnergyService::WriteWithoutResponse);
@@ -171,11 +187,13 @@ void BLEProthesisInterfaceQtWorker::timerEvent(QTimerEvent*)
}
}
-void BLEProthesisInterfaceQtWorker::deviceDiscovered(const QBluetoothDeviceInfo& device)
+void
+BLEProthesisInterfaceQtWorker::deviceDiscovered(const QBluetoothDeviceInfo& device)
{
if (device.address().toString() == _mac)
{
- qDebug() << '[' << _mac << ']' << " Discovered target device " << device.address().toString();
+ qDebug() << '[' << _mac << ']' << " Discovered target device "
+ << device.address().toString();
_currentDevice = device;
_deviceDiscovered = true;
qDebug() << '[' << _mac << ']' << " State DiscoveringDevicesDone";
@@ -188,21 +206,25 @@ void BLEProthesisInterfaceQtWorker::deviceDiscovered(const QBluetoothDeviceInfo&
}
}
-void BLEProthesisInterfaceQtWorker::deviceDiscoverFinished()
+void
+BLEProthesisInterfaceQtWorker::deviceDiscoverFinished()
{
qDebug() << '[' << _mac << ']' << " Discovering of services done.";
_owner->_state = State::DiscoveringDevicesDone;
}
-void BLEProthesisInterfaceQtWorker::deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error error)
+void
+BLEProthesisInterfaceQtWorker::deviceDiscoverError(QBluetoothDeviceDiscoveryAgent::Error error)
{
if (error == QBluetoothDeviceDiscoveryAgent::PoweredOffError)
{
- qDebug() << '[' << _mac << ']' << "The Bluetooth adaptor is powered off, power it on before doing discovery.";
+ qDebug() << '[' << _mac << ']'
+ << "The Bluetooth adaptor is powered off, power it on before doing discovery.";
}
else if (error == QBluetoothDeviceDiscoveryAgent::InputOutputError)
{
- qDebug() << '[' << _mac << ']' << "Writing or reading from the device resulted in an error.";
+ qDebug() << '[' << _mac << ']'
+ << "Writing or reading from the device resulted in an error.";
}
else
{
@@ -211,7 +233,8 @@ void BLEProthesisInterfaceQtWorker::deviceDiscoverError(QBluetoothDeviceDiscover
kill();
}
-void BLEProthesisInterfaceQtWorker::serviceDiscovered(const QBluetoothUuid& gatt)
+void
+BLEProthesisInterfaceQtWorker::serviceDiscovered(const QBluetoothUuid& gatt)
{
qDebug() << '[' << _mac << ']' << " Discovered service " << gatt.toString();
if (gatt == QBluetoothUuid(QUuid(UARTSERVICEUUID)))
@@ -222,20 +245,24 @@ void BLEProthesisInterfaceQtWorker::serviceDiscovered(const QBluetoothUuid& gatt
}
}
-void BLEProthesisInterfaceQtWorker::serviceDiscoverFinished()
+void
+BLEProthesisInterfaceQtWorker::serviceDiscoverFinished()
{
qDebug() << '[' << _mac << ']' << " State DiscoveringServicesDone";
_owner->_state = State::DiscoveringServicesDone;
}
-void BLEProthesisInterfaceQtWorker::controllerError(QLowEnergyController::Error error)
+void
+BLEProthesisInterfaceQtWorker::controllerError(QLowEnergyController::Error error)
{
qDebug() << '[' << _mac << ']' << " Cannot connect to remote device.";
qWarning() << '[' << _mac << ']' << " Controller Error:" << error;
kill();
}
-void BLEProthesisInterfaceQtWorker::receiveDeviceDisconnec(const QLowEnergyDescriptor& d, const QByteArray& value)
+void
+BLEProthesisInterfaceQtWorker::receiveDeviceDisconnec(const QLowEnergyDescriptor& d,
+ const QByteArray& value)
{
if (d.isValid() && d == _notificationDescTx && value == QByteArray("0000"))
{
@@ -250,7 +277,8 @@ void BLEProthesisInterfaceQtWorker::receiveDeviceDisconnec(const QLowEnergyDescr
}
}
-void BLEProthesisInterfaceQtWorker::serviceStateChanged(QLowEnergyService::ServiceState s)
+void
+BLEProthesisInterfaceQtWorker::serviceStateChanged(QLowEnergyService::ServiceState s)
{
// A descriptoc can only be written if the service is in the ServiceDiscovered state
qDebug() << '[' << _mac << ']' << " serviceStateChanged -> " << s;
@@ -260,7 +288,8 @@ void BLEProthesisInterfaceQtWorker::serviceStateChanged(QLowEnergyService::Servi
{
//looking for the TX characteristic
- const QLowEnergyCharacteristic TxChar = _service->characteristic(QBluetoothUuid(QUuid(TXUUID)));
+ const QLowEnergyCharacteristic TxChar =
+ _service->characteristic(QBluetoothUuid(QUuid(TXUUID)));
if (!TxChar.isValid())
{
qDebug() << '[' << _mac << ']' << " Tx characteristic not found";
@@ -268,7 +297,8 @@ void BLEProthesisInterfaceQtWorker::serviceStateChanged(QLowEnergyService::Servi
}
//looking for the RX characteristic
- const QLowEnergyCharacteristic RxChar = _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
+ const QLowEnergyCharacteristic RxChar =
+ _service->characteristic(QBluetoothUuid(QUuid(RXUUID)));
if (!RxChar.isValid())
{
qDebug() << '[' << _mac << ']' << " Rx characteristic not found";
@@ -278,7 +308,8 @@ void BLEProthesisInterfaceQtWorker::serviceStateChanged(QLowEnergyService::Servi
// Bluetooth LE spec Where a characteristic can be notified, a Client Characteristic Configuration descriptor
// shall be included in that characteristic as required by the Bluetooth Core Specification
// Tx notify is enabled
- _notificationDescTx = TxChar.descriptor(QBluetoothUuid::ClientCharacteristicConfiguration);
+ _notificationDescTx =
+ TxChar.descriptor(QBluetoothUuid::ClientCharacteristicConfiguration);
if (_notificationDescTx.isValid())
{
// enable notification
@@ -293,27 +324,32 @@ void BLEProthesisInterfaceQtWorker::serviceStateChanged(QLowEnergyService::Servi
}
}
-void BLEProthesisInterfaceQtWorker::deviceConnected()
+void
+BLEProthesisInterfaceQtWorker::deviceConnected()
{
qDebug() << '[' << _mac << ']' << " State ConnectingDone";
_owner->_state = State::ConnectingDone;
}
-void BLEProthesisInterfaceQtWorker::deviceDisconnected()
+void
+BLEProthesisInterfaceQtWorker::deviceDisconnected()
{
qDebug() << '[' << _mac << ']' << " State Disconnected";
_owner->_state = State::Disconnected;
}
-
-template<>
-void BLEProthesisInterfaceQtWorker::consumeData<BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm>()
+template <>
+void
+BLEProthesisInterfaceQtWorker::consumeData<
+ BLEProthesisInterface::SensorValueProtocol::tpos_tpwm_fpos_fpwm>()
{
if (!_valueAkk.contains('\n'))
{
if (_owner->_verboseReceive)
{
- qDebug() << '[' << _mac << ']' << " data does not contain \\n -> no new sensor values\n Buffer:\n" << _valueAkk;
+ qDebug() << '[' << _mac << ']'
+ << " data does not contain \\n -> no new sensor values\n Buffer:\n"
+ << _valueAkk;
}
return;
}
@@ -333,23 +369,28 @@ void BLEProthesisInterfaceQtWorker::consumeData<BLEProthesisInterface::SensorVal
_valueAkk = listPacks.back();
}
-template<>
-void BLEProthesisInterfaceQtWorker::consumeData<BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm>()
+template <>
+void
+BLEProthesisInterfaceQtWorker::consumeData<BLEProthesisInterface::SensorValueProtocol::mx_pos_pwm>()
{
if (!_valueAkk.contains('\n'))
{
if (_owner->_verboseReceive)
{
- qDebug() << '[' << _mac << ']' << " data does not contain \\n -> no new sensor values\n Buffer:\n" << _valueAkk;
+ qDebug() << '[' << _mac << ']'
+ << " data does not contain \\n -> no new sensor values\n Buffer:\n"
+ << _valueAkk;
}
return;
}
auto listPacks = _valueAkk.split('\n');
- static const QRegularExpression m2(R"(^M2:[ \t]+Pos.:[ \t]+(-?[1-9][0-9]*|0)[ \t]+PWM:[ \t]+(-?[1-9][0-9]*|0)[ \t\n\r]+$)");
- static const QRegularExpression m3(R"(^M3:[ \t]+Pos.:[ \t]+(-?[1-9][0-9]*|0)[ \t]+PWM:[ \t]+(-?[1-9][0-9]*|0)[ \t\n\r]+$)");
+ static const QRegularExpression m2(
+ R"(^M2:[ \t]+Pos.:[ \t]+(-?[1-9][0-9]*|0)[ \t]+PWM:[ \t]+(-?[1-9][0-9]*|0)[ \t\n\r]+$)");
+ static const QRegularExpression m3(
+ R"(^M3:[ \t]+Pos.:[ \t]+(-?[1-9][0-9]*|0)[ \t]+PWM:[ \t]+(-?[1-9][0-9]*|0)[ \t\n\r]+$)");
- for (int i = 0; i < listPacks.size() - 1; ++i)
+ for (int i = 0; i < listPacks.size() - 1; ++i)
{
if (listPacks.at(i).size() == 0)
{
@@ -379,13 +420,15 @@ void BLEProthesisInterfaceQtWorker::consumeData<BLEProthesisInterface::SensorVal
}
else
{
- qWarning() << "unknown format for data: " << listPacks.at(i).toLocal8Bit() << "\nSkipping";
+ qWarning() << "unknown format for data: " << listPacks.at(i).toLocal8Bit()
+ << "\nSkipping";
}
}
_valueAkk = listPacks.back();
}
-void BLEProthesisInterfaceQtWorker::readData(const QLowEnergyCharacteristic& c, const QByteArray& value)
+void
+BLEProthesisInterfaceQtWorker::readData(const QLowEnergyCharacteristic& c, const QByteArray& value)
{
// ignore any other characteristic change
if (c.uuid() != QBluetoothUuid(QUuid(TXUUID)))
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h
index ce14c53a2c4e987e6c5cfb3a5e1faed21616b06e..9960e3bc49a91f0576696ab1af50ba4971dd61be 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorker.h
@@ -6,11 +6,9 @@
#include <QBluetoothDeviceInfo>
#include <QLowEnergyController>
#include <QLowEnergyService>
-
-#include "BLEProthesisInterface.h"
-
#include <QThread>
+#include "BLEProthesisInterface.h"
class BLEProthesisInterfaceQtWorker : public QThread
{
@@ -22,8 +20,10 @@ public:
void kill();
void sendCommand(const std::string& cmd);
+
private:
void cleanup();
+
protected:
void timerEvent(QTimerEvent* event) override;
signals:
@@ -44,28 +44,30 @@ private slots:
void serviceStateChanged(QLowEnergyService::ServiceState s);
void readData(const QLowEnergyCharacteristic& c, const QByteArray& value);
+
private:
- template<BLEProthesisInterface::SensorValueProtocol P> void consumeData();
+ template <BLEProthesisInterface::SensorValueProtocol P>
+ void consumeData();
- BLEProthesisInterface* _owner;
- QString _mac;
+ BLEProthesisInterface* _owner;
+ QString _mac;
//management (sensor/control)
- int _timer{-1};
- std::atomic_bool _killed{false};
+ int _timer{-1};
+ std::atomic_bool _killed{false};
- QString _valueAkk;
+ QString _valueAkk;
- QString _cmd;
- std::mutex _cmdMutex;
+ QString _cmd;
+ std::mutex _cmdMutex;
//ble discover
- QBluetoothDeviceDiscoveryAgent* _deviceDiscoveryAgent{nullptr};
- QBluetoothDeviceInfo _currentDevice;
- bool _deviceDiscovered{false};
+ QBluetoothDeviceDiscoveryAgent* _deviceDiscoveryAgent{nullptr};
+ QBluetoothDeviceInfo _currentDevice;
+ bool _deviceDiscovered{false};
//ble dev
- QLowEnergyController* _control{nullptr};
+ QLowEnergyController* _control{nullptr};
//ble ser
- bool _serviceDiscovered{false};
- QLowEnergyDescriptor _notificationDescTx;
- QLowEnergyService* _service{nullptr};
+ bool _serviceDiscovered{false};
+ QLowEnergyDescriptor _notificationDescTx;
+ QLowEnergyService* _service{nullptr};
};
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.cpp
index 404978fc9beee6afb7f5bf4d9c7c9d0f5e96e25b..29ea45b127b7bf0d94893bab41af2d00997ccc22 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.cpp
@@ -1,25 +1,28 @@
#include "BLEProthesisInterfaceQtWorkerThread.h"
+BLEProthesisInterfaceQtWorkerThread::BLEProthesisInterfaceQtWorkerThread(
+ const std::string& mac,
+ BLEProthesisInterface& owner) :
+ _owner{&owner}, _mac{QString::fromStdString(mac)}
+{
+}
-BLEProthesisInterfaceQtWorkerThread::BLEProthesisInterfaceQtWorkerThread(const std::string& mac, BLEProthesisInterface& owner) :
- _owner{&owner},
- _mac{QString::fromStdString(mac)}
-{}
-
-BLEProthesisInterfaceQtWorkerThread::~BLEProthesisInterfaceQtWorkerThread()
- = default;
+BLEProthesisInterfaceQtWorkerThread::~BLEProthesisInterfaceQtWorkerThread() = default;
-void BLEProthesisInterfaceQtWorkerThread::kill()
+void
+BLEProthesisInterfaceQtWorkerThread::kill()
{
_worker->kill();
}
-void BLEProthesisInterfaceQtWorkerThread::sendCommand(const std::string& cmd)
+void
+BLEProthesisInterfaceQtWorkerThread::sendCommand(const std::string& cmd)
{
_worker->sendCommand(cmd);
}
-void BLEProthesisInterfaceQtWorkerThread::run()
+void
+BLEProthesisInterfaceQtWorkerThread::run()
{
_worker = new BLEProthesisInterfaceQtWorker{_mac, *_owner};
qDebug() << '[' << _mac << ']' << " Starting qt event loop.";
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.h b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.h
index 258194b17f99a3d7e23e61cd69f4604989a00d0d..0d66d6ac8491e4b9c795cd15de10f9f0543c20aa 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.h
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterfaceQtWorkerThread.h
@@ -6,13 +6,11 @@
#include <QBluetoothDeviceInfo>
#include <QLowEnergyController>
#include <QLowEnergyService>
+#include <QThread>
#include "BLEProthesisInterface.h"
#include "BLEProthesisInterfaceQtWorker.h"
-#include <QThread>
-
-
class BLEProthesisInterfaceQtWorkerThread : public QThread
{
Q_OBJECT
@@ -21,10 +19,12 @@ public:
~BLEProthesisInterfaceQtWorkerThread();
void kill();
void sendCommand(const std::string& cmd);
+
private:
void run() override;
+
private:
- BLEProthesisInterface* _owner{nullptr};
- QString _mac;
- BLEProthesisInterfaceQtWorker* _worker{nullptr};
+ BLEProthesisInterface* _owner{nullptr};
+ QString _mac;
+ BLEProthesisInterfaceQtWorker* _worker{nullptr};
};
diff --git a/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp b/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
index a8a524a0dff47ce79912f1b25b80093cc7f45ae5..15c2f50eff5c726a209799315ecd3c3f1c2eeb27 100644
--- a/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
+++ b/source/RobotAPI/drivers/KITProstheticHandDriver/example/KITProstheticHandDriverExampleNoArmarX.cpp
@@ -1,15 +1,16 @@
+#include <iostream>
+#include <thread>
+
#include <QCoreApplication>
#include <RobotAPI/drivers/KITProstheticHandDriver/BLEProthesisInterface.h>
-#include <iostream>
-#include <thread>
-
-[[maybe_unused]] static constexpr auto old = "DF:70:E8:81:DB:D6";
-[[maybe_unused]] static constexpr auto dlr = "DF:A2:F5:48:E7:50";
+[[maybe_unused]] static constexpr auto old = "DF:70:E8:81:DB:D6";
+[[maybe_unused]] static constexpr auto dlr = "DF:A2:F5:48:E7:50";
[[maybe_unused]] static constexpr auto arches = "XX:XX:XX:XX:XX:XX";
-int main(int argc, char* argv[])
+int
+main(int argc, char* argv[])
{
for (int i = 0; i < argc; ++i)
{
@@ -47,10 +48,8 @@ int main(int argc, char* argv[])
{
for (std::size_t i2 = 0; i2 < n; ++i2)
{
- std::cout << iface.getThumbPos() << "\t"
- << iface.getThumbPWM() << "\t"
- << iface.getFingerPos() << "\t"
- << iface.getFingerPWM() << std::endl;
+ std::cout << iface.getThumbPos() << "\t" << iface.getThumbPWM() << "\t"
+ << iface.getFingerPos() << "\t" << iface.getFingerPWM() << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds{100});
}
};
@@ -67,4 +66,3 @@ int main(int argc, char* argv[])
}
return 0;
}
-
diff --git a/source/RobotAPI/drivers/MetaWearIMU/MetaWearIMU.cpp b/source/RobotAPI/drivers/MetaWearIMU/MetaWearIMU.cpp
index 23efa0543398c1036b06cf4cb149f5cad6494fb0..2d0dcb8a9f5bc2542323359b634f85debbed8e16 100644
--- a/source/RobotAPI/drivers/MetaWearIMU/MetaWearIMU.cpp
+++ b/source/RobotAPI/drivers/MetaWearIMU/MetaWearIMU.cpp
@@ -27,40 +27,41 @@
using namespace armarx;
-
-void MetaWearIMU::onInitComponent()
+void
+MetaWearIMU::onInitComponent()
{
usingTopic(getProperty<std::string>("MetaWearPythonTopicName"));
offeringTopic(getProperty<std::string>("MetaWearTopicName"));
}
-
-void MetaWearIMU::onConnectComponent()
+void
+MetaWearIMU::onConnectComponent()
{
- topicPrx = getTopic<MetaWearIMUListenerPrx>(getProperty<std::string>("MetaWearTopicName").getValue());
+ topicPrx =
+ getTopic<MetaWearIMUListenerPrx>(getProperty<std::string>("MetaWearTopicName").getValue());
}
-
-void MetaWearIMU::onDisconnectComponent()
+void
+MetaWearIMU::onDisconnectComponent()
{
-
}
-
-void MetaWearIMU::onExitComponent()
+void
+MetaWearIMU::onExitComponent()
{
-
}
-armarx::PropertyDefinitionsPtr MetaWearIMU::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+MetaWearIMU::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new MetaWearIMUPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new MetaWearIMUPropertyDefinitions(getConfigIdentifier()));
}
-
-
-void armarx::MetaWearIMU::reportIMUValues(const std::string& name, const MetaWearIMUDataExt& data_ext, const Ice::Current&)
+void
+armarx::MetaWearIMU::reportIMUValues(const std::string& name,
+ const MetaWearIMUDataExt& data_ext,
+ const Ice::Current&)
{
IceUtil::Time now = IceUtil::Time::now();
TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
diff --git a/source/RobotAPI/drivers/MetaWearIMU/MetaWearIMU.h b/source/RobotAPI/drivers/MetaWearIMU/MetaWearIMU.h
index 39d30c9f5f2ace0e8a6d1686fffdfe4195e4ed09..5905e0c08b7c70edf4bd9df0f661efdbfcb575e5 100644
--- a/source/RobotAPI/drivers/MetaWearIMU/MetaWearIMU.h
+++ b/source/RobotAPI/drivers/MetaWearIMU/MetaWearIMU.h
@@ -24,8 +24,9 @@
#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/interface/units/MetaWearIMUInterface.h>
+
#include <RobotAPI/interface/units/MetaWearIMU.h>
+#include <RobotAPI/interface/units/MetaWearIMUInterface.h>
namespace armarx
{
@@ -33,16 +34,18 @@ namespace armarx
* @class MetaWearIMUPropertyDefinitions
* @brief
*/
- class MetaWearIMUPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class MetaWearIMUPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- MetaWearIMUPropertyDefinitions(std::string prefix):
+ MetaWearIMUPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
- defineOptionalProperty<std::string>("MetaWearPythonTopicName", "MetaWearPythonData", "Name of the topic provided by the python driver");
- defineOptionalProperty<std::string>("MetaWearTopicName", "MetaWearData", "Name of the MetaWear topic");
+ defineOptionalProperty<std::string>("MetaWearPythonTopicName",
+ "MetaWearPythonData",
+ "Name of the topic provided by the python driver");
+ defineOptionalProperty<std::string>(
+ "MetaWearTopicName", "MetaWearData", "Name of the MetaWear topic");
}
};
@@ -65,7 +68,8 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "MetaWearIMU";
}
@@ -100,7 +104,8 @@ namespace armarx
// MetaWearIMUInterface interface
public:
- void reportIMUValues(const std::string& name, const MetaWearIMUDataExt& data_ext, const Ice::Current&) override;
+ void reportIMUValues(const std::string& name,
+ const MetaWearIMUDataExt& data_ext,
+ const Ice::Current&) override;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/OptoForce/OptoForce.cpp b/source/RobotAPI/drivers/OptoForce/OptoForce.cpp
index f511b88a747d444a080883c6d7e14cffedc103d0..afb14e448975a85f1a2160bfb7d310dbb5721e5a 100644
--- a/source/RobotAPI/drivers/OptoForce/OptoForce.cpp
+++ b/source/RobotAPI/drivers/OptoForce/OptoForce.cpp
@@ -25,33 +25,28 @@
using namespace armarx;
-
-void OptoForce::onInitComponent()
+void
+OptoForce::onInitComponent()
{
-
}
-
-void OptoForce::onConnectComponent()
+void
+OptoForce::onConnectComponent()
{
-
}
-
-void OptoForce::onDisconnectComponent()
+void
+OptoForce::onDisconnectComponent()
{
-
}
-
-void OptoForce::onExitComponent()
+void
+OptoForce::onExitComponent()
{
-
}
-armarx::PropertyDefinitionsPtr OptoForce::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+OptoForce::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new OptoForcePropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(new OptoForcePropertyDefinitions(getConfigIdentifier()));
}
-
diff --git a/source/RobotAPI/drivers/OptoForce/OptoForce.h b/source/RobotAPI/drivers/OptoForce/OptoForce.h
index 4c4724b3988d621357433d4011bb686e87af65d3..378dc1b1683a390158df803999d4aa60af4dc4b8 100644
--- a/source/RobotAPI/drivers/OptoForce/OptoForce.h
+++ b/source/RobotAPI/drivers/OptoForce/OptoForce.h
@@ -24,6 +24,7 @@
#include <ArmarXCore/core/Component.h>
+
#include <opto.h>
namespace armarx
@@ -32,11 +33,10 @@ namespace armarx
* @class OptoForcePropertyDefinitions
* @brief
*/
- class OptoForcePropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class OptoForcePropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- OptoForcePropertyDefinitions(std::string prefix):
+ OptoForcePropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
@@ -55,14 +55,14 @@ namespace armarx
*
* Detailed description of class OptoForce.
*/
- class OptoForce :
- virtual public armarx::Component
+ class OptoForce : virtual public armarx::Component
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- virtual std::string getDefaultName() const
+ virtual std::string
+ getDefaultName() const
{
return "OptoForce";
}
@@ -93,5 +93,4 @@ namespace armarx
*/
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/OptoForce/test/OptoForceTest.cpp b/source/RobotAPI/drivers/OptoForce/test/OptoForceTest.cpp
index 721c33f3de840451e4ef7ce2b6092d8ede4c0a80..aee4bfc1472dede9b47f7b9e01d0c97e0ad998a4 100644
--- a/source/RobotAPI/drivers/OptoForce/test/OptoForceTest.cpp
+++ b/source/RobotAPI/drivers/OptoForce/test/OptoForceTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/components/OptoForceOMD/OptoForceOMD.h>
#include <iostream>
+#include <RobotAPI/components/OptoForceOMD/OptoForceOMD.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::OptoForceOMD instance;
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
index dc56112a064925baa1c0892697883cb1fa4f94fa..9541b6bd3f122b189699680314d46c693b8c5811 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.cpp
@@ -22,20 +22,18 @@
#include "OptoForceUnit.h"
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <ArmarXCore/core/util/StringHelpers.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
using namespace armarx;
-
-OptoForceUnit::OptoForceUnit()
- : isRecording(false), stopRecordingFlag(false)
+OptoForceUnit::OptoForceUnit() : isRecording(false), stopRecordingFlag(false)
{
-
}
-void OptoForceUnit::onInitComponent()
+void
+OptoForceUnit::onInitComponent()
{
std::string calibrationFilePath = getProperty<std::string>("CalibrationFilePath").getValue();
if (!ArmarXDataPath::getAbsolutePath(calibrationFilePath, calibrationFilePath))
@@ -71,11 +69,13 @@ void OptoForceUnit::onInitComponent()
{
std::string deviceName(portlist[i].name);
std::string serialNumber(portlist[i].serialNumber);
- ARMARX_INFO << "Found DAQ: deviceName='" << deviceName << "', serialNumber='" << serialNumber << "'";
+ ARMARX_INFO << "Found DAQ: deviceName='" << deviceName << "', serialNumber='"
+ << serialNumber << "'";
RapidXmlReaderNode daqNode = findDaqNode(serialNumber);
if (!daqNode.is_valid())
{
- throw LocalException("Could not find config node for device deviceName='") << deviceName << "', serialNumber='" << serialNumber << "'";
+ throw LocalException("Could not find config node for device deviceName='")
+ << deviceName << "', serialNumber='" << serialNumber << "'";
}
DaqWrapperPtr daqPtr(new DaqWrapper(deviceName, serialNumber, daqNode));
daqList.push_back(daqPtr);
@@ -93,17 +93,18 @@ void OptoForceUnit::onInitComponent()
}
readTask = new RunningTask<OptoForceUnit>(this, &OptoForceUnit::run, "OptoForceUnit");
-
}
-
-void OptoForceUnit::onConnectComponent()
+void
+OptoForceUnit::onConnectComponent()
{
- topicPrx = getTopic<OptoForceUnitListenerPrx>(getProperty<std::string>("OptoForceTopicName").getValue());
+ topicPrx = getTopic<OptoForceUnitListenerPrx>(
+ getProperty<std::string>("OptoForceTopicName").getValue());
readTask->start();
}
-void OptoForceUnit::run()
+void
+OptoForceUnit::run()
{
ARMARX_IMPORTANT << "run";
@@ -152,7 +153,12 @@ void OptoForceUnit::run()
float y = pa[i * size + n].y / countsPerN - daqPtr->offsets.at(i).at(1);
float z = pa[i * size + n].z / countsPerN - daqPtr->offsets.at(i).at(2);
- batchPrx->reportSensorValues(daqPtr->deviceName + ":" + to_string(i), daqPtr->sensorNames.at(i), x, y, z, nowTimestamp);
+ batchPrx->reportSensorValues(daqPtr->deviceName + ":" + to_string(i),
+ daqPtr->sensorNames.at(i),
+ x,
+ y,
+ z,
+ nowTimestamp);
flushNeeded = true;
data.at(i) = {x, y, z};
}
@@ -175,12 +181,12 @@ void OptoForceUnit::run()
recordingFile << ",";
}
first = false;
- recordingFile << "[" << data.at(i).at(0) << "," << data.at(i).at(1) << "," << data.at(i).at(2) << "]";
+ recordingFile << "[" << data.at(i).at(0) << "," << data.at(i).at(1) << ","
+ << data.at(i).at(2) << "]";
}
recordingFile << "]}\n";
recordingFile.flush();
}
-
}
}
if (isRecording && stopRecordingFlag)
@@ -200,44 +206,49 @@ void OptoForceUnit::run()
}
}
-
-void OptoForceUnit::onDisconnectComponent()
+void
+OptoForceUnit::onDisconnectComponent()
{
-
}
-
-void OptoForceUnit::onExitComponent()
+void
+OptoForceUnit::onExitComponent()
{
readTask->stop();
}
-armarx::PropertyDefinitionsPtr OptoForceUnit::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+OptoForceUnit::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new OptoForceUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new OptoForceUnitPropertyDefinitions(getConfigIdentifier()));
}
-
-
-OptoForceUnit::DaqWrapper::DaqWrapper(const std::string& deviceName, const std::string& serialNumber, const RapidXmlReaderNode& daqNode)
- : deviceName(deviceName), serialNumber(serialNumber)
+OptoForceUnit::DaqWrapper::DaqWrapper(const std::string& deviceName,
+ const std::string& serialNumber,
+ const RapidXmlReaderNode& daqNode) :
+ deviceName(deviceName), serialNumber(serialNumber)
{
int i = 0;
for (RapidXmlReaderNode sensorNode : daqNode.nodes("Sensor"))
{
float counts_at_nc = sensorNode.attribute_as_float("counts_at_nc");
float nominalCapacity = sensorNode.attribute_as_float("nominalCapacity");
- std::string sensorName = sensorNode.attribute_value_or_default("name", serialNumber + "-" + to_string(i));
+ std::string sensorName =
+ sensorNode.attribute_value_or_default("name", serialNumber + "-" + to_string(i));
countsPerN.push_back(counts_at_nc / nominalCapacity);
sensorNames.push_back(sensorName);
- enableFlags.push_back(sensorNode.attribute_as_optional_bool("enabled", "true", "false", true));
- offsets.push_back({sensorNode.attribute_as_float("offsetX"), sensorNode.attribute_as_float("offsetY"), sensorNode.attribute_as_float("offsetZ")});
+ enableFlags.push_back(
+ sensorNode.attribute_as_optional_bool("enabled", "true", "false", true));
+ offsets.push_back({sensorNode.attribute_as_float("offsetX"),
+ sensorNode.attribute_as_float("offsetY"),
+ sensorNode.attribute_as_float("offsetZ")});
i++;
}
}
-void OptoForceUnit::DaqWrapper::printInfo()
+void
+OptoForceUnit::DaqWrapper::printInfo()
{
SensorConfig config = daq.getConfig();
int state = config.getState();
@@ -256,24 +267,28 @@ void OptoForceUnit::DaqWrapper::printInfo()
ARMARX_IMPORTANT_S << "getVersion: " << daq.getVersion();
}
-void OptoForceUnit::DaqWrapper::checkSensorCount()
+void
+OptoForceUnit::DaqWrapper::checkSensorCount()
{
if ((int)countsPerN.size() != daq.getSensorSize())
{
- throw LocalException("Sensor count mismatch. Configured: ") << ((int)countsPerN.size()) << " found: " << daq.getSensorSize();
+ throw LocalException("Sensor count mismatch. Configured: ")
+ << ((int)countsPerN.size()) << " found: " << daq.getSensorSize();
}
- ARMARX_INFO_S << "Configured: " << ((int)countsPerN.size()) << " found: " << daq.getSensorSize() << " sensors";
+ ARMARX_INFO_S << "Configured: " << ((int)countsPerN.size()) << " found: " << daq.getSensorSize()
+ << " sensors";
}
-
-void armarx::OptoForceUnit::startRecording(const std::string& filepath, const Ice::Current&)
+void
+armarx::OptoForceUnit::startRecording(const std::string& filepath, const Ice::Current&)
{
ARMARX_IMPORTANT << "start recording: " << filepath;
recordingFile.open(filepath);
isRecording = true;
}
-void armarx::OptoForceUnit::stopRecording(const Ice::Current&)
+void
+armarx::OptoForceUnit::stopRecording(const Ice::Current&)
{
stopRecordingFlag = true;
}
diff --git a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
index 8b41a421d97e85fd83dca6bd8cfd563dbb3bfee2..a9761fed2409e81c91c09f030a6fb603cd24678f 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
+++ b/source/RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h
@@ -24,10 +24,12 @@
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
+
#include <RobotAPI/interface/units/OptoForceUnit.h>
+
#include <opto.h>
-#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
namespace armarx
{
@@ -35,16 +37,18 @@ namespace armarx
* @class OptoForceUnitPropertyDefinitions
* @brief
*/
- class OptoForceUnitPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class OptoForceUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- OptoForceUnitPropertyDefinitions(std::string prefix):
+ OptoForceUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
//defineRequiredProperty<std::string>("PropertyName", "Description");
- defineOptionalProperty<std::string>("OptoForceTopicName", "OptoForceValues", "Name of the OptoForce Topic");
- defineOptionalProperty<std::string>("CalibrationFilePath", "RobotAPI/sensors/OptoForceCalibration.xml", "Path of the Calibration File");
+ defineOptionalProperty<std::string>(
+ "OptoForceTopicName", "OptoForceValues", "Name of the OptoForce Topic");
+ defineOptionalProperty<std::string>("CalibrationFilePath",
+ "RobotAPI/sensors/OptoForceCalibration.xml",
+ "Path of the Calibration File");
}
};
@@ -59,15 +63,15 @@ namespace armarx
*
* Detailed description of class OptoForceUnit.
*/
- class OptoForceUnit :
- virtual public OptoForceUnitInterface,
- virtual public armarx::Component
+ class OptoForceUnit : virtual public OptoForceUnitInterface, virtual public armarx::Component
{
private:
class DaqWrapper
{
public:
- DaqWrapper(const std::string& deviceName, const std::string& serialNumber, const RapidXmlReaderNode& daqNode);
+ DaqWrapper(const std::string& deviceName,
+ const std::string& serialNumber,
+ const RapidXmlReaderNode& daqNode);
OptoDAQ daq;
std::string deviceName;
@@ -80,6 +84,7 @@ namespace armarx
void printInfo();
void checkSensorCount();
};
+
using DaqWrapperPtr = std::shared_ptr<DaqWrapper>;
public:
@@ -88,7 +93,8 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "OptoForceUnit";
}
@@ -134,11 +140,9 @@ namespace armarx
bool stopRecordingFlag;
-
// OptoForceUnitInterface interface
public:
void startRecording(const std::string& filepath, const Ice::Current&) override;
void stopRecording(const Ice::Current&) override;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/OptoForceUnit/test/OptoForceUnitTest.cpp b/source/RobotAPI/drivers/OptoForceUnit/test/OptoForceUnitTest.cpp
index 3f12161380ccbae533b0d649ac5dd957ad57b086..e82d92a067b54685dd1d9c225b77286cbeb5bba9 100644
--- a/source/RobotAPI/drivers/OptoForceUnit/test/OptoForceUnitTest.cpp
+++ b/source/RobotAPI/drivers/OptoForceUnit/test/OptoForceUnitTest.cpp
@@ -25,10 +25,11 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h>
#include <iostream>
+#include <RobotAPI/drivers/OptoForceUnit/OptoForceUnit.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
armarx::OptoForceUnit instance;
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
index 61a75d997f2ef1871b12cffc78cde76cb797b3ca..97f4c58b2618dffed3843517bbd0935d9c464157 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -1,12 +1,16 @@
#include "OrientedTactileSensorUnit.h"
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <termios.h>
-#include <sys/ioctl.h>
-#include <fcntl.h>
+
#include <math.h>
+
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <termios.h>
+
#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
#include <RobotAPI/libraries/core/Pose.h>
using namespace armarx;
@@ -20,7 +24,8 @@ OrientedTactileSensorUnit::OrientedTactileSensorUnit()
sampleIndexPressureRate = 0;
}
-void OrientedTactileSensorUnit::onInitComponent()
+void
+OrientedTactileSensorUnit::onInitComponent()
{
//logger part
if (getProperty<bool>("logData").getValue())
@@ -34,7 +39,7 @@ void OrientedTactileSensorUnit::onInitComponent()
std::string packageName = "RobotAPI";
armarx::CMakePackageFinder finder(packageName);
std::string dataDir = finder.getDataDir() + "/" + packageName + "/logs/";
- std::string filename = dataDir + buffer + std::string("_data") + ".json";
+ std::string filename = dataDir + buffer + std::string("_data") + ".json";
//ARMARX_IMPORTANT << filename;
logger.reset(new SimpleJsonLogger(filename, true));
@@ -76,12 +81,14 @@ void OrientedTactileSensorUnit::onInitComponent()
/* Flush anything already in the serial buffer */
tcflush(fd, TCIFLUSH);
- ARMARX_INFO << "opening device " << getProperty<std::string>("SerialInterfaceDevice").getValue();
+ ARMARX_INFO << "opening device "
+ << getProperty<std::string>("SerialInterfaceDevice").getValue();
if (!arduinoIn.is_open())
{
- throw LocalException("Cannot open Arduino on ") << getProperty<std::string>("SerialInterfaceDevice").getValue();
+ throw LocalException("Cannot open Arduino on ")
+ << getProperty<std::string>("SerialInterfaceDevice").getValue();
}
ARMARX_INFO << "Arduino restarts, please wait ...";
@@ -118,7 +125,8 @@ void OrientedTactileSensorUnit::onInitComponent()
else
{
//load calibration
- ARMARX_INFO << "load calibration data " << getProperty<std::string>("CalibrationData").getValue();
+ ARMARX_INFO << "load calibration data "
+ << getProperty<std::string>("CalibrationData").getValue();
if (loadCalibration())
{
ARMARX_IMPORTANT << "loaded calibration";
@@ -134,23 +142,26 @@ void OrientedTactileSensorUnit::onInitComponent()
}
readTask = new RunningTask<OrientedTactileSensorUnit>(this, &OrientedTactileSensorUnit::run);
readTask->start();
-
}
-void OrientedTactileSensorUnit::onConnectComponent()
+void
+OrientedTactileSensorUnit::onConnectComponent()
{
- debugDrawerTopic = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
+ debugDrawerTopic = getTopic<DebugDrawerInterfacePrx>(
+ getProperty<std::string>("DebugDrawerTopicName").getValue());
std::string topicName = getProperty<std::string>("TopicName").getValue();
topicPrx = getTopic<OrientedTactileSensorUnitListenerPrx>(topicName);
}
-PropertyDefinitionsPtr OrientedTactileSensorUnit::createPropertyDefinitions()
+PropertyDefinitionsPtr
+OrientedTactileSensorUnit::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new OrientedTactileSensorUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(
+ new OrientedTactileSensorUnitPropertyDefinitions(getConfigIdentifier()));
}
-void OrientedTactileSensorUnit::run()
+void
+OrientedTactileSensorUnit::run()
{
while (readTask->isRunning())
{
@@ -168,9 +179,10 @@ void OrientedTactileSensorUnit::run()
{
RotationRate sampleRotation;
sampleRotation.timestamp = now;
- sampleRotation.orientation = Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
+ sampleRotation.orientation = Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
//sampleRotation.orientation = sampleRotation.orientation * inverseOrientation;
- if (0 < maxSamplesRotation && samplesRotation.size() < static_cast<std::size_t>(maxSamplesRotation))
+ if (0 < maxSamplesRotation &&
+ samplesRotation.size() < static_cast<std::size_t>(maxSamplesRotation))
{
samplesRotation.push_back(sampleRotation);
}
@@ -182,9 +194,12 @@ void OrientedTactileSensorUnit::run()
RotationRate oldsampleRotation;
oldsampleRotation.timestamp = samplesRotation.at(sampleIndexRotation).timestamp;
oldsampleRotation.orientation = samplesRotation.at(sampleIndexRotation).orientation;
- Eigen::AngleAxisf aa(sampleRotation.orientation * oldsampleRotation.orientation.inverse());
+ Eigen::AngleAxisf aa(sampleRotation.orientation *
+ oldsampleRotation.orientation.inverse());
//ARMARX_IMPORTANT << "aa: " << aa.axis() << " " << aa.angle();
- rotationRate = aa.angle() / (sampleRotation.timestamp - oldsampleRotation.timestamp).toSecondsDouble();
+ rotationRate =
+ aa.angle() /
+ (sampleRotation.timestamp - oldsampleRotation.timestamp).toSecondsDouble();
}
}
//compute pressureRate
@@ -192,7 +207,8 @@ void OrientedTactileSensorUnit::run()
PressureRate samplePressure;
samplePressure.timestamp = now;
samplePressure.pressure = data.pressure;
- if (0 < maxSamplesPressure && samplesPressure.size() < static_cast<std::size_t>(maxSamplesPressure))
+ if (0 < maxSamplesPressure &&
+ samplesPressure.size() < static_cast<std::size_t>(maxSamplesPressure))
{
samplesPressure.push_back(samplePressure);
}
@@ -203,7 +219,9 @@ void OrientedTactileSensorUnit::run()
PressureRate oldsamplePressure;
oldsamplePressure.timestamp = samplesPressure.at(sampleIndexPressure).timestamp;
oldsamplePressure.pressure = samplesPressure.at(sampleIndexPressure).pressure;
- pressureRate = (samplePressure.pressure - oldsamplePressure.pressure) / (samplePressure.timestamp - oldsamplePressure.timestamp).toSecondsDouble();
+ pressureRate =
+ (samplePressure.pressure - oldsamplePressure.pressure) /
+ (samplePressure.timestamp - oldsamplePressure.timestamp).toSecondsDouble();
}
//compute angular accceleration Rate
@@ -211,7 +229,8 @@ void OrientedTactileSensorUnit::run()
AccelerationRate sampleAcceleration;
sampleAcceleration.timestamp = now;
sampleAcceleration.rotationRate = rotationRate;
- if (0 < maxSamplesAcceleration && samplesAcceleration.size() < static_cast<std::size_t>(maxSamplesAcceleration))
+ if (0 < maxSamplesAcceleration &&
+ samplesAcceleration.size() < static_cast<std::size_t>(maxSamplesAcceleration))
{
samplesAcceleration.push_back(sampleAcceleration);
}
@@ -220,11 +239,16 @@ void OrientedTactileSensorUnit::run()
samplesAcceleration[sampleIndexAcceleration] = sampleAcceleration;
sampleIndexAcceleration = (sampleIndexAcceleration + 1) % maxSamplesAcceleration;
AccelerationRate oldsampleAcceleration;
- oldsampleAcceleration.timestamp = samplesAcceleration.at(sampleIndexAcceleration).timestamp;
- oldsampleAcceleration.rotationRate = samplesAcceleration.at(sampleIndexAcceleration).rotationRate;
- accelerationRate = (sampleAcceleration.rotationRate - oldsampleAcceleration.rotationRate) / (sampleAcceleration.timestamp - oldsampleAcceleration.timestamp).toSecondsDouble();
+ oldsampleAcceleration.timestamp =
+ samplesAcceleration.at(sampleIndexAcceleration).timestamp;
+ oldsampleAcceleration.rotationRate =
+ samplesAcceleration.at(sampleIndexAcceleration).rotationRate;
+ accelerationRate =
+ (sampleAcceleration.rotationRate - oldsampleAcceleration.rotationRate) /
+ (sampleAcceleration.timestamp - oldsampleAcceleration.timestamp).toSecondsDouble();
}
- if (0 < maxSamplesPressure && pressureRates.size() < static_cast<std::size_t>(maxSamplesPressure))
+ if (0 < maxSamplesPressure &&
+ pressureRates.size() < static_cast<std::size_t>(maxSamplesPressure))
{
pressureRates.push_back(pressureRate);
}
@@ -238,7 +262,8 @@ void OrientedTactileSensorUnit::run()
ARMARX_IMPORTANT << "contact";
}
- Eigen::Quaternionf orientationQuaternion = Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
+ Eigen::Quaternionf orientationQuaternion =
+ Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
if (getProperty<bool>("logData").getValue())
{
@@ -304,17 +329,30 @@ void OrientedTactileSensorUnit::run()
data.qx = quaternion.x();
data.qy = quaternion.y();
data.qz = quaternion.z();
- ARMARX_IMPORTANT << "or " << orientationQuaternion.w() << " " << orientationQuaternion.x() << " " << orientationQuaternion.y() << " " << orientationQuaternion.z();
+ ARMARX_IMPORTANT << "or " << orientationQuaternion.w() << " " << orientationQuaternion.x()
+ << " " << orientationQuaternion.y() << " " << orientationQuaternion.z();
if (topicPrx)
{
- topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, pressureRate, rotationRate, accelerationRate, data.accelx, data.accely, data.accelz, nowTimestamp);
+ topicPrx->reportSensorValues(data.id,
+ data.pressure,
+ data.qw,
+ data.qx,
+ data.qy,
+ data.qz,
+ pressureRate,
+ rotationRate,
+ accelerationRate,
+ data.accelx,
+ data.accely,
+ data.accelz,
+ nowTimestamp);
}
}
-
}
// get imu values from incoming string
-OrientedTactileSensorUnit::SensorData OrientedTactileSensorUnit::getValues(std::string line)
+OrientedTactileSensorUnit::SensorData
+OrientedTactileSensorUnit::getValues(std::string line)
{
SensorData data;
std::vector<std::string> splitValues = Split(line, " ");
@@ -331,7 +369,9 @@ OrientedTactileSensorUnit::SensorData OrientedTactileSensorUnit::getValues(std::
}
std::string space = " ";
-bool OrientedTactileSensorUnit::loadCalibration()
+
+bool
+OrientedTactileSensorUnit::loadCalibration()
{
std::string calibrationStream = getProperty<std::string>("CalibrationData").getValue();
std::string arduinoLine;
@@ -356,7 +396,8 @@ bool OrientedTactileSensorUnit::loadCalibration()
return true;
}
-bool OrientedTactileSensorUnit::getCalibrationValues(std::string line)
+bool
+OrientedTactileSensorUnit::getCalibrationValues(std::string line)
{
std::vector<std::string> splitValues = Split(line, " ");
calibration.accel_offset_x = stoi(splitValues.at(0));
@@ -372,8 +413,14 @@ bool OrientedTactileSensorUnit::getCalibrationValues(std::string line)
calibration.mag_radius = stoi(splitValues.at(10));
std::string space = " ";
std::string calibrationStream = "";
- calibrationStream = calibrationStream + to_string(calibration.accel_offset_x) + space + to_string(calibration.accel_offset_y) + space + to_string(calibration.accel_offset_z) + space + to_string(calibration.gyro_offset_x) + space + to_string(calibration.gyro_offset_y) + space +
- to_string(calibration.gyro_offset_z) + space + to_string(calibration.mag_offset_x) + space + to_string(calibration.mag_offset_y) + space + to_string(calibration.mag_offset_z) + space + to_string(calibration.accel_radius) + space + to_string(calibration.mag_radius) + space;
- ARMARX_IMPORTANT << "calibration data: " << calibrationStream ;
+ calibrationStream =
+ calibrationStream + to_string(calibration.accel_offset_x) + space +
+ to_string(calibration.accel_offset_y) + space + to_string(calibration.accel_offset_z) +
+ space + to_string(calibration.gyro_offset_x) + space +
+ to_string(calibration.gyro_offset_y) + space + to_string(calibration.gyro_offset_z) +
+ space + to_string(calibration.mag_offset_x) + space + to_string(calibration.mag_offset_y) +
+ space + to_string(calibration.mag_offset_z) + space + to_string(calibration.accel_radius) +
+ space + to_string(calibration.mag_radius) + space;
+ ARMARX_IMPORTANT << "calibration data: " << calibrationStream;
return true;
}
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index 7e584629a24c0673cd3fb145a28ea3b771358974..817d56960d93ae865a1f333a25abb5fb54c65ee1 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -1,66 +1,62 @@
#pragma once
+#include <stdio.h>
+
+#include <fstream>
+#include <iostream>
+
+#include <netinet/in.h>
+
+#include <Eigen/Dense>
+
#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/interface/units/UnitInterface.h>
-#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
-#include <netinet/in.h>
-#include <iostream>
-#include <fstream>
-#include <stdio.h>
-#include <Eigen/Dense>
-#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+
+#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
+#include <RobotAPI/interface/units/UnitInterface.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
namespace armarx
{
- class OrientedTactileSensorUnitPropertyDefinitions:
- public ComponentPropertyDefinitions
+ class OrientedTactileSensorUnitPropertyDefinitions : public ComponentPropertyDefinitions
{
public:
- OrientedTactileSensorUnitPropertyDefinitions(std::string prefix):
+ OrientedTactileSensorUnitPropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>(
- "SerialInterfaceDevice",
- "/dev/ttyACM0",
- "The serial device the arduino is connected to.");
+ defineOptionalProperty<std::string>("SerialInterfaceDevice",
+ "/dev/ttyACM0",
+ "The serial device the arduino is connected to.");
defineOptionalProperty<std::string>(
"TopicName",
"OrientedTactileSensorValues",
"Name of the topic on which the sensor values are provided");
+ defineOptionalProperty<std::string>("CalibrationData",
+ "65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
+ "Sensor Register Data to calibrate the sensor");
+
defineOptionalProperty<std::string>(
- "CalibrationData",
- "65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
- "Sensor Register Data to calibrate the sensor");
+ "SamplesRotation", "20", "number of orientation values to differentiate");
defineOptionalProperty<std::string>(
- "SamplesRotation",
- "20",
- "number of orientation values to differentiate");
+ "SamplesPressure", "10", "number of pressure values to differentiate");
defineOptionalProperty<std::string>(
- "SamplesPressure",
- "10",
- "number of pressure values to differentiate");
+ "SamplesAcceleration", "20", "number of pressure values to differentiate");
+ defineOptionalProperty<bool>("logData", "false", "log data from sensor");
+ defineOptionalProperty<bool>("calibrateSensor",
+ "false"
+ "Set true to calibrate the sensor and get calibration "
+ "data and false to use existent calibration data");
defineOptionalProperty<std::string>(
- "SamplesAcceleration",
- "20",
- "number of pressure values to differentiate");
-
- defineOptionalProperty<bool>(
- "logData",
- "false",
- "log data from sensor");
- defineOptionalProperty<bool>(
- "calibrateSensor",
- "false"
- "Set true to calibrate the sensor and get calibration data and false to use existent calibration data");
- defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "Name of the debug drawer topic that should be used");
+ "DebugDrawerTopicName",
+ "DebugDrawerUpdates",
+ "Name of the debug drawer topic that should be used");
}
};
@@ -69,14 +65,14 @@ namespace armarx
* @brief ArmarX wrapper for an arduino due with one BNO055 IMU and one BMP280 pressure sensor
*
*/
- class OrientedTactileSensorUnit:
- virtual public armarx::Component
+ class OrientedTactileSensorUnit : virtual public armarx::Component
//TODO: needs interface to send calibration data
{
public:
OrientedTactileSensorUnit();
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "OrientedTactileSensorUnit";
}
@@ -91,7 +87,8 @@ namespace armarx
struct CalibrationData
{
- int accel_offset_x, accel_offset_y, accel_offset_z, gyro_offset_x, gyro_offset_y, gyro_offset_z, mag_offset_x, mag_offset_y, mag_offset_z, accel_radius, mag_radius;
+ int accel_offset_x, accel_offset_y, accel_offset_z, gyro_offset_x, gyro_offset_y,
+ gyro_offset_z, mag_offset_x, mag_offset_y, mag_offset_z, accel_radius, mag_radius;
};
struct PressureRate
@@ -111,6 +108,7 @@ namespace armarx
IceUtil::Time timestamp;
float rotationRate;
};
+
struct LinAccRate
{
IceUtil::Time timestamp;
@@ -158,4 +156,4 @@ namespace armarx
SimpleJsonLoggerPtr logger;
std::string prefix;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/ProsthesisObserver/ProsthesisInterface.ice b/source/RobotAPI/drivers/ProsthesisObserver/ProsthesisInterface.ice
index 28219519e85357d13747a234352dd594d6f4c805..57fe20a784171867eb04b62829f37fd31d3d695f 100644
--- a/source/RobotAPI/drivers/ProsthesisObserver/ProsthesisInterface.ice
+++ b/source/RobotAPI/drivers/ProsthesisObserver/ProsthesisInterface.ice
@@ -44,7 +44,7 @@ module armarx
interface ProsthesisInterface
{
- string getTopicName();
+ string getTopicName();
};
};
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp b/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp
index 856fffc1a8df18e9088ec2cd588ba41fb4932ae2..1318e0c979346b4cfffa33eceb9aa2a01128e210 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.cpp
@@ -22,27 +22,28 @@
* GNU General Public License
*/
#include "AbstractInterface.h"
-#include "Response.h"
-#include "Checksum.h"
-#include "Types.h"
-#include "TransmissionException.h"
+
+#include <stdio.h>
#include <string.h>
+
#include <iostream>
-#include <stdio.h>
#include <stdexcept>
+#include "Checksum.h"
+#include "Response.h"
+#include "TransmissionException.h"
+#include "Types.h"
-AbstractInterface::AbstractInterface()
- : connected(false)
+AbstractInterface::AbstractInterface() : connected(false)
{
-
}
AbstractInterface::~AbstractInterface()
{
}
-int AbstractInterface::read(unsigned char* buf, unsigned int len)
+int
+AbstractInterface::read(unsigned char* buf, unsigned int len)
{
int res = readInternal(buf, len);
@@ -54,7 +55,8 @@ int AbstractInterface::read(unsigned char* buf, unsigned int len)
return res;
}
-int AbstractInterface::write(unsigned char* buf, unsigned int len)
+int
+AbstractInterface::write(unsigned char* buf, unsigned int len)
{
if (log != NULL)
{
@@ -64,14 +66,21 @@ int AbstractInterface::write(unsigned char* buf, unsigned int len)
return writeInternal(buf, len);
}
-
-Response AbstractInterface::submitCmd(unsigned char id, unsigned char* payload, unsigned int len, bool pending)
+Response
+AbstractInterface::submitCmd(unsigned char id,
+ unsigned char* payload,
+ unsigned int len,
+ bool pending)
{
fireAndForgetCmd(id, payload, len, pending);
return receive(pending, id);
}
-void AbstractInterface::fireAndForgetCmd(unsigned char id, unsigned char* payload, unsigned int len, bool pending)
+void
+AbstractInterface::fireAndForgetCmd(unsigned char id,
+ unsigned char* payload,
+ unsigned int len,
+ bool pending)
{
int res;
@@ -91,12 +100,14 @@ void AbstractInterface::fireAndForgetCmd(unsigned char id, unsigned char* payloa
}
}
-void AbstractInterface::startLogging(std::string file)
+void
+AbstractInterface::startLogging(std::string file)
{
log.reset(new BinaryLogger(file));
}
-void AbstractInterface::logText(std::string message)
+void
+AbstractInterface::logText(std::string message)
{
if (log != NULL)
{
@@ -104,7 +115,8 @@ void AbstractInterface::logText(std::string message)
}
}
-Response AbstractInterface::receiveWithoutChecks()
+Response
+AbstractInterface::receiveWithoutChecks()
{
int res;
status_t status;
@@ -118,12 +130,13 @@ Response AbstractInterface::receiveWithoutChecks()
throw TransmissionException("Message receive failed");
}
- status = (status_t) make_short(msg.data[0], msg.data[1]);
+ status = (status_t)make_short(msg.data[0], msg.data[1]);
return Response(res, msg.id, status, msg.data, msg.len);
}
-Response AbstractInterface::receive(bool pending, unsigned char expectedId)
+Response
+AbstractInterface::receive(bool pending, unsigned char expectedId)
{
int res;
status_t status;
@@ -146,7 +159,9 @@ Response AbstractInterface::receive(bool pending, unsigned char expectedId)
//std::strstream strStream;
//strStream << "Response ID ()" << msg.id << ") does not match submitted command ID (" << id << ")";
//throw std::runtime_error(strStream.str());
- throw TransmissionException(str(boost::format("Response ID (%02X) does not match submitted command ID (%02X)") % (int)msg.id % (int)expectedId));
+ throw TransmissionException(
+ str(boost::format("Response ID (%02X) does not match submitted command ID (%02X)") %
+ (int)msg.id % (int)expectedId));
}
if (pending)
@@ -156,21 +171,21 @@ Response AbstractInterface::receive(bool pending, unsigned char expectedId)
throw TransmissionException("No status code received");
}
- status = (status_t) make_short(msg.data[0], msg.data[1]);
+ status = (status_t)make_short(msg.data[0], msg.data[1]);
}
- }
- while (pending && status == E_CMD_PENDING);
+ } while (pending && status == E_CMD_PENDING);
- status = (status_t) make_short(msg.data[0], msg.data[1]);
+ status = (status_t)make_short(msg.data[0], msg.data[1]);
return Response(res, msg.id, status, msg.data, msg.len);
}
-int AbstractInterface::receive(msg_t* msg)
+int
+AbstractInterface::receive(msg_t* msg)
{
int res;
- unsigned char header[3]; // 1 byte command, 2 bytes payload length
- unsigned short checksum = 0x50f5; // Checksum over preamble (0xaa 0xaa 0xaa)
+ unsigned char header[3]; // 1 byte command, 2 bytes payload length
+ unsigned short checksum = 0x50f5; // Checksum over preamble (0xaa 0xaa 0xaa)
unsigned int sync;
logText("read preamble");
@@ -196,7 +211,8 @@ int AbstractInterface::receive(msg_t* msg)
if (res < 3)
{
- throw TransmissionException(str(boost::format("Failed to receive header data (%d bytes read)") % res));
+ throw TransmissionException(
+ str(boost::format("Failed to receive header data (%d bytes read)") % res));
}
// Calculate checksum over header
@@ -213,7 +229,7 @@ int AbstractInterface::receive(msg_t* msg)
//if ( !msg->data ) return -1;
//std::shared_ptr<unsigned char[]> data(new unsigned char[ msg->len + 2u ]);
- unsigned char* data = new unsigned char[ msg->len + 2u ];
+ unsigned char* data = new unsigned char[msg->len + 2u];
// Read payload and checksum
@@ -244,7 +260,9 @@ int AbstractInterface::receive(msg_t* msg)
if (read != (int)msg->len + 2)
{
delete[] data;
- throw TransmissionException(str(boost::format("Not enough data (%d, expected %d), Command = %02X") % res % (msg->len + 2) % msg->id));
+ throw TransmissionException(
+ str(boost::format("Not enough data (%d, expected %d), Command = %02X") % res %
+ (msg->len + 2) % msg->id));
}
/*
@@ -272,7 +290,8 @@ int AbstractInterface::receive(msg_t* msg)
return msg->len + 8;
}
-int AbstractInterface::send(unsigned char id, unsigned int len, unsigned char* data)
+int
+AbstractInterface::send(unsigned char id, unsigned int len, unsigned char* data)
{
unsigned char header[MSG_PREAMBLE_LEN + 3];
unsigned short crc;
@@ -298,10 +317,10 @@ int AbstractInterface::send(unsigned char id, unsigned int len, unsigned char* d
crc = Checksum::Update_crc16(data, len, crc);
- unsigned char* buf = new unsigned char[ 6 + len + 2 ];
+ unsigned char* buf = new unsigned char[6 + len + 2];
memcpy(buf, header, 6);
memcpy(buf + 6, data, len);
- memcpy(buf + 6 + len, (unsigned char*) &crc, 2);
+ memcpy(buf + 6 + len, (unsigned char*)&crc, 2);
res = write(buf, 6 + len + 2);
@@ -320,10 +339,8 @@ int AbstractInterface::send(unsigned char id, unsigned int len, unsigned char* d
return len + 8;
}
-
-
-
-std::ostream& operator<<(std::ostream& strm, const AbstractInterface& a)
+std::ostream&
+operator<<(std::ostream& strm, const AbstractInterface& a)
{
return strm << a.toString();
}
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.h b/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.h
index 7170f6b4dfeefdd9e09899dba08552f75466a39d..0e839818379e97a8b15d4c0481ad749f26d9947a 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/AbstractInterface.h
@@ -25,23 +25,26 @@
#include <memory>
#include <string>
-#include "Types.h"
+
#include "BinaryLogger.h"
+#include "Types.h"
-#define MSG_PREAMBLE_BYTE 0xaa
-#define MSG_PREAMBLE_LEN 3
+#define MSG_PREAMBLE_BYTE 0xaa
+#define MSG_PREAMBLE_LEN 3
// Combine bytes to different types
-#define make_short( lowbyte, highbyte ) ( (unsigned short)lowbyte | ( (unsigned short)highbyte << 8 ) )
-#define make_signed_short( lowbyte, highbyte ) ( (signed short) ( (unsigned short) lowbyte | ( (unsigned short) highbyte << 8 ) ) )
-#define make_int( lowbyte, mid1, mid2, highbyte ) ( (unsigned int) lowbyte | ( (unsigned int) mid1 << 8 ) | ( (unsigned int) mid2 << 16 ) | ( (unsigned int) highbyte << 24 ) )
-#define make_float( result, byteptr ) memcpy( &result, byteptr, sizeof( float ) )
+#define make_short(lowbyte, highbyte) ((unsigned short)lowbyte | ((unsigned short)highbyte << 8))
+#define make_signed_short(lowbyte, highbyte) \
+ ((signed short)((unsigned short)lowbyte | ((unsigned short)highbyte << 8)))
+#define make_int(lowbyte, mid1, mid2, highbyte) \
+ ((unsigned int)lowbyte | ((unsigned int)mid1 << 8) | ((unsigned int)mid2 << 16) | \
+ ((unsigned int)highbyte << 24))
+#define make_float(result, byteptr) memcpy(&result, byteptr, sizeof(float))
// Byte access
-#define hi( x ) (unsigned char) ( ((x) >> 8) & 0xff ) // Returns the upper byte of the passed short
-#define lo( x ) (unsigned char) ( (x) & 0xff ) // Returns the lower byte of the passed short
-
+#define hi(x) (unsigned char)(((x) >> 8) & 0xff) // Returns the upper byte of the passed short
+#define lo(x) (unsigned char)((x) & 0xff) // Returns the lower byte of the passed short
struct Response;
@@ -56,7 +59,8 @@ public:
int read(unsigned char* buf, unsigned int len);
int write(unsigned char* buf, unsigned int len);
- bool IsConnected() const
+ bool
+ IsConnected() const
{
return connected;
}
@@ -85,5 +89,3 @@ private:
};
std::ostream& operator<<(std::ostream& strm, const AbstractInterface& a);
-
-
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/BinaryLogger.cpp b/source/RobotAPI/drivers/WeissHapticSensor/BinaryLogger.cpp
index 7bc2d3ddb8716406450842ff0df6b22d8951f804..a4f9b0f542b3e9b3c8e5943916221eb8a7f3e146 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/BinaryLogger.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/BinaryLogger.cpp
@@ -22,6 +22,7 @@
* GNU General Public License
*/
#include "BinaryLogger.h"
+
#include <boost/format.hpp>
BinaryLogger::BinaryLogger(std::string filename)
@@ -34,7 +35,8 @@ BinaryLogger::~BinaryLogger()
log.close();
}
-void BinaryLogger::logRead(unsigned char* buf, unsigned int len)
+void
+BinaryLogger::logRead(unsigned char* buf, unsigned int len)
{
log << "READ";
@@ -47,7 +49,8 @@ void BinaryLogger::logRead(unsigned char* buf, unsigned int len)
log.flush();
}
-void BinaryLogger::logWrite(unsigned char* buf, unsigned int len)
+void
+BinaryLogger::logWrite(unsigned char* buf, unsigned int len)
{
log << "WRITE";
@@ -60,7 +63,8 @@ void BinaryLogger::logWrite(unsigned char* buf, unsigned int len)
log.flush();
}
-void BinaryLogger::logText(std::string message)
+void
+BinaryLogger::logText(std::string message)
{
log << message << std::endl;
log.flush();
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/BinaryLogger.h b/source/RobotAPI/drivers/WeissHapticSensor/BinaryLogger.h
index d2a05bc19c54f0022ecde8b6d78b9071a51e0f4e..56c74b8f60a999a8ca3250b9e5cbd78e3db19458 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/BinaryLogger.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/BinaryLogger.h
@@ -38,4 +38,3 @@ public:
private:
std::ofstream log;
};
-
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.cpp b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.cpp
index b0a8998f21843b5917f68b4b306aee41f84fe513..af5fee693a320e4d30142f620a1c975700db1dba 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.cpp
@@ -33,12 +33,14 @@ CalibrationHelper::CalibrationHelper(int rows, int cols, float noiseThreshold)
this->noiseThreshold = noiseThreshold;
}
-void CalibrationHelper::addNoiseSample(Eigen::MatrixXf data)
+void
+CalibrationHelper::addNoiseSample(Eigen::MatrixXf data)
{
this->noiseSamples.push_back(data);
}
-bool CalibrationHelper::addMaxValueSample(Eigen::MatrixXf data)
+bool
+CalibrationHelper::addMaxValueSample(Eigen::MatrixXf data)
{
if (data.maxCoeff() <= noiseThreshold)
{
@@ -51,27 +53,33 @@ bool CalibrationHelper::addMaxValueSample(Eigen::MatrixXf data)
}
}
-CalibrationInfo CalibrationHelper::getCalibrationInfo(float calibratedMinimum, float calibratedMaximum)
+CalibrationInfo
+CalibrationHelper::getCalibrationInfo(float calibratedMinimum, float calibratedMaximum)
{
- return CalibrationInfo(getMatrixAverage(noiseSamples), maximumValues, calibratedMinimum, calibratedMaximum);
+ return CalibrationInfo(
+ getMatrixAverage(noiseSamples), maximumValues, calibratedMinimum, calibratedMaximum);
}
-bool CalibrationHelper::checkMaximumValueThreshold(float threshold)
+bool
+CalibrationHelper::checkMaximumValueThreshold(float threshold)
{
return this->maximumValues.minCoeff() >= threshold;
}
-Eigen::MatrixXf CalibrationHelper::getMaximumValues()
+Eigen::MatrixXf
+CalibrationHelper::getMaximumValues()
{
return this->maximumValues;
}
-int CalibrationHelper::getNoiseSampleCount()
+int
+CalibrationHelper::getNoiseSampleCount()
{
return this->noiseSamples.size();
}
-Eigen::MatrixXf CalibrationHelper::getMatrixAverage(std::vector<Eigen::MatrixXf> samples)
+Eigen::MatrixXf
+CalibrationHelper::getMatrixAverage(std::vector<Eigen::MatrixXf> samples)
{
if (samples.size() == 0)
{
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.h b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.h
index 019f6cdf73ab9b32b7f0382b0ea0f0e660d13211..c6db38091976be13c114bdc892c80f8742c09b3a 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationHelper.h
@@ -24,6 +24,7 @@
#pragma once
#include <vector>
+
#include <Eigen/Core>
#include "CalibrationInfo.h"
@@ -53,5 +54,4 @@ namespace armarx
Eigen::MatrixXf getMatrixAverage(std::vector<Eigen::MatrixXf> samples);
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.cpp b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.cpp
index 4a9503c821a08810e9dac16cf06a10fe0a146016..9410eef3ea5e2d792b2a43e56af54cfea61c2d1f 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.cpp
@@ -25,7 +25,10 @@
using namespace armarx;
-CalibrationInfo::CalibrationInfo(Eigen::MatrixXf averageNoiseValues, Eigen::MatrixXf maximumValues, float minValue, float maxValue)
+CalibrationInfo::CalibrationInfo(Eigen::MatrixXf averageNoiseValues,
+ Eigen::MatrixXf maximumValues,
+ float minValue,
+ float maxValue)
{
this->calibratedMinimum = minValue;
this->calibratedMaximum = maxValue;
@@ -33,7 +36,13 @@ CalibrationInfo::CalibrationInfo(Eigen::MatrixXf averageNoiseValues, Eigen::Matr
this->maximumValues = maximumValues;
}
-Eigen::MatrixXf CalibrationInfo::applyCalibration(Eigen::MatrixXf rawData)
+Eigen::MatrixXf
+CalibrationInfo::applyCalibration(Eigen::MatrixXf rawData)
{
- return ((rawData - averageNoiseValues).cwiseQuotient(maximumValues - averageNoiseValues).array() * (calibratedMaximum - calibratedMinimum) + calibratedMinimum).matrix();
+ return ((rawData - averageNoiseValues)
+ .cwiseQuotient(maximumValues - averageNoiseValues)
+ .array() *
+ (calibratedMaximum - calibratedMinimum) +
+ calibratedMinimum)
+ .matrix();
}
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.h b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.h
index 8f3874cdfc999e7a4cdac7bb9a769e49e8ab006e..f21b185ba9d295d94cbe50099bda63d488765618 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/CalibrationInfo.h
@@ -24,12 +24,16 @@
#pragma once
#include <Eigen/Core>
+
namespace armarx
{
class CalibrationInfo
{
public:
- CalibrationInfo(Eigen::MatrixXf averageNoiseValues, Eigen::MatrixXf maximumValues, float calibratedMinimum, float calibratedMaximum);
+ CalibrationInfo(Eigen::MatrixXf averageNoiseValues,
+ Eigen::MatrixXf maximumValues,
+ float calibratedMinimum,
+ float calibratedMaximum);
Eigen::MatrixXf applyCalibration(Eigen::MatrixXf rawData);
@@ -40,5 +44,4 @@ namespace armarx
float calibratedMaximum;
float calibratedMinimum;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/Checksum.cpp b/source/RobotAPI/drivers/WeissHapticSensor/Checksum.cpp
index 73a47996fe11afe9a40c8c4951d52e4ed9df58dd..45a647906164798ddc41368d9d20853a5ee91f3f 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/Checksum.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/Checksum.cpp
@@ -31,41 +31,29 @@
* corresponding to x^16 + x^12 + x^5 + 1
*/
-const unsigned short Checksum::CRC_TABLE_CCITT16[256] =
-{
- 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
- 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
- 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
- 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
- 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
- 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
- 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
- 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
- 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
- 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
- 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
- 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
- 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
- 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
- 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
- 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
- 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
- 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
- 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
- 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
- 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
- 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
- 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
- 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
- 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
- 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
- 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
- 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
- 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
- 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
- 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
- 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
-};
+const unsigned short Checksum::CRC_TABLE_CCITT16[256] = {
+ 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, 0xa14a, 0xb16b,
+ 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
+ 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401,
+ 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
+ 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738,
+ 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
+ 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 0x5af5, 0x4ad4, 0x7ab7, 0x6a96,
+ 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
+ 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd,
+ 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
+ 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, 0xa1eb,
+ 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
+ 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2,
+ 0x4235, 0x5214, 0x6277, 0x7256, 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
+ 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8,
+ 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
+ 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, 0x6827,
+ 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
+ 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d,
+ 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
+ 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74,
+ 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
/**
* Calculates the CRC16 checksum of an array by using a table.
@@ -83,20 +71,20 @@ const unsigned short Checksum::CRC_TABLE_CCITT16[256] =
* @return CRC16 checksum
*/
-unsigned short Checksum::Update_crc16(unsigned char* data, unsigned int size, unsigned short crc)
+unsigned short
+Checksum::Update_crc16(unsigned char* data, unsigned int size, unsigned short crc)
{
unsigned long c;
/* process each byte prior to checksum field */
for (c = 0; c < size; c++)
{
- crc = CRC_TABLE_CCITT16[(crc ^ * (data ++)) & 0x00FF ] ^ (crc >> 8);
+ crc = CRC_TABLE_CCITT16[(crc ^ *(data++)) & 0x00FF] ^ (crc >> 8);
}
return crc;
}
-
/**
* Calculates the CRC16 checksum of an array by using a table.
* The crc16 polynomial is 0x1021 ( x^16 + x^12 + x^5 + 1 ).
@@ -112,8 +100,8 @@ unsigned short Checksum::Update_crc16(unsigned char* data, unsigned int size, un
* @return CRC16 checksum
*/
-unsigned short Checksum::Crc16(unsigned char* data, unsigned int size)
+unsigned short
+Checksum::Crc16(unsigned char* data, unsigned int size)
{
return (Update_crc16(data, size, 0xffff));
}
-
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/Checksum.h b/source/RobotAPI/drivers/WeissHapticSensor/Checksum.h
index fefd7838de70c65ea979ca420835a445fc4ac1a9..34c9ff140685be40108b1c36b66e553d5bc184f9 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/Checksum.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/Checksum.h
@@ -32,4 +32,3 @@ public:
private:
static const unsigned short CRC_TABLE_CCITT16[256];
};
-
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/Response.h b/source/RobotAPI/drivers/WeissHapticSensor/Response.h
index 3e03fe1907f09124e04683f6366cfb7e715a1579..fa7438edcab2d97a658dfaa357eeda5c508511a0 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/Response.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/Response.h
@@ -23,46 +23,63 @@
*/
#pragma once
-#include "Types.h"
#include "TransmissionException.h"
+#include "Types.h"
//#include <strstream>
#include <stdexcept>
-#include <boost/format.hpp>
#include <vector>
+
+#include <boost/format.hpp>
+
#include <ArmarXCore/core/logging/Logging.h>
struct Response
{
public:
- Response(int res, unsigned char cmdId, status_t status, const std::vector<unsigned char>& data, unsigned int len)
- : res(res), cmdId(cmdId), status(status), data(data), len(len) {}
+ Response(int res,
+ unsigned char cmdId,
+ status_t status,
+ const std::vector<unsigned char>& data,
+ unsigned int len) :
+ res(res), cmdId(cmdId), status(status), data(data), len(len)
+ {
+ }
- unsigned int getUInt(int index)
+ unsigned int
+ getUInt(int index)
{
- return (unsigned int)data[index] | ((unsigned int)data[index + 1] << 8) | ((unsigned int)data[index + 2] << 16) | ((unsigned int)data[index + 3] << 24);
+ return (unsigned int)data[index] | ((unsigned int)data[index + 1] << 8) |
+ ((unsigned int)data[index + 2] << 16) | ((unsigned int)data[index + 3] << 24);
}
- unsigned short getShort(int index)
+ unsigned short
+ getShort(int index)
{
return (unsigned short)data[index] | ((unsigned short)data[index + 1] << 8);
}
- unsigned char getByte(int index)
+
+ unsigned char
+ getByte(int index)
{
return data[index];
}
- void ensureMinLength(int len)
+ void
+ ensureMinLength(int len)
{
if (res < len)
{
//std::strstream strStream;
//strStream << "Response length is too short, should be = " << len << " (is " << res << ")";
//throw std::runtime_error(strStream.str());
- throw TransmissionException(str(boost::format("Response length is too short, should be = %1% (is %2%)") % len % res));
+ throw TransmissionException(
+ str(boost::format("Response length is too short, should be = %1% (is %2%)") % len %
+ res));
}
}
- void ensureSuccess()
+ void
+ ensureSuccess()
{
if (status != E_SUCCESS)
{
@@ -78,7 +95,9 @@ public:
}
ARMARX_ERROR_S << ss.str();
- throw TransmissionException(str(boost::format("Command not successful: %1% (0x%2$02X)") % status_to_str(status) % status));
+ throw TransmissionException(
+ str(boost::format("Command not successful: %1% (0x%2$02X)") %
+ status_to_str(status) % status));
}
}
@@ -88,7 +107,8 @@ public:
std::vector<unsigned char> data;
unsigned int len;
- static const char* status_to_str(status_t status)
+ static const char*
+ status_to_str(status_t status)
{
switch (status)
{
@@ -190,4 +210,3 @@ public:
}
}
};
-
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
index b0a2f0b3705d9c00c88fff8e0470651f8f3f2f43..c27af4a3b01b4d7020fbbb958e04acad3e8a9f8f 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.cpp
@@ -23,44 +23,47 @@
*/
#include "SerialInterface.h"
-#include <iostream>
+#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
+
+#include <iostream>
+
#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
#include <termios.h>
-#include <errno.h>
#include <unistd.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <sys/ioctl.h>
-#include <iostream>
+
#include <boost/format.hpp>
-static inline tcflag_t __bitrate_to_flag(unsigned int bitrate)
+static inline tcflag_t
+__bitrate_to_flag(unsigned int bitrate)
{
switch (bitrate)
{
- case 1200:
- return B1200;
+ case 1200:
+ return B1200;
- case 2400:
- return B2400;
+ case 2400:
+ return B2400;
- case 4800:
- return B4800;
+ case 4800:
+ return B4800;
- case 9600:
- return B9600;
+ case 9600:
+ return B9600;
- case 19200:
- return B19200;
+ case 19200:
+ return B19200;
- case 38400:
- return B38400;
+ case 38400:
+ return B38400;
- case 57600:
- return B57600;
+ case 57600:
+ return B57600;
case 115200:
return B115200;
@@ -76,7 +79,6 @@ static inline tcflag_t __bitrate_to_flag(unsigned int bitrate)
}
}
-
SerialInterface::SerialInterface(const char* device, unsigned int bitrate)
{
this->device = device;
@@ -86,10 +88,10 @@ SerialInterface::SerialInterface(const char* device, unsigned int bitrate)
SerialInterface::~SerialInterface()
{
-
}
-int SerialInterface::open()
+int
+SerialInterface::open()
{
// Convert bitrate to flag
tcflag_t bitrate = __bitrate_to_flag(this->bitrate);
@@ -117,11 +119,11 @@ int SerialInterface::open()
}
-
// Check if device is a terminal device
if (!isatty(fd))
{
- fprintf(stderr, "Device '%s' is not a terminal device (errno: %s)!\n", device, strerror(errno));
+ fprintf(
+ stderr, "Device '%s' is not a terminal device (errno: %s)!\n", device, strerror(errno));
::close(fd);
return -1;
}
@@ -129,30 +131,29 @@ int SerialInterface::open()
struct termios settings;
// Set input flags
- settings.c_iflag = IGNBRK // Ignore BREAKS on Input
- | IGNPAR; // No Parity
+ settings.c_iflag = IGNBRK // Ignore BREAKS on Input
+ | IGNPAR; // No Parity
// ICRNL: map CR to NL (otherwise a CR input on the other computer will not terminate input)
// Set output flags
- settings.c_oflag = 0; // Raw output
+ settings.c_oflag = 0; // Raw output
// Set controlflags
- settings.c_cflag = bitrate
- | CS8 // 8 bits per byte
- | CSTOPB // Stop bit
- | CREAD // characters may be read
- | CLOCAL; // ignore modem state, local connection
+ settings.c_cflag = bitrate | CS8 // 8 bits per byte
+ | CSTOPB // Stop bit
+ | CREAD // characters may be read
+ | CLOCAL; // ignore modem state, local connection
// Set local flags
- settings.c_lflag = 0; // Other option: ICANON = enable canonical input
+ settings.c_lflag = 0; // Other option: ICANON = enable canonical input
// Set maximum wait time on input - cf. Linux Serial Programming HowTo, non-canonical mode
// http://tldp.org/HOWTO/Serial-Programming-HOWTO/x115.html
- settings.c_cc[VTIME] = 10; // 0 means timer is not uses
+ settings.c_cc[VTIME] = 10; // 0 means timer is not uses
// Set minimum bytes to read
- settings.c_cc[VMIN] = 0; // 1 means wait until at least 1 character is received
+ settings.c_cc[VMIN] = 0; // 1 means wait until at least 1 character is received
// Now clean the modem line and activate the settings for the port
tcflush(fd, TCIFLUSH);
@@ -164,7 +165,8 @@ int SerialInterface::open()
return 0;
}
-void SerialInterface::close()
+void
+SerialInterface::close()
{
if (connected)
{
@@ -174,7 +176,8 @@ void SerialInterface::close()
connected = false;
}
-int SerialInterface::readInternal(unsigned char* buf, unsigned int len)
+int
+SerialInterface::readInternal(unsigned char* buf, unsigned int len)
{
int res;
@@ -186,10 +189,10 @@ int SerialInterface::readInternal(unsigned char* buf, unsigned int len)
}
return res;
-
}
-int SerialInterface::blockingReadAll(unsigned char* buf, unsigned int len)
+int
+SerialInterface::blockingReadAll(unsigned char* buf, unsigned int len)
{
int dataToRead = len;
@@ -219,14 +222,15 @@ int SerialInterface::blockingReadAll(unsigned char* buf, unsigned int len)
}
}
-int SerialInterface::writeInternal(unsigned char* buf, unsigned int len)
+int
+SerialInterface::writeInternal(unsigned char* buf, unsigned int len)
{
- return (::write(fd, (void*) buf, len));
+ return (::write(fd, (void*)buf, len));
}
-
-std::string SerialInterface::toString() const
+std::string
+SerialInterface::toString() const
{
- return str(boost::format("SerialInterface(connected=%1%, device=%2%, bitrate=%3%, fd=%4%)")
- % connected % device % bitrate % fd);
+ return str(boost::format("SerialInterface(connected=%1%, device=%2%, bitrate=%3%, fd=%4%)") %
+ connected % device % bitrate % fd);
}
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.h b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.h
index 883e3a96eece51ee8bcce3af569d926f149a6c38..53a24be11188a3e51e277cec63290c10a179f424 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/SerialInterface.h
@@ -23,9 +23,9 @@
*/
#pragma once
-#include "AbstractInterface.h"
#include <iostream>
+#include "AbstractInterface.h"
class SerialInterface : public AbstractInterface
{
@@ -49,4 +49,3 @@ private:
int blockingReadAll(unsigned char* buf, unsigned int len);
};
-
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.cpp b/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.cpp
index d93904c59a4dc1d22bf391a29391ed98f425fd4e..3b4e37c8c1cc9f6ca48044b993a59018e512ba84 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.cpp
@@ -22,11 +22,14 @@
* GNU General Public License
*/
#include "TactileSensor.h"
+
+#include <stdio.h>
+
+#include <string>
+
#include "AbstractInterface.h"
#include "Response.h"
#include "Types.h"
-#include <string>
-#include <stdio.h>
TactileSensor::TactileSensor(std::shared_ptr<AbstractInterface> interface)
{
@@ -38,7 +41,8 @@ TactileSensor::~TactileSensor()
// TODO Auto-generated destructor stub
}
-tac_matrix_info_t TactileSensor::getMatrixInformation()
+tac_matrix_info_t
+TactileSensor::getMatrixInformation()
{
Response response = interface->submitCmd(0x30, nullptr, 0, false);
response.ensureMinLength(12);
@@ -46,23 +50,29 @@ tac_matrix_info_t TactileSensor::getMatrixInformation()
tac_matrix_info_t matrix_info;
- matrix_info.res_x = response.getShort(2);
- matrix_info.res_y = response.getShort(4);
- matrix_info.cell_width = response.getShort(6);
+ matrix_info.res_x = response.getShort(2);
+ matrix_info.res_y = response.getShort(4);
+ matrix_info.cell_width = response.getShort(6);
matrix_info.cell_height = response.getShort(8);
- matrix_info.fullscale = response.getShort(10);
+ matrix_info.fullscale = response.getShort(10);
//delete response;
return matrix_info;
}
-void TactileSensor::printMatrixInfo(tac_matrix_info_t* mi)
+void
+TactileSensor::printMatrixInfo(tac_matrix_info_t* mi)
{
printf("res_x = %d, res_y = %d, cell_width = %d, cell_height = %d, fullscale = %X\n",
- mi->res_x, mi->res_y, mi->cell_width, mi->cell_height, mi->fullscale);
+ mi->res_x,
+ mi->res_y,
+ mi->cell_width,
+ mi->cell_height,
+ mi->fullscale);
}
-void TactileSensor::printMatrix(short* matrix, int width, int height)
+void
+TactileSensor::printMatrix(short* matrix, int width, int height)
{
int x, y;
@@ -79,14 +89,17 @@ void TactileSensor::printMatrix(short* matrix, int width, int height)
}
}
-FrameData TactileSensor::readSingleFrame()
+FrameData
+TactileSensor::readSingleFrame()
{
unsigned char payload[1];
payload[0] = 0x00; // FLAGS = 0
Response response = interface->submitCmd(0x20, payload, sizeof(payload), false);
return getFrameData(&response);
}
-PeriodicFrameData TactileSensor::receicePeriodicFrame()
+
+PeriodicFrameData
+TactileSensor::receicePeriodicFrame()
{
Response response = interface->receiveWithoutChecks();
@@ -101,11 +114,14 @@ PeriodicFrameData TactileSensor::receicePeriodicFrame()
}
else
{
- throw TransmissionException(str(boost::format("Response ID (%02X) does not match submitted command ID (%02X)") % (int)response.cmdId % (int)0x00));
+ throw TransmissionException(
+ str(boost::format("Response ID (%02X) does not match submitted command ID (%02X)") %
+ (int)response.cmdId % (int)0x00));
}
}
-PeriodicFrameData TactileSensor::getPeriodicFrameData(Response* response)
+PeriodicFrameData
+TactileSensor::getPeriodicFrameData(Response* response)
{
response->ensureMinLength(7);
@@ -114,7 +130,7 @@ PeriodicFrameData TactileSensor::getPeriodicFrameData(Response* response)
int count = (response->len - offset) / 2;
int i;
- std::shared_ptr<std::vector<short> > data;
+ std::shared_ptr<std::vector<short>> data;
data.reset(new std::vector<short>(count, 0));
//short* data = new short[ count ];
@@ -127,7 +143,8 @@ PeriodicFrameData TactileSensor::getPeriodicFrameData(Response* response)
return PeriodicFrameData(data, count, timestamp);
}
-FrameData TactileSensor::getFrameData(Response* response)
+FrameData
+TactileSensor::getFrameData(Response* response)
{
response->ensureMinLength(7);
response->ensureSuccess();
@@ -136,7 +153,7 @@ FrameData TactileSensor::getFrameData(Response* response)
int count = (response->len - offset) / 2;
int i;
- std::shared_ptr<std::vector<short> > data;
+ std::shared_ptr<std::vector<short>> data;
data.reset(new std::vector<short>(count, 0));
for (i = 0; i < count; i++)
@@ -148,7 +165,8 @@ FrameData TactileSensor::getFrameData(Response* response)
return FrameData(data, count);
}
-void TactileSensor::startPeriodicFrameAcquisition(unsigned short delay_ms)
+void
+TactileSensor::startPeriodicFrameAcquisition(unsigned short delay_ms)
{
unsigned char payload[3];
payload[0] = 0x00; // FLAGS = 0
@@ -156,7 +174,9 @@ void TactileSensor::startPeriodicFrameAcquisition(unsigned short delay_ms)
payload[2] = (delay_ms >> 8) & 0xFF;
interface->fireAndForgetCmd(0x21, payload, sizeof(payload), false);
}
-void TactileSensor::stopPeriodicFrameAcquisition(void)
+
+void
+TactileSensor::stopPeriodicFrameAcquisition(void)
{
while (1)
{
@@ -173,22 +193,34 @@ void TactileSensor::stopPeriodicFrameAcquisition(void)
}
else
{
- std::cout << boost::format("stopPeriodicFrameAcquisition :: Discarding Response with ID 0x%02X") % (int)response.cmdId << std::endl;
+ std::cout
+ << boost::format(
+ "stopPeriodicFrameAcquisition :: Discarding Response with ID 0x%02X") %
+ (int)response.cmdId
+ << std::endl;
}
waitCount--;
}
}
}
-void TactileSensor::tareSensorMatrix(unsigned char operation)
+
+void
+TactileSensor::tareSensorMatrix(unsigned char operation)
{
unsigned char payload[1];
- payload[0] = operation; // OPERATION: 0 = un-tare the sensor matrix using the currently set threshold value, 1 = tare the sensor matrix
+ payload[0] =
+ operation; // OPERATION: 0 = un-tare the sensor matrix using the currently set threshold value, 1 = tare the sensor matrix
Response response = interface->submitCmd(0x23, payload, sizeof(payload), false);
response.ensureMinLength(2);
response.ensureSuccess();
}
-void TactileSensor::setAquisitionWindow(unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2)
+
+void
+TactileSensor::setAquisitionWindow(unsigned char x1,
+ unsigned char y1,
+ unsigned char x2,
+ unsigned char y2)
{
unsigned char payload[4];
payload[0] = x1;
@@ -199,15 +231,21 @@ void TactileSensor::setAquisitionWindow(unsigned char x1, unsigned char y1, unsi
response.ensureMinLength(2);
response.ensureSuccess();
}
-int TactileSensor::setAdvanvedAcquisitionMask(char* mask)
+
+int
+TactileSensor::setAdvanvedAcquisitionMask(char* mask)
{
return 0;
}
-int TactileSensor::getAcquisitionMask(char** mask, int* mask_len)
+
+int
+TactileSensor::getAcquisitionMask(char** mask, int* mask_len)
{
return 0;
}
-void TactileSensor::setThreshold(short threshold)
+
+void
+TactileSensor::setThreshold(short threshold)
{
unsigned char payload[2];
payload[0] = threshold & 0xFF;
@@ -216,14 +254,18 @@ void TactileSensor::setThreshold(short threshold)
response.ensureMinLength(2);
response.ensureSuccess();
}
-unsigned short TactileSensor::getThreshold()
+
+unsigned short
+TactileSensor::getThreshold()
{
Response response = interface->submitCmd(0x35, nullptr, 0, false);
response.ensureMinLength(2);
response.ensureSuccess();
return response.getShort(2);
}
-void TactileSensor::setFrontEndGain(unsigned char gain)
+
+void
+TactileSensor::setFrontEndGain(unsigned char gain)
{
/*
* Adjust the pressure sensitivity of a matrix by setting the gain of the Analog Front-End. The gain can
@@ -236,7 +278,9 @@ void TactileSensor::setFrontEndGain(unsigned char gain)
response.ensureMinLength(2);
response.ensureSuccess();
}
-unsigned char TactileSensor::getFrontEndGain()
+
+unsigned char
+TactileSensor::getFrontEndGain()
{
/*
* Get the currently set analog front-end gain value. The gain is as an integer value ranging from 0 to
@@ -248,7 +292,9 @@ unsigned char TactileSensor::getFrontEndGain()
unsigned char gain = response.getByte(2);
return gain;
}
-std::string TactileSensor::getSensorType()
+
+std::string
+TactileSensor::getSensorType()
{
/*
* Return a string containing the sensor type.
@@ -259,14 +305,18 @@ std::string TactileSensor::getSensorType()
std::string type = std::string((char*)response.data.data() + 2, response.len - 2);
return type;
}
-float TactileSensor::readDeviceTemperature()
+
+float
+TactileSensor::readDeviceTemperature()
{
Response response = interface->submitCmd(0x46, nullptr, 0, false);
response.ensureMinLength(2);
short value = (short)response.getShort(2);
return value * 0.1f;
}
-tac_system_information_t TactileSensor::getSystemInformation()
+
+tac_system_information_t
+TactileSensor::getSystemInformation()
{
Response response = interface->submitCmd(0x50, nullptr, 0, false);
response.ensureMinLength(9);
@@ -278,23 +328,31 @@ tac_system_information_t TactileSensor::getSystemInformation()
si.sn = response.getShort(6);
return si;
}
-void TactileSensor::printSystemInformation(tac_system_information_t si)
+
+void
+TactileSensor::printSystemInformation(tac_system_information_t si)
{
int v1 = (si.fw_version & 0xF000) >> 12;
int v2 = (si.fw_version & 0x0F00) >> 8;
int v3 = (si.fw_version & 0x00F0) >> 4;
int v4 = (si.fw_version & 0x000F) >> 0;
- std::cout << boost::format("System Type=%1%, Hardware Revision=%2%, Firmware Version=%3%.%4%.%5%.%6% (0x%7$04X), Serial Number=%8%")
- % (int)si.type % (int)si.hw_rev % v1 % v2 % v3 % v4 % si.fw_version % si.sn << std::endl;
+ std::cout << boost::format("System Type=%1%, Hardware Revision=%2%, Firmware "
+ "Version=%3%.%4%.%5%.%6% (0x%7$04X), Serial Number=%8%") %
+ (int)si.type % (int)si.hw_rev % v1 % v2 % v3 % v4 % si.fw_version % si.sn
+ << std::endl;
}
-void TactileSensor::setDeviceTag(std::string tag)
+
+void
+TactileSensor::setDeviceTag(std::string tag)
{
unsigned char* payload = (unsigned char*)tag.c_str();
Response response = interface->submitCmd(0x51, payload, tag.length(), false);
response.ensureMinLength(2);
response.ensureSuccess();
}
-std::string TactileSensor::getDeviceTag()
+
+std::string
+TactileSensor::getDeviceTag()
{
Response response = interface->submitCmd(0x52, nullptr, 0, false);
response.ensureMinLength(2);
@@ -303,7 +361,8 @@ std::string TactileSensor::getDeviceTag()
return tag;
}
-bool TactileSensor::tryGetDeviceTag(std::string& tag)
+bool
+TactileSensor::tryGetDeviceTag(std::string& tag)
{
Response response = interface->submitCmd(0x52, nullptr, 0, false);
response.ensureMinLength(2);
@@ -317,17 +376,21 @@ bool TactileSensor::tryGetDeviceTag(std::string& tag)
tag = std::string((char*)response.data.data() + 2, response.len - 2);
return true;
}
-int TactileSensor::loop(char* data, int data_len)
+
+int
+TactileSensor::loop(char* data, int data_len)
{
return 0;
}
-std::string TactileSensor::getInterfaceInfo()
+std::string
+TactileSensor::getInterfaceInfo()
{
return interface->toString();
}
-std::ostream& operator<<(std::ostream& strm, const TactileSensor& a)
+std::ostream&
+operator<<(std::ostream& strm, const TactileSensor& a)
{
return strm << a.interface;
}
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.h b/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.h
index c48bc871b8b879a2d2a0c57b6845266a5e7906a9..7242aa9ce49e0007ed08ddc4225a44a82c9cd672 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/TactileSensor.h
@@ -25,20 +25,24 @@
#include "AbstractInterface.h"
-#define extract_short( array, index ) ( (unsigned short)array[index] | ( (unsigned short)array[index + 1] << 8 ) )
-#define TAC_CHECK_RES( res, expected, resp ) { \
- if ( res < expected ) { \
- dbgPrint( "Response length is too short, should be = %d (is %d)\n", expected, res ); \
- if ( res > 0 ) free( resp ); \
- return -1; \
- } \
- status_t status = cmd_get_response_status( resp ); \
- if ( status != E_SUCCESS ) \
- { \
- dbgPrint( "Command not successful: %s\n", status_to_str( status ) ); \
- free( resp ); \
- return -1; \
- } \
+#define extract_short(array, index) \
+ ((unsigned short)array[index] | ((unsigned short)array[index + 1] << 8))
+#define TAC_CHECK_RES(res, expected, resp) \
+ { \
+ if (res < expected) \
+ { \
+ dbgPrint("Response length is too short, should be = %d (is %d)\n", expected, res); \
+ if (res > 0) \
+ free(resp); \
+ return -1; \
+ } \
+ status_t status = cmd_get_response_status(resp); \
+ if (status != E_SUCCESS) \
+ { \
+ dbgPrint("Command not successful: %s\n", status_to_str(status)); \
+ free(resp); \
+ return -1; \
+ } \
}
typedef struct
@@ -54,26 +58,31 @@ typedef struct
{
unsigned char type; // System Type: 0 - unknown, 4 - WTS Tactile Sensor Module
unsigned char hw_rev; // Hardware Revision
- unsigned short fw_version; // Firmware Version: D15...12: major version, D11...8: minor version 1, D7..4 minor version 2, D3..0: 0 for release version, 1..15 for release candidate versions
+ unsigned short
+ fw_version; // Firmware Version: D15...12: major version, D11...8: minor version 1, D7..4 minor version 2, D3..0: 0 for release version, 1..15 for release candidate versions
unsigned short sn; // Serial Number of the device
} tac_system_information_t;
struct FrameData
{
public:
- FrameData(std::shared_ptr<std::vector<short> > data, int count)
- : data(data), count(count)
- {}
- std::shared_ptr<std::vector<short> > data;
+ FrameData(std::shared_ptr<std::vector<short>> data, int count) : data(data), count(count)
+ {
+ }
+
+ std::shared_ptr<std::vector<short>> data;
int count;
};
+
struct PeriodicFrameData
{
public:
- PeriodicFrameData(std::shared_ptr<std::vector<short> > data, int count, unsigned int timestamp)
- : data(data), count(count), timestamp(timestamp)
- {}
- std::shared_ptr<std::vector<short> > data;
+ PeriodicFrameData(std::shared_ptr<std::vector<short>> data, int count, unsigned int timestamp) :
+ data(data), count(count), timestamp(timestamp)
+ {
+ }
+
+ std::shared_ptr<std::vector<short>> data;
int count;
unsigned int timestamp;
};
@@ -93,7 +102,8 @@ public:
void stopPeriodicFrameAcquisition(void);
PeriodicFrameData receicePeriodicFrame();
void tareSensorMatrix(unsigned char operation);
- void setAquisitionWindow(unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2);
+ void
+ setAquisitionWindow(unsigned char x1, unsigned char y1, unsigned char x2, unsigned char y2);
int setAdvanvedAcquisitionMask(char* mask);
int getAcquisitionMask(char** mask, int* mask_len);
void setThreshold(short threshold);
@@ -121,4 +131,3 @@ private:
};
std::ostream& operator<<(std::ostream& strm, const TactileSensor& a);
-
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/TextWriter.cpp b/source/RobotAPI/drivers/WeissHapticSensor/TextWriter.cpp
index f7b735d8aba885bf60240ad8a517c2860e2d5c8b..c52d17400a9046419ceb69b138b225c923b3a2e0 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/TextWriter.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/TextWriter.cpp
@@ -35,7 +35,8 @@ TextWriter::~TextWriter()
file.close();
}
-void TextWriter::writeLine(std::string message)
+void
+TextWriter::writeLine(std::string message)
{
file << message;
file << std::endl;
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/TextWriter.h b/source/RobotAPI/drivers/WeissHapticSensor/TextWriter.h
index 37a03936ef1a7bd500a7c5f82c6761eae9e298eb..92cbbaef5e97105ba0a346f06d9e8f204139a1f5 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/TextWriter.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/TextWriter.h
@@ -38,5 +38,4 @@ namespace armarx
private:
std::ofstream file;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/TransmissionException.h b/source/RobotAPI/drivers/WeissHapticSensor/TransmissionException.h
index b7f033a9aa0d5c40999a95520e1839b4cb04ca95..8ad7d5c72dd2fb13b1d465733e919c8e8f8fad6c 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/TransmissionException.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/TransmissionException.h
@@ -28,16 +28,15 @@
class TransmissionException : public std::runtime_error
{
public:
- TransmissionException(std::string message)
- : runtime_error(message)
- { }
+ TransmissionException(std::string message) : runtime_error(message)
+ {
+ }
};
class ChecksumErrorException : public TransmissionException
{
public:
- ChecksumErrorException(std::string message)
- : TransmissionException(message)
- { }
+ ChecksumErrorException(std::string message) : TransmissionException(message)
+ {
+ }
};
-
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/Types.h b/source/RobotAPI/drivers/WeissHapticSensor/Types.h
index 14a8cadbed7c9bdfaf78568094d6f0708d568cdd..a261f02373e512e85dd8fdf386e1ac1cbee63a1a 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/Types.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/Types.h
@@ -32,42 +32,39 @@ typedef struct
std::vector<unsigned char> data;
} msg_t;
-
typedef enum
{
- E_SUCCESS = 0, // No error
- E_NOT_AVAILABLE, // Device, service or data is not available
- E_NO_SENSOR, // No sensor connected
- E_NOT_INITIALIZED, // The device is not initialized
- E_ALREADY_RUNNING, // Service is already running
- E_FEATURE_NOT_SUPPORTED, // The asked feature is not supported
- E_INCONSISTENT_DATA, // One or more dependent parameters mismatch
- E_TIMEOUT, // Timeout error
- E_READ_ERROR, // Error while reading from a device
- E_WRITE_ERROR, // Error while writing to a device
- E_INSUFFICIENT_RESOURCES, // No memory available
- E_CHECKSUM_ERROR, // Checksum error
- E_NO_PARAM_EXPECTED, // No parameters expected
- E_NOT_ENOUGH_PARAMS, // Not enough parameters
- E_CMD_UNKNOWN, // Unknown command
- E_CMD_FORMAT_ERROR, // Command format error
- E_ACCESS_DENIED, // Access denied
- E_ALREADY_OPEN, // The interface is already open
- E_CMD_FAILED, // Command failed
- E_CMD_ABORTED, // Command aborted
- E_INVALID_HANDLE, // invalid handle
- E_NOT_FOUND, // device not found
- E_NOT_OPEN, // device not open
- E_IO_ERROR, // I/O error
- E_INVALID_PARAMETER, // invalid parameter
- E_INDEX_OUT_OF_BOUNDS, // index out of bounds
- E_CMD_PENDING, // Command was received correctly, but the execution needs more time. If the command was completely processed, another status message is returned indicating the command's result
- E_OVERRUN, // Data overrun
- E_RANGE_ERROR, // Range error
- E_AXIS_BLOCKED, // Axis is blocked
- E_FILE_EXISTS // File already exists
+ E_SUCCESS = 0, // No error
+ E_NOT_AVAILABLE, // Device, service or data is not available
+ E_NO_SENSOR, // No sensor connected
+ E_NOT_INITIALIZED, // The device is not initialized
+ E_ALREADY_RUNNING, // Service is already running
+ E_FEATURE_NOT_SUPPORTED, // The asked feature is not supported
+ E_INCONSISTENT_DATA, // One or more dependent parameters mismatch
+ E_TIMEOUT, // Timeout error
+ E_READ_ERROR, // Error while reading from a device
+ E_WRITE_ERROR, // Error while writing to a device
+ E_INSUFFICIENT_RESOURCES, // No memory available
+ E_CHECKSUM_ERROR, // Checksum error
+ E_NO_PARAM_EXPECTED, // No parameters expected
+ E_NOT_ENOUGH_PARAMS, // Not enough parameters
+ E_CMD_UNKNOWN, // Unknown command
+ E_CMD_FORMAT_ERROR, // Command format error
+ E_ACCESS_DENIED, // Access denied
+ E_ALREADY_OPEN, // The interface is already open
+ E_CMD_FAILED, // Command failed
+ E_CMD_ABORTED, // Command aborted
+ E_INVALID_HANDLE, // invalid handle
+ E_NOT_FOUND, // device not found
+ E_NOT_OPEN, // device not open
+ E_IO_ERROR, // I/O error
+ E_INVALID_PARAMETER, // invalid parameter
+ E_INDEX_OUT_OF_BOUNDS, // index out of bounds
+ E_CMD_PENDING, // Command was received correctly, but the execution needs more time. If the command was completely processed, another status message is returned indicating the command's result
+ E_OVERRUN, // Data overrun
+ E_RANGE_ERROR, // Range error
+ E_AXIS_BLOCKED, // Axis is blocked
+ E_FILE_EXISTS // File already exists
} status_t;
-
const char* status_to_str(status_t status);
-
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
index 128043174d57794f4df610b2dd9d4658898e4d65..f1910fdc2950a1890605d282904506836f316064 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.cpp
@@ -23,23 +23,29 @@
*/
#include "WeissHapticSensor.h"
-#include "TransmissionException.h"
-#include <boost/regex.hpp>
#include <boost/format.hpp>
+#include <boost/regex.hpp>
+
+#include "TransmissionException.h"
using namespace armarx;
-WeissHapticSensor::WeissHapticSensor(std::string device, int minimumReportIntervalMs)
- : device(device), connected(false), setDeviceTagScheduled(false), minimumReportIntervalMs(minimumReportIntervalMs)
+WeissHapticSensor::WeissHapticSensor(std::string device, int minimumReportIntervalMs) :
+ device(device),
+ connected(false),
+ setDeviceTagScheduled(false),
+ minimumReportIntervalMs(minimumReportIntervalMs)
{
- sensorTask = new RunningTask<WeissHapticSensor>(this, &WeissHapticSensor::frameAcquisitionTaskLoop);
+ sensorTask =
+ new RunningTask<WeissHapticSensor>(this, &WeissHapticSensor::frameAcquisitionTaskLoop);
boost::smatch match;
boost::regex_search(device, match, boost::regex("\\w+$"));
this->deviceFileName = match[0];
}
-void WeissHapticSensor::connect()
+void
+WeissHapticSensor::connect()
{
ARMARX_INFO << "Open Serial" << std::endl;
@@ -101,36 +107,42 @@ void WeissHapticSensor::connect()
ARMARX_LOG << device << ": Connect done, Interface=" << sensor->getInterfaceInfo();
}
-void WeissHapticSensor::disconnect()
+void
+WeissHapticSensor::disconnect()
{
connected = false;
sensorTask->stop(true);
}
-void WeissHapticSensor::setListenerPrx(HapticUnitListenerPrx listenerPrx)
+void
+WeissHapticSensor::setListenerPrx(HapticUnitListenerPrx listenerPrx)
{
this->listenerPrx = listenerPrx;
}
-void WeissHapticSensor::startSampling()
+void
+WeissHapticSensor::startSampling()
{
ARMARX_LOG << device << ": startSampling" << std::endl;
sensorTask->start();
}
-std::string WeissHapticSensor::getDeviceName()
+std::string
+WeissHapticSensor::getDeviceName()
{
return device;
}
-void WeissHapticSensor::scheduleSetDeviceTag(std::string tag)
+void
+WeissHapticSensor::scheduleSetDeviceTag(std::string tag)
{
std::unique_lock lock(mutex);
setDeviceTagValue = tag;
setDeviceTagScheduled = true;
}
-void WeissHapticSensor::frameAcquisitionTaskLoop()
+void
+WeissHapticSensor::frameAcquisitionTaskLoop()
{
ARMARX_LOG << device << ": readAndReportSensorValues";
//bool periodic = false;
@@ -141,7 +153,8 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
IceUtil::Time lastFrameTime = IceUtil::Time::now();
- math::SlidingWindowVectorMedian slidingMedian(mi.res_x * mi.res_y, 21); // inter sample dely ~= 3,7ms, 11 samples ~== 40ms delay
+ math::SlidingWindowVectorMedian slidingMedian(
+ mi.res_x * mi.res_y, 21); // inter sample dely ~= 3,7ms, 11 samples ~== 40ms delay
while (!sensorTask->isStopped())
@@ -211,10 +224,12 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
std::unique_lock lock(mutex);
setDeviceTagScheduled = false;
- ARMARX_INFO << "[" << device << "] Stopping periodic frame aquisition to set new device tag";
+ ARMARX_INFO << "[" << device
+ << "] Stopping periodic frame aquisition to set new device tag";
sensor->stopPeriodicFrameAcquisition();
- ARMARX_IMPORTANT << "[" << device << "] Setting new device tag '" << setDeviceTagValue << "'";
+ ARMARX_IMPORTANT << "[" << device << "] Setting new device tag '" << setDeviceTagValue
+ << "'";
sensor->setDeviceTag(setDeviceTagValue);
this->tag = setDeviceTagValue;
@@ -227,11 +242,13 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
sensor->stopPeriodicFrameAcquisition();
}
-void WeissHapticSensor::writeMatrixToJs(MatrixFloatPtr matrix, TimestampVariantPtr timestamp)
+void
+WeissHapticSensor::writeMatrixToJs(MatrixFloatPtr matrix, TimestampVariantPtr timestamp)
{
//std::cout << "writeMatrixToJs" << std::endl;
if (jsWriter != NULL)
{
- jsWriter->writeLine(str(boost::format("%s.data.push([%i, %s]);") % deviceFileName % timestamp->getTimestamp() % matrix->toJsonRowMajor()));
+ jsWriter->writeLine(str(boost::format("%s.data.push([%i, %s]);") % deviceFileName %
+ timestamp->getTimestamp() % matrix->toJsonRowMajor()));
}
}
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
index 169d1cf3690bcc2c53e091ab5343836dc93916cf..ed6091a81eb05d3af5ee6227bfc46291030d157c 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticSensor.h
@@ -23,21 +23,21 @@
*/
#pragma once
-#include "SerialInterface.h"
-#include "TactileSensor.h"
-#include "TextWriter.h"
-
-#include <RobotAPI/interface/units/HapticUnit.h>
-#include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
+#include <mutex>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <mutex>
+#include <RobotAPI/interface/units/HapticUnit.h>
+#include <RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h>
+
+#include "SerialInterface.h"
+#include "TactileSensor.h"
+#include "TextWriter.h"
namespace armarx
{
@@ -79,5 +79,4 @@ namespace armarx
std::mutex mutex;
int minimumReportIntervalMs;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.cpp b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.cpp
index 9cf656e32b1280b6ad3c60024094a4016cff08c6..df925c5a6ef2e58157d73a3f991ab2e6f04a2ed8 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.cpp
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.cpp
@@ -25,38 +25,43 @@
#include "WeissHapticUnit.h"
-#include <boost/regex.hpp>
#include <filesystem>
+#include <boost/regex.hpp>
+
using namespace armarx;
-void WeissHapticUnit::onInitHapticUnit()
+void
+WeissHapticUnit::onInitHapticUnit()
{
std::vector<std::string> devices = getDevices();
for (std::vector<std::string>::iterator it = devices.begin(); it != devices.end(); ++it)
{
- WeissHapticSensorPtr sensor(new WeissHapticSensor(*it, 20)); // minimumReportIntervalMs = 20, limit to maximum 50 frames/s
+ WeissHapticSensorPtr sensor(new WeissHapticSensor(
+ *it, 20)); // minimumReportIntervalMs = 20, limit to maximum 50 frames/s
this->sensors.push_back(sensor);
}
std::cout << "Connect Interfaces" << std::endl;
- for (std::vector<std::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
+ for (std::vector<std::shared_ptr<WeissHapticSensor>>::iterator it = sensors.begin();
+ it != sensors.end();
+ ++it)
{
(*it)->connect();
}
-
}
-std::vector< std::string > WeissHapticUnit::getDevices()
+std::vector<std::string>
+WeissHapticUnit::getDevices()
{
const std::string target_path("/dev/");
const boost::regex my_filter("ttyACM[0-9]+");
- std::vector< std::string > files;
+ std::vector<std::string> files;
std::filesystem::directory_iterator end_itr; // Default ctor yields past-the-end
@@ -97,13 +102,17 @@ std::vector< std::string > WeissHapticUnit::getDevices()
return files;
}
-void WeissHapticUnit::setDeviceTag(const std::string& deviceName, const std::string& tag, const Ice::Current&)
+void
+WeissHapticUnit::setDeviceTag(const std::string& deviceName,
+ const std::string& tag,
+ const Ice::Current&)
{
for (WeissHapticSensorPtr sensor : sensors)
{
if (sensor->getDeviceName() == deviceName)
{
- ARMARX_IMPORTANT << "scheduling to set new device tag for " << deviceName << ": " << tag;
+ ARMARX_IMPORTANT << "scheduling to set new device tag for " << deviceName << ": "
+ << tag;
sensor->scheduleSetDeviceTag(tag);
return;
}
@@ -112,30 +121,37 @@ void WeissHapticUnit::setDeviceTag(const std::string& deviceName, const std::str
ARMARX_WARNING << "device not found: " << deviceName;
}
-void WeissHapticUnit::startLogging(const Ice::Current&)
+void
+WeissHapticUnit::startLogging(const Ice::Current&)
{
// @@@ TODO NotImplemented
}
-void WeissHapticUnit::stopLogging(const Ice::Current&)
+void
+WeissHapticUnit::stopLogging(const Ice::Current&)
{
// @@@ TODO NotImplemented
}
-void WeissHapticUnit::onStartHapticUnit()
+void
+WeissHapticUnit::onStartHapticUnit()
{
- for (std::vector<std::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
+ for (std::vector<std::shared_ptr<WeissHapticSensor>>::iterator it = sensors.begin();
+ it != sensors.end();
+ ++it)
{
(*it)->setListenerPrx(hapticTopicPrx);
(*it)->startSampling();
}
-
}
-void WeissHapticUnit::onExitHapticUnit()
+void
+WeissHapticUnit::onExitHapticUnit()
{
- for (std::vector<std::shared_ptr<WeissHapticSensor> >::iterator it = sensors.begin(); it != sensors.end(); ++it)
+ for (std::vector<std::shared_ptr<WeissHapticSensor>>::iterator it = sensors.begin();
+ it != sensors.end();
+ ++it)
{
(*it)->disconnect();
}
@@ -146,13 +162,13 @@ void WeissHapticUnit::onExitHapticUnit()
}*/
-void WeissHapticUnit::onDisconnectComponent()
+void
+WeissHapticUnit::onDisconnectComponent()
{
}
-
-PropertyDefinitionsPtr WeissHapticUnit::createPropertyDefinitions()
+PropertyDefinitionsPtr
+WeissHapticUnit::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new WeissHapticUnitPropertyDefinitions(getConfigIdentifier()));
}
-
diff --git a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.h b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.h
index 694196b1ef3e8061e3731eec9e835119a70c8ed9..d66eeb0607ebab5c17056dd4f644e0777848b576 100644
--- a/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.h
+++ b/source/RobotAPI/drivers/WeissHapticSensor/WeissHapticUnit.h
@@ -25,11 +25,13 @@
#pragma once
+#include <string>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
#include <RobotAPI/components/units/HapticUnit.h>
#include <RobotAPI/interface/units/WeissHapticUnit.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <string>
#include "WeissHapticSensor.h"
namespace armarx
@@ -37,18 +39,17 @@ namespace armarx
class WeissHapticUnitPropertyDefinitions : public HapticUnitPropertyDefinitions
{
public:
- WeissHapticUnitPropertyDefinitions(std::string prefix):
+ WeissHapticUnitPropertyDefinitions(std::string prefix) :
HapticUnitPropertyDefinitions(prefix)
{
}
};
- class WeissHapticUnit :
- virtual public WeissHapticUnitInterface,
- virtual public HapticUnit
+ class WeissHapticUnit : virtual public WeissHapticUnitInterface, virtual public HapticUnit
{
public:
- virtual std::string getDefaultName()
+ virtual std::string
+ getDefaultName()
{
return "WeissHapticUnit";
}
@@ -68,21 +69,21 @@ namespace armarx
//std::map<std::string, MatrixFloatPtr> currentValues;
-
//HapticSensorProtocolMaster hapticProtocol;
//bool remoteSystemReady;
private:
- std::vector< std::string > getDevices();
+ std::vector<std::string> getDevices();
- std::vector< std::shared_ptr< WeissHapticSensor > > sensors;
+ std::vector<std::shared_ptr<WeissHapticSensor>> sensors;
// WeissHapticUnitInterface interface
public:
- void setDeviceTag(const std::string& deviceName, const std::string& tag, const Ice::Current&) override;
+ void setDeviceTag(const std::string& deviceName,
+ const std::string& tag,
+ const Ice::Current&) override;
void startLogging(const Ice::Current&) override;
void stopLogging(const Ice::Current&) override;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp b/source/RobotAPI/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp
index f55fdfc2251a6e005d196dc73b8f75555911e7ef..ee59a130f618b159e32c4889b30e2f51b934dfd7 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IIMUEventDispatcher.cpp
@@ -7,12 +7,22 @@
*/
#include "IIMUEventDispatcher.h"
+
#include "IMUDevice.h"
namespace IMU
{
IIMUEventDispatcher::IIMUEventDispatcher(CIMUDevice* pIMUDevice) :
- m_DispatchingMode(eCoupled), m_MaximalPendingEvents(2), m_EventFlags(0XFFFF), m_pIMUDevice(pIMUDevice), m_LastStartTimeStamp(CTimeStamp::s_Zero), m_LastStopTimeStamp(CTimeStamp::s_Zero), m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero), m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
+ m_DispatchingMode(eCoupled),
+ m_MaximalPendingEvents(2),
+ m_EventFlags(0XFFFF),
+ m_pIMUDevice(pIMUDevice),
+ m_LastStartTimeStamp(CTimeStamp::s_Zero),
+ m_LastStopTimeStamp(CTimeStamp::s_Zero),
+ m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero),
+ m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero),
+ m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero),
+ m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
{
pthread_mutex_init(&m_DispatchingModeMutex, nullptr);
pthread_mutex_init(&m_MaximalPendingEventsMutex, nullptr);
@@ -28,7 +38,17 @@ namespace IMU
}
IIMUEventDispatcher::IIMUEventDispatcher() :
- m_DispatchingMode(eCoupled), m_MaximalPendingEvents(2), m_EventFlags(0XFFFF), m_pIMUDevice(nullptr), m_EventsQueue(), m_LastStartTimeStamp(CTimeStamp::s_Zero), m_LastStopTimeStamp(CTimeStamp::s_Zero), m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero), m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
+ m_DispatchingMode(eCoupled),
+ m_MaximalPendingEvents(2),
+ m_EventFlags(0XFFFF),
+ m_pIMUDevice(nullptr),
+ m_EventsQueue(),
+ m_LastStartTimeStamp(CTimeStamp::s_Zero),
+ m_LastStopTimeStamp(CTimeStamp::s_Zero),
+ m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero),
+ m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero),
+ m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero),
+ m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
{
pthread_mutex_init(&m_DispatchingModeMutex, nullptr);
pthread_mutex_init(&m_MaximalPendingEventsMutex, nullptr);
@@ -51,14 +71,16 @@ namespace IMU
}
}
- void IIMUEventDispatcher::SetIMU(CIMUDevice* pIMUDevice)
+ void
+ IIMUEventDispatcher::SetIMU(CIMUDevice* pIMUDevice)
{
_MINIMAL___LOCK(m_IMUDeviceMutex)
m_pIMUDevice = pIMUDevice;
_MINIMAL_UNLOCK(m_IMUDeviceMutex)
}
- uint32_t IIMUEventDispatcher::GetEventFlags()
+ uint32_t
+ IIMUEventDispatcher::GetEventFlags()
{
_MINIMAL___LOCK(m_EventFlagsMutex)
const uint32_t EventFlagsCurrentState = m_EventFlags;
@@ -66,14 +88,16 @@ namespace IMU
return EventFlagsCurrentState;
}
- void IIMUEventDispatcher::SetDispatchingMode(const DispatchingMode Mode)
+ void
+ IIMUEventDispatcher::SetDispatchingMode(const DispatchingMode Mode)
{
_MINIMAL___LOCK(m_DispatchingModeMutex)
m_DispatchingMode = Mode;
_MINIMAL_UNLOCK(m_DispatchingModeMutex)
}
- IIMUEventDispatcher::DispatchingMode IIMUEventDispatcher::GetDispatchingMode()
+ IIMUEventDispatcher::DispatchingMode
+ IIMUEventDispatcher::GetDispatchingMode()
{
_MINIMAL___LOCK(m_DispatchingModeMutex)
const DispatchingMode DispatchingModeCurrentState = m_DispatchingMode;
@@ -81,7 +105,8 @@ namespace IMU
return DispatchingModeCurrentState;
}
- void IIMUEventDispatcher::SetMaximalPendingEvents(const uint32_t MaximalPendingEvents)
+ void
+ IIMUEventDispatcher::SetMaximalPendingEvents(const uint32_t MaximalPendingEvents)
{
if ((MaximalPendingEvents > 1) && (MaximalPendingEvents != m_MaximalPendingEvents))
{
@@ -96,7 +121,8 @@ namespace IMU
}
}
- uint32_t IIMUEventDispatcher::GetMaximalPendingEvents()
+ uint32_t
+ IIMUEventDispatcher::GetMaximalPendingEvents()
{
_MINIMAL___LOCK(m_MaximalPendingEventsMutex)
const uint32_t MaximalPendingEventsCurrentState = m_MaximalPendingEvents;
@@ -104,14 +130,16 @@ namespace IMU
return MaximalPendingEventsCurrentState;
}
- void IIMUEventDispatcher::SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled)
+ void
+ IIMUEventDispatcher::SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled)
{
_MINIMAL___LOCK(m_EventFlagsMutex)
m_EventFlags = Enabled ? (m_EventFlags | Type) : (m_EventFlags & (~Type));
_MINIMAL_UNLOCK(m_EventFlagsMutex)
}
- uint32_t IIMUEventDispatcher::GetEventHandlingFlags()
+ uint32_t
+ IIMUEventDispatcher::GetEventHandlingFlags()
{
_MINIMAL___LOCK(m_EventFlagsMutex);
const uint32_t EventHandlingFlagsCurrentState = m_EventFlags;
@@ -119,7 +147,8 @@ namespace IMU
return EventHandlingFlagsCurrentState;
}
- void IIMUEventDispatcher::ReceiveEvent(const CIMUEvent& Event)
+ void
+ IIMUEventDispatcher::ReceiveEvent(const CIMUEvent& Event)
{
_MINIMAL___LOCK(m_EventFlagsMutex)
const bool HandelEvent = Event.GetEventType() & m_EventFlags;
@@ -185,7 +214,8 @@ namespace IMU
}
}
- bool IIMUEventDispatcher::ProcessPendingEvent()
+ bool
+ IIMUEventDispatcher::ProcessPendingEvent()
{
_MINIMAL___LOCK(m_EventsQueueMutex)
@@ -203,7 +233,8 @@ namespace IMU
}
}
- void IIMUEventDispatcher::SetReferenceTimeStamps(const timeval& Reference)
+ void
+ IIMUEventDispatcher::SetReferenceTimeStamps(const timeval& Reference)
{
_MINIMAL___LOCK(m_LastStartTimeStampMutex)
m_LastStartTimeStamp = Reference;
@@ -230,7 +261,8 @@ namespace IMU
_MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
}
- timeval IIMUEventDispatcher::GetLastStartTimeStamp()
+ timeval
+ IIMUEventDispatcher::GetLastStartTimeStamp()
{
_MINIMAL___LOCK(m_LastStartTimeStampMutex)
timeval TimeStampCurrentState = m_LastStartTimeStamp;
@@ -238,7 +270,8 @@ namespace IMU
return TimeStampCurrentState;
}
- timeval IIMUEventDispatcher::GetLastStopTimeStamp()
+ timeval
+ IIMUEventDispatcher::GetLastStopTimeStamp()
{
_MINIMAL___LOCK(m_LastStopTimeStampMutex)
timeval TimeStampCurrentState = m_LastStopTimeStamp;
@@ -246,7 +279,8 @@ namespace IMU
return TimeStampCurrentState;
}
- timeval IIMUEventDispatcher::GetLastCycleReferenceTimeStamp()
+ timeval
+ IIMUEventDispatcher::GetLastCycleReferenceTimeStamp()
{
_MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
timeval TimeStampCurrentState = m_LastCycleReferenceTimeStamp;
@@ -254,7 +288,8 @@ namespace IMU
return TimeStampCurrentState;
}
- timeval IIMUEventDispatcher::GetLastFusedCycleReferenceTimeStamp()
+ timeval
+ IIMUEventDispatcher::GetLastFusedCycleReferenceTimeStamp()
{
_MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
timeval TimeStampCurrentState = m_LastFusedCycleReferenceTimeStamp;
@@ -262,7 +297,8 @@ namespace IMU
return TimeStampCurrentState;
}
- timeval IIMUEventDispatcher::GetLastIntegratedStateReferenceTimeStamp()
+ timeval
+ IIMUEventDispatcher::GetLastIntegratedStateReferenceTimeStamp()
{
_MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
timeval TimeStampCurrentState = m_LastIntegratedStateReferenceTimeStamp;
@@ -270,7 +306,8 @@ namespace IMU
return TimeStampCurrentState;
}
- timeval IIMUEventDispatcher::GetLastCustomEventReferenceTimeStamp()
+ timeval
+ IIMUEventDispatcher::GetLastCustomEventReferenceTimeStamp()
{
_MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
timeval TimeStampCurrentState = m_LastCustomEventReferenceTimeStamp;
@@ -278,15 +315,17 @@ namespace IMU
return TimeStampCurrentState;
}
- void IIMUEventDispatcher::PurgeEvents()
+ void
+ IIMUEventDispatcher::PurgeEvents()
{
_MINIMAL___LOCK(m_EventsQueueMutex)
if (m_EventsQueue.size() >= m_MaximalPendingEvents)
{
- const uint32_t TotalEventsToRemove = (uint32_t(m_EventsQueue.size()) - m_MaximalPendingEvents) + 1;
+ const uint32_t TotalEventsToRemove =
+ (uint32_t(m_EventsQueue.size()) - m_MaximalPendingEvents) + 1;
- for (uint32_t i = 0 ; i < TotalEventsToRemove ; ++i)
+ for (uint32_t i = 0; i < TotalEventsToRemove; ++i)
{
m_EventsQueue.pop_front();
}
@@ -295,5 +334,4 @@ namespace IMU
_MINIMAL_UNLOCK(m_EventsQueueMutex)
}
-}
-
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IIMUEventDispatcher.h b/source/RobotAPI/drivers/XsensIMU/IMU/IIMUEventDispatcher.h
index 862be33c708ade3ab88fd9a572b8f43d550f6bb8..89667fe9b2ca728b9f80e36a9701a13a292124f7 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IIMUEventDispatcher.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IIMUEventDispatcher.h
@@ -8,20 +8,21 @@
#pragma once
-#include "Includes.h"
#include "IMUEvent.h"
#include "IMUHelpers.h"
+#include "Includes.h"
namespace IMU
{
class CIMUDevice;
+
class IIMUEventDispatcher
{
public:
-
enum DispatchingMode
{
- eCoupled, eDecoupled
+ eCoupled,
+ eDecoupled
};
IIMUEventDispatcher(CIMUDevice* pIMUDevice);
@@ -44,12 +45,14 @@ namespace IMU
void ReceiveEvent(const CIMUEvent& Event);
- inline uint32_t GetTotalPendingEvents()
+ inline uint32_t
+ GetTotalPendingEvents()
{
return uint32_t(m_EventsQueue.size());
}
- inline bool HasPendingEvents()
+ inline bool
+ HasPendingEvents()
{
return m_EventsQueue.size();
}
@@ -68,7 +71,6 @@ namespace IMU
virtual void OnIMUEvent(const CIMUEvent& Event) = 0;
private:
-
void PurgeEvents();
DispatchingMode m_DispatchingMode;
@@ -94,5 +96,4 @@ namespace IMU
timeval m_LastCustomEventReferenceTimeStamp;
pthread_mutex_t m_LastCustomEventReferenceTimeStampMutex;
};
-}
-
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IMU.h b/source/RobotAPI/drivers/XsensIMU/IMU/IMU.h
index 96fe244e7ee7d0acb1c2e7a4277cf5bc4c218a79..366b0a7b7fb4aa73ac7068fea616c40ab19f4888 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IMU.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IMU.h
@@ -12,4 +12,3 @@
#include "IMUDevice.h"
#include "IMUEvent.h"
#include "IMUState.h"
-
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp b/source/RobotAPI/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp
index d1f21adcd4b755511c1b90aac23ad7a4dd7088d1..ff406b567f420e8a3b168f376840f2f7b06ea82f 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IMUDeducedReckoning.cpp
@@ -11,7 +11,9 @@
namespace IMU
{
CIMUDeducedReckoning::CIMUDeducedReckoning(const bool IsVerbose) :
- IIMUEventDispatcher(), m_IsVerbose(IsVerbose), m_G(IMU::CGeolocationInformation::GetGravitationalAcceleration())
+ IIMUEventDispatcher(),
+ m_IsVerbose(IsVerbose),
+ m_G(IMU::CGeolocationInformation::GetGravitationalAcceleration())
{
SetEventHandling(CIMUEvent::eOnIMUStart, true);
SetEventHandling(CIMUEvent::eOnIMUStop, true);
@@ -21,10 +23,10 @@ namespace IMU
SetEventHandling(CIMUEvent::eOnIMUCustomEvent, true);
}
- CIMUDeducedReckoning::~CIMUDeducedReckoning()
- = default;
+ CIMUDeducedReckoning::~CIMUDeducedReckoning() = default;
- void CIMUDeducedReckoning::OnIMUEvent(const CIMUEvent& Event)
+ void
+ CIMUDeducedReckoning::OnIMUEvent(const CIMUEvent& Event)
{
switch (Event.GetEventType())
{
@@ -54,136 +56,263 @@ namespace IMU
}
}
- void CIMUDeducedReckoning::OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ void
+ CIMUDeducedReckoning::OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
{
if (m_IsVerbose)
{
- std::cout << "OnIMUStart(IMU Device ID = " << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")" << std::endl;
- std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastStartTimeStamp()) << "]" << std::endl;
+ std::cout << "OnIMUStart(IMU Device ID = " << pIMUDevice->GetDeviceId()
+ << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")"
+ << std::endl;
+ std::cout << "\t[ Global time = "
+ << CTimeStamp::GetElapsedSeconds(TimeStamp,
+ pIMUDevice->GetReferenceTimeStamp())
+ << " s]" << std::endl;
+ std::cout << "\t[ Latency = "
+ << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastStartTimeStamp())
+ << "]" << std::endl;
if (GetDispatchingMode() == eDecoupled)
{
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
+ << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
+ << std::endl;
}
}
}
- void CIMUDeducedReckoning::OnIMUStop(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ void
+ CIMUDeducedReckoning::OnIMUStop(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
{
if (m_IsVerbose)
{
- std::cout << "OnIMUStop(IMU Device ID = " << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")" << std::endl;
- std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastStopTimeStamp()) << "]" << std::endl;
+ std::cout << "OnIMUStop(IMU Device ID = " << pIMUDevice->GetDeviceId()
+ << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")"
+ << std::endl;
+ std::cout << "\t[ Global time = "
+ << CTimeStamp::GetElapsedSeconds(TimeStamp,
+ pIMUDevice->GetReferenceTimeStamp())
+ << " s]" << std::endl;
+ std::cout << "\t[ Latency = "
+ << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastStopTimeStamp())
+ << "]" << std::endl;
if (GetDispatchingMode() == eDecoupled)
{
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
+ << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
+ << std::endl;
}
}
}
- void CIMUDeducedReckoning::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ void
+ CIMUDeducedReckoning::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
{
const IMUState CurrentState = pIMUDevice->GetIMUState();
if (m_IsVerbose)
{
- std::cout << "OnIMUCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
- std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastCycleReferenceTimeStamp()) << " µs]" << std::endl;
+ std::cout << "OnIMUCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId()
+ << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled")
+ << std::dec << ")" << std::endl;
+ std::cout << "\t[ Global time = "
+ << CTimeStamp::GetElapsedSeconds(TimeStamp,
+ pIMUDevice->GetReferenceTimeStamp())
+ << " s]" << std::endl;
+ std::cout << "\t[ Latency = "
+ << CTimeStamp::GetElapsedMicroseconds(TimeStamp,
+ GetLastCycleReferenceTimeStamp())
+ << " µs]" << std::endl;
if (GetDispatchingMode() == eDecoupled)
{
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
+ << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
+ << std::endl;
}
- std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
- std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
- std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
- std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
- std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
- std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
+ std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]"
+ << std::endl;
+ std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0]
+ << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << ","
+ << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
+ std::cout << "\t[ Acceleration Magnitude = ["
+ << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) *
+ 1000.0f
+ << " mm/s^2]" << std::endl;
+ std::cout << "\t[ Gyroscope Rotation = ["
+ << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << ","
+ << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << ","
+ << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Magnetic Rotation = ["
+ << CurrentState.m_PhysicalData.m_MagneticRotation[0] << ","
+ << CurrentState.m_PhysicalData.m_MagneticRotation[1] << ","
+ << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Quaternion Rotation = ["
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << ","
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << ","
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << ","
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
}
- memcpy(m_OrientationQuaternion, CurrentState.m_PhysicalData.m_QuaternionRotation, sizeof(float) * 4);
- memcpy(m_MagneticRotation, CurrentState.m_PhysicalData.m_MagneticRotation, sizeof(float) * 3);
- memcpy(m_GyroscopeRotation, CurrentState.m_PhysicalData.m_GyroscopeRotation, sizeof(float) * 3);
+ memcpy(m_OrientationQuaternion,
+ CurrentState.m_PhysicalData.m_QuaternionRotation,
+ sizeof(float) * 4);
+ memcpy(
+ m_MagneticRotation, CurrentState.m_PhysicalData.m_MagneticRotation, sizeof(float) * 3);
+ memcpy(m_GyroscopeRotation,
+ CurrentState.m_PhysicalData.m_GyroscopeRotation,
+ sizeof(float) * 3);
memcpy(m_Accelaration, CurrentState.m_PhysicalData.m_Acceleration, sizeof(float) * 3);
}
- void CIMUDeducedReckoning::OnIMUFusedCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ void
+ CIMUDeducedReckoning::OnIMUFusedCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
{
const IMUState CurrentState = pIMUDevice->GetIMUState();
if (m_IsVerbose)
{
- std::cout << "OnIMUFusedCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
- std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastFusedCycleReferenceTimeStamp()) << " µs]" << std::endl;
+ std::cout << "OnIMUFusedCycle(IMU Device ID = 0x" << std::hex
+ << pIMUDevice->GetDeviceId()
+ << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled")
+ << std::dec << ")" << std::endl;
+ std::cout << "\t[ Global time = "
+ << CTimeStamp::GetElapsedSeconds(TimeStamp,
+ pIMUDevice->GetReferenceTimeStamp())
+ << " s]" << std::endl;
+ std::cout << "\t[ Latency = "
+ << CTimeStamp::GetElapsedMicroseconds(TimeStamp,
+ GetLastFusedCycleReferenceTimeStamp())
+ << " µs]" << std::endl;
if (GetDispatchingMode() == eDecoupled)
{
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
+ << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
+ << std::endl;
}
- std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
- std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
- std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
- std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
- std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
- std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
+ std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]"
+ << std::endl;
+ std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0]
+ << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << ","
+ << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
+ std::cout << "\t[ Acceleration Magnitude = ["
+ << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) *
+ 1000.0f
+ << " mm/s^2]" << std::endl;
+ std::cout << "\t[ Gyroscope Rotation = ["
+ << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << ","
+ << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << ","
+ << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Magnetic Rotation = ["
+ << CurrentState.m_PhysicalData.m_MagneticRotation[0] << ","
+ << CurrentState.m_PhysicalData.m_MagneticRotation[1] << ","
+ << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Quaternion Rotation = ["
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << ","
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << ","
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << ","
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
}
- memcpy(m_OrientationQuaternion, CurrentState.m_PhysicalData.m_QuaternionRotation, sizeof(float) * 4);
- memcpy(m_MagneticRotation, CurrentState.m_PhysicalData.m_MagneticRotation, sizeof(float) * 3);
- memcpy(m_GyroscopeRotation, CurrentState.m_PhysicalData.m_GyroscopeRotation, sizeof(float) * 3);
+ memcpy(m_OrientationQuaternion,
+ CurrentState.m_PhysicalData.m_QuaternionRotation,
+ sizeof(float) * 4);
+ memcpy(
+ m_MagneticRotation, CurrentState.m_PhysicalData.m_MagneticRotation, sizeof(float) * 3);
+ memcpy(m_GyroscopeRotation,
+ CurrentState.m_PhysicalData.m_GyroscopeRotation,
+ sizeof(float) * 3);
memcpy(m_Accelaration, CurrentState.m_PhysicalData.m_Acceleration, sizeof(float) * 3);
}
- void CIMUDeducedReckoning::OnIMUIntegratedState(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
+ void
+ CIMUDeducedReckoning::OnIMUIntegratedState(const timeval& TimeStamp,
+ const CIMUDevice* pIMUDevice)
{
const IMUState CurrentState = pIMUDevice->GetIMUState();
if (m_IsVerbose)
{
- std::cout << "OnIMUStateUpdate(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
- std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastIntegratedStateReferenceTimeStamp()) << " µs]" << std::endl;
+ std::cout << "OnIMUStateUpdate(IMU Device ID = 0x" << std::hex
+ << pIMUDevice->GetDeviceId()
+ << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled")
+ << std::dec << ")" << std::endl;
+ std::cout << "\t[ Global time = "
+ << CTimeStamp::GetElapsedSeconds(TimeStamp,
+ pIMUDevice->GetReferenceTimeStamp())
+ << " s]" << std::endl;
+ std::cout << "\t[ Latency = "
+ << CTimeStamp::GetElapsedMicroseconds(
+ TimeStamp, GetLastIntegratedStateReferenceTimeStamp())
+ << " µs]" << std::endl;
if (GetDispatchingMode() == eDecoupled)
{
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
+ << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
+ << std::endl;
}
- std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
- std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
- std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
- std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
- std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
- std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
+ std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]"
+ << std::endl;
+ std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0]
+ << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << ","
+ << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
+ std::cout << "\t[ Acceleration Magnitude = ["
+ << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) *
+ 1000.0f
+ << " mm/s^2]" << std::endl;
+ std::cout << "\t[ Gyroscope Rotation = ["
+ << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << ","
+ << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << ","
+ << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Magnetic Rotation = ["
+ << CurrentState.m_PhysicalData.m_MagneticRotation[0] << ","
+ << CurrentState.m_PhysicalData.m_MagneticRotation[1] << ","
+ << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
+ std::cout << "\t[ Quaternion Rotation = ["
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << ","
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << ","
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << ","
+ << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
}
}
- void CIMUDeducedReckoning::OnIMUCustomEvent(const CIMUEvent& CustomEvent)
+ void
+ CIMUDeducedReckoning::OnIMUCustomEvent(const CIMUEvent& CustomEvent)
{
if (m_IsVerbose)
{
- std::cout << "OnIMUCustomEvent(IMU Device ID = 0x" << std::hex << CustomEvent.GetIMU()->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(), CustomEvent.GetIMU()->GetReferenceTimeStamp()) << " µs]" << std::endl;
- std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(), GetLastCustomEventReferenceTimeStamp()) << " µs]" << std::endl;
+ std::cout << "OnIMUCustomEvent(IMU Device ID = 0x" << std::hex
+ << CustomEvent.GetIMU()->GetDeviceId()
+ << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled")
+ << std::dec << ")" << std::endl;
+ std::cout << "\t[ Latency = "
+ << CTimeStamp::GetElapsedMicroseconds(
+ CustomEvent.GetTimeStamp(),
+ CustomEvent.GetIMU()->GetReferenceTimeStamp())
+ << " µs]" << std::endl;
+ std::cout << "\t[ Latency = "
+ << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(),
+ GetLastCustomEventReferenceTimeStamp())
+ << " µs]" << std::endl;
if (GetDispatchingMode() == eDecoupled)
{
- std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
- std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
+ std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
+ << std::endl;
+ std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
+ << std::endl;
}
}
}
-}
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IMUDeducedReckoning.h b/source/RobotAPI/drivers/XsensIMU/IMU/IMUDeducedReckoning.h
index 3c04ea058e2f08bb899638cede12a150f9b2119a..e933a3e3f777114f26d9c98b86fc4a0179ece9b6 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IMUDeducedReckoning.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IMUDeducedReckoning.h
@@ -9,40 +9,42 @@
#pragma once
#include "IIMUEventDispatcher.h"
-#include "IMUState.h"
#include "IMUDevice.h"
+#include "IMUState.h"
namespace IMU
{
class CIMUDeducedReckoning : public IIMUEventDispatcher
{
public:
-
CIMUDeducedReckoning(const bool IsVerbose);
~CIMUDeducedReckoning() override;
- inline const float* GetOrientationQuaternion() const
+ inline const float*
+ GetOrientationQuaternion() const
{
return m_OrientationQuaternion;
}
- inline const float* GetMagneticRotation() const
+ inline const float*
+ GetMagneticRotation() const
{
return m_MagneticRotation;
}
- inline const float* GetGyroscopeRotation() const
+ inline const float*
+ GetGyroscopeRotation() const
{
return m_GyroscopeRotation;
}
- inline const float* GetAccelaration() const
+ inline const float*
+ GetAccelaration() const
{
return m_Accelaration;
}
protected:
-
void OnIMUEvent(const CIMUEvent& Event) override;
void OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice);
@@ -60,5 +62,4 @@ namespace IMU
float m_GyroscopeRotation[3];
float m_Accelaration[3];
};
-}
-
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IMUDevice.cpp b/source/RobotAPI/drivers/XsensIMU/IMU/IMUDevice.cpp
index 9b17e1002baabb8d396c15422aa835860324b1ba..7739c5ccb9275d8d824cca933f5e15d0c1ee51ab 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IMUDevice.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IMUDevice.cpp
@@ -7,16 +7,30 @@
*/
#include "IMUDevice.h"
+
#include "IIMUEventDispatcher.h"
namespace IMU
{
CIMUDevice::CIMUDevice() :
- m_DeviceId(0), m_SamplingFrequency(SamplingFrequency(0)), m_PeriodMicroSeconds(0), m_FusionStrategy(eNoFusion), m_SamplesPerFusion(0), m_CollectedFusionSamples(0), m_IsActive(false), m_IsDispatching(false), m_IsInitialized(false), m_pInternalThreadHandel(0), m_IMUEventDispatchers(), m_ReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFrameTimeStamp(CTimeStamp::s_Zero)
+ m_DeviceId(0),
+ m_SamplingFrequency(SamplingFrequency(0)),
+ m_PeriodMicroSeconds(0),
+ m_FusionStrategy(eNoFusion),
+ m_SamplesPerFusion(0),
+ m_CollectedFusionSamples(0),
+ m_IsActive(false),
+ m_IsDispatching(false),
+ m_IsInitialized(false),
+ m_pInternalThreadHandel(0),
+ m_IMUEventDispatchers(),
+ m_ReferenceTimeStamp(CTimeStamp::s_Zero),
+ m_LastFrameTimeStamp(CTimeStamp::s_Zero)
#ifdef _IMU_USE_XSENS_DEVICE_
- , m_pXsensMTiModule(nullptr)
+ ,
+ m_pXsensMTiModule(nullptr)
#endif
{
@@ -31,12 +45,14 @@ namespace IMU
FinalizeModuleDevice();
}
- uint64_t CIMUDevice::GetDeviceId() const
+ uint64_t
+ CIMUDevice::GetDeviceId() const
{
return m_DeviceId;
}
- bool CIMUDevice::Connect(const std::string& PortName, const SamplingFrequency Frequency)
+ bool
+ CIMUDevice::Connect(const std::string& PortName, const SamplingFrequency Frequency)
{
if (m_IsInitialized)
{
@@ -45,7 +61,9 @@ namespace IMU
if (!PortName.length())
{
- std::cerr << "[IMU Error: Cannot connect to empty port name!]\n\t[Operation result: (PortName.length()==0)]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Error: Cannot connect to empty port name!]\n\t[Operation result: "
+ "(PortName.length()==0)]\n[Source location: "
+ << __FILE__ << ":" << __LINE__ << "]" << std::endl;
return false;
}
@@ -54,11 +72,13 @@ namespace IMU
return m_IsInitialized;
}
- bool CIMUDevice::Start(const bool Blocking)
+ bool
+ CIMUDevice::Start(const bool Blocking)
{
if (m_IsInitialized && (!m_IsActive))
{
- const int Result = pthread_create(&m_pInternalThreadHandel, nullptr, CIMUDevice::ThreadLoop, (void*) this);
+ const int Result = pthread_create(
+ &m_pInternalThreadHandel, nullptr, CIMUDevice::ThreadLoop, (void*)this);
if (Result == 0)
{
@@ -74,7 +94,8 @@ namespace IMU
return false;
}
- void CIMUDevice::Stop(const bool Blocking)
+ void
+ CIMUDevice::Stop(const bool Blocking)
{
if (m_IsActive)
{
@@ -90,7 +111,8 @@ namespace IMU
}
}
- bool CIMUDevice::SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion)
+ bool
+ CIMUDevice::SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion)
{
if (SamplesPerFusion > 1)
{
@@ -105,17 +127,21 @@ namespace IMU
}
else
{
- std::cerr << "[IMU Device error: Cannot set fusion with less than 2 samples per fusion!]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: Cannot set fusion with less than 2 samples per "
+ "fusion!]\n\t[Source location: "
+ << __FILE__ << ":" << __LINE__ << "]" << std::endl;
return false;
}
}
- bool CIMUDevice::IsActive() const
+ bool
+ CIMUDevice::IsActive() const
{
return m_IsActive;
}
- bool CIMUDevice::RegisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
+ bool
+ CIMUDevice::RegisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
{
if (pIMUEventDispatcher)
{
@@ -125,7 +151,8 @@ namespace IMU
{
pIMUEventDispatcher->SetIMU(this);
pIMUEventDispatcher->SetReferenceTimeStamps(m_ReferenceTimeStamp);
- std::pair<std::set<IIMUEventDispatcher*>::iterator, bool> Result = m_IMUEventDispatchers.insert(pIMUEventDispatcher);
+ std::pair<std::set<IIMUEventDispatcher*>::iterator, bool> Result =
+ m_IMUEventDispatchers.insert(pIMUEventDispatcher);
_MINIMAL_UNLOCK(m_EventDispatchersMutex)
return Result.second;
}
@@ -136,12 +163,14 @@ namespace IMU
return false;
}
- bool CIMUDevice::UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
+ bool
+ CIMUDevice::UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
{
if (pIMUEventDispatcher)
{
_MINIMAL___LOCK(m_EventDispatchersMutex)
- std::set<IIMUEventDispatcher*>::iterator ppElement = m_IMUEventDispatchers.find(pIMUEventDispatcher);
+ std::set<IIMUEventDispatcher*>::iterator ppElement =
+ m_IMUEventDispatchers.find(pIMUEventDispatcher);
if (ppElement != m_IMUEventDispatchers.end())
{
@@ -157,7 +186,8 @@ namespace IMU
return false;
}
- void CIMUDevice::UnregisterEventDispatchers()
+ void
+ CIMUDevice::UnregisterEventDispatchers()
{
if (m_IMUEventDispatchers.size())
{
@@ -175,7 +205,9 @@ namespace IMU
#ifdef _IMU_USE_XSENS_DEVICE_
- bool CIMUDevice::InitializeXsensDevice(const std::string& PortName, const SamplingFrequency Frequency)
+ bool
+ CIMUDevice::InitializeXsensDevice(const std::string& PortName,
+ const SamplingFrequency Frequency)
{
if (m_IsInitialized)
{
@@ -186,28 +218,39 @@ namespace IMU
if (m_pXsensMTiModule->openPort(PortName.c_str()) != MTRV_OK)
{
- std::cerr << "[IMU Device error: Cannot open port!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: Cannot open port!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
+ << ":" << __LINE__ << "]" << std::endl;
DestroyXsensModuleDevice();
return false;
}
if (m_pXsensMTiModule->writeMessage(MID_GOTOCONFIG) != MTRV_OK)
{
- std::cerr << "[IMU Device error: Cannot set configuration state!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr
+ << "[IMU Device error: Cannot set configuration state!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
+ << ":" << __LINE__ << "]" << std::endl;
DestroyXsensModuleDevice();
return false;
}
- if (m_pXsensMTiModule->setDeviceMode(OUTPUTMODE_CALIB | OUTPUTMODE_ORIENT, OUTPUTSETTINGS_ORIENTMODE_QUATERNION | OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) != MTRV_OK)
+ if (m_pXsensMTiModule->setDeviceMode(OUTPUTMODE_CALIB | OUTPUTMODE_ORIENT,
+ OUTPUTSETTINGS_ORIENTMODE_QUATERNION |
+ OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) != MTRV_OK)
{
- std::cerr << "[IMU Device error: Cannot set output mode!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: Cannot set output mode!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
+ << ":" << __LINE__ << "]" << std::endl;
DestroyXsensModuleDevice();
return false;
}
if (m_pXsensMTiModule->setSetting(MID_SETPERIOD, Frequency, LEN_PERIOD) != MTRV_OK)
{
- std::cerr << "[IMU Device error: Cannot set sampling period!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: Cannot set sampling period!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
+ << ":" << __LINE__ << "]" << std::endl;
DestroyXsensModuleDevice();
return false;
}
@@ -216,7 +259,9 @@ namespace IMU
if (m_pXsensMTiModule->reqSetting(MID_REQDID, DeviceId) != MTRV_OK)
{
- std::cerr << "[IMU Device error: Cannot get device ID!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: Cannot get device ID!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
+ << ":" << __LINE__ << "]" << std::endl;
DestroyXsensModuleDevice();
return false;
}
@@ -225,7 +270,10 @@ namespace IMU
if (m_pXsensMTiModule->writeMessage(MID_GOTOMEASUREMENT) != MTRV_OK)
{
- std::cerr << "[IMU Device error: Cannot enter measurement state!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr
+ << "[IMU Device error: Cannot enter measurement state!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
+ << ":" << __LINE__ << "]" << std::endl;
DestroyXsensModuleDevice();
return false;
}
@@ -233,7 +281,8 @@ namespace IMU
return true;
}
- void CIMUDevice::FinalizeXsensModuleDevice()
+ void
+ CIMUDevice::FinalizeXsensModuleDevice()
{
if (m_IsInitialized)
{
@@ -248,7 +297,8 @@ namespace IMU
}
}
- void CIMUDevice::DestroyXsensModuleDevice()
+ void
+ CIMUDevice::DestroyXsensModuleDevice()
{
if (m_pXsensMTiModule)
{
@@ -265,58 +315,106 @@ namespace IMU
#endif
- bool CIMUDevice::LoadCurrentState()
+ bool
+ CIMUDevice::LoadCurrentState()
{
_MINIMAL___LOCK(m_DeviceMutex)
#ifdef _IMU_USE_XSENS_DEVICE_
- if (m_pXsensMTiModule->readDataMessage(m_XsensMTiFrame.m_Data, m_XsensMTiFrame.m_DataLength) == MTRV_OK)
- if (m_pXsensMTiModule->getValue(VALUE_CALIB_ACC, m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration, m_XsensMTiFrame.m_Data) == MTRV_OK)
- if (m_pXsensMTiModule->getValue(VALUE_CALIB_GYR, m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_GyroscopeRotation, m_XsensMTiFrame.m_Data) == MTRV_OK)
- if (m_pXsensMTiModule->getValue(VALUE_CALIB_MAG, m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_MagneticRotation, m_XsensMTiFrame.m_Data) == MTRV_OK)
- if (m_pXsensMTiModule->getValue(VALUE_ORIENT_QUAT, m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_QuaternionRotation, m_XsensMTiFrame.m_Data) == MTRV_OK)
- if (m_pXsensMTiModule->getValue(VALUE_SAMPLECNT, m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount, m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->readDataMessage(m_XsensMTiFrame.m_Data,
+ m_XsensMTiFrame.m_DataLength) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(
+ VALUE_CALIB_ACC,
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration,
+ m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(
+ VALUE_CALIB_GYR,
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_GyroscopeRotation,
+ m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(
+ VALUE_CALIB_MAG,
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_MagneticRotation,
+ m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(
+ VALUE_ORIENT_QUAT,
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_QuaternionRotation,
+ m_XsensMTiFrame.m_Data) == MTRV_OK)
+ if (m_pXsensMTiModule->getValue(
+ VALUE_SAMPLECNT,
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount,
+ m_XsensMTiFrame.m_Data) == MTRV_OK)
{
- if (m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount != -1)
+ if (m_XsensMTiFrame.m_IMUState.m_ControlData
+ .m_PreviousSampleCount != -1)
{
- m_XsensMTiFrame.m_IMUState.m_ControlData.m_IsConsecutive = ((m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount + 1) % 65536) == m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount;
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_IsConsecutive =
+ ((m_XsensMTiFrame.m_IMUState.m_ControlData
+ .m_PreviousSampleCount +
+ 1) %
+ 65536) == m_XsensMTiFrame.m_IMUState.m_ControlData
+ .m_CurrentSampleCount;
}
- m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount = m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount;
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount =
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount;
m_XsensMTiFrame.m_IMUState.m_ControlData.m_MessageCounter++;
- gettimeofday(&m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp, nullptr);
- m_LastFrameTimeStamp = m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp;
+ gettimeofday(&m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp,
+ nullptr);
+ m_LastFrameTimeStamp =
+ m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp;
- m_XsensMTiFrame.m_IMUState.m_PhysicalData.UpdateAccelerationMagnitud();
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData
+ .UpdateAccelerationMagnitud();
_MINIMAL_UNLOCK(m_DeviceMutex)
return true;
}
else
{
- std::cerr << "[IMU Device error: Fail to get sample count!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: Fail to get sample "
+ "count!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal()
+ << "]\n\t[Source location: " << __FILE__ << ":"
+ << __LINE__ << "]" << std::endl;
}
else
{
- std::cerr << "[IMU Device error: Fail to get quaternion rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: Fail to get quaternion "
+ "rotation!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal()
+ << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__
+ << "]" << std::endl;
}
else
{
- std::cerr << "[IMU Device error: Fail to get magnetic rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: Fail to get magnetic "
+ "rotation!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal()
+ << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]"
+ << std::endl;
}
else
{
- std::cerr << "[IMU Device error: Fail to get gyroscope rotation!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: Fail to get gyroscope "
+ "rotation!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal()
+ << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]"
+ << std::endl;
}
else
{
- std::cerr << "[IMU Device error: Fail to get acceleration vector!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr
+ << "[IMU Device error: Fail to get acceleration vector!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
+ << ":" << __LINE__ << "]" << std::endl;
}
else
{
- std::cerr << "[IMU Device error: Fail to read message!]\n\t[Operation result: " << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: Fail to read message!]\n\t[Operation result: "
+ << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
+ << ":" << __LINE__ << "]" << std::endl;
}
#endif
@@ -326,7 +424,8 @@ namespace IMU
return false;
}
- bool CIMUDevice::MeanFuseCurrentState()
+ bool
+ CIMUDevice::MeanFuseCurrentState()
{
IncorporateCurrentStateMeanFusion();
@@ -339,21 +438,29 @@ namespace IMU
return false;
}
- void CIMUDevice::IncorporateCurrentStateMeanFusion()
+ void
+ CIMUDevice::IncorporateCurrentStateMeanFusion()
{
- m_FusedPhysicalData.m_Acceleration[0] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
- m_FusedPhysicalData.m_Acceleration[1] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
- m_FusedPhysicalData.m_Acceleration[2] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
+ m_FusedPhysicalData.m_Acceleration[0] +=
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
+ m_FusedPhysicalData.m_Acceleration[1] +=
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
+ m_FusedPhysicalData.m_Acceleration[2] +=
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
++m_CollectedFusionSamples;
}
- void CIMUDevice::MeanFusion()
+ void
+ CIMUDevice::MeanFusion()
{
//Execution the fusion
const float NormalizationFactor = 1.0f / float(m_CollectedFusionSamples);
- m_FusedIMUState.m_PhysicalData.m_Acceleration[0] = m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
- m_FusedIMUState.m_PhysicalData.m_Acceleration[1] = m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
- m_FusedIMUState.m_PhysicalData.m_Acceleration[2] = m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[0] =
+ m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[1] =
+ m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[2] =
+ m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
//Derivated from fusion
m_FusedIMUState.m_PhysicalData.UpdateAccelerationMagnitud();
@@ -363,7 +470,8 @@ namespace IMU
m_CollectedFusionSamples = 0;
}
- bool CIMUDevice::GaussianFuseCurrentState()
+ bool
+ CIMUDevice::GaussianFuseCurrentState()
{
IncorporateCurrentStateGaussianFusion();
@@ -376,22 +484,30 @@ namespace IMU
return false;
}
- void CIMUDevice::IncorporateCurrentStateGaussianFusion()
+ void
+ CIMUDevice::IncorporateCurrentStateGaussianFusion()
{
- m_FusedPhysicalData.m_Acceleration[0] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
- m_FusedPhysicalData.m_Acceleration[1] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
- m_FusedPhysicalData.m_Acceleration[2] += m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
+ m_FusedPhysicalData.m_Acceleration[0] +=
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
+ m_FusedPhysicalData.m_Acceleration[1] +=
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
+ m_FusedPhysicalData.m_Acceleration[2] +=
+ m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
++m_CollectedFusionSamples;
}
- void CIMUDevice::GaussianFusion()
+ void
+ CIMUDevice::GaussianFusion()
{
//Execution the fusion
const float NormalizationFactor = 1.0f / float(m_CollectedFusionSamples);
- m_FusedIMUState.m_PhysicalData.m_Acceleration[0] = m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
- m_FusedIMUState.m_PhysicalData.m_Acceleration[1] = m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
- m_FusedIMUState.m_PhysicalData.m_Acceleration[2] = m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[0] =
+ m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[1] =
+ m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
+ m_FusedIMUState.m_PhysicalData.m_Acceleration[2] =
+ m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
//Derivated from fusion
m_FusedIMUState.m_PhysicalData.UpdateAccelerationMagnitud();
@@ -401,7 +517,8 @@ namespace IMU
m_CollectedFusionSamples = 0;
}
- bool CIMUDevice::IntegrateCurrentState()
+ bool
+ CIMUDevice::IntegrateCurrentState()
{
if (m_FusionStrategy == eNoFusion)
{
@@ -413,17 +530,20 @@ namespace IMU
}
}
- bool CIMUDevice::IntegrateWithOutFusion()
+ bool
+ CIMUDevice::IntegrateWithOutFusion()
{
return true;
}
- bool CIMUDevice::IntegrateWithFusion()
+ bool
+ CIMUDevice::IntegrateWithFusion()
{
return true;
}
- bool CIMUDevice::InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency)
+ bool
+ CIMUDevice::InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency)
{
_MINIMAL___LOCK(m_DeviceMutex)
@@ -434,7 +554,8 @@ namespace IMU
{
m_SamplingFrequency = Frequency;
const int Cylces = 0X1C200 / int(m_SamplingFrequency);
- m_PeriodMicroSeconds = int(round((1000000.0f * _IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_) / float(Cylces)));
+ m_PeriodMicroSeconds =
+ int(round((1000000.0f * _IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_) / float(Cylces)));
_MINIMAL_UNLOCK(m_DeviceMutex)
return true;
}
@@ -445,10 +566,10 @@ namespace IMU
}
#endif
-
}
- void CIMUDevice::FinalizeModuleDevice()
+ void
+ CIMUDevice::FinalizeModuleDevice()
{
Stop(true);
@@ -459,12 +580,13 @@ namespace IMU
#endif
UnregisterEventDispatchers();
-
}
- void CIMUDevice::ShouldYield()
+ void
+ CIMUDevice::ShouldYield()
{
- const long int RemainingTime = m_PeriodMicroSeconds - CTimeStamp::GetElapsedMicroseconds(m_LastFrameTimeStamp);
+ const long int RemainingTime =
+ m_PeriodMicroSeconds - CTimeStamp::GetElapsedMicroseconds(m_LastFrameTimeStamp);
if (RemainingTime > 0)
{
@@ -472,7 +594,8 @@ namespace IMU
}
}
- bool CIMUDevice::DispatchCylcle()
+ bool
+ CIMUDevice::DispatchCylcle()
{
if (LoadCurrentState())
{
@@ -483,11 +606,17 @@ namespace IMU
case eMeanFusion:
if (MeanFuseCurrentState())
{
- SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUFusedCycle, this, m_FusedIMUState));
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,
+ CIMUEvent::eOnIMUFusedCycle,
+ this,
+ m_FusedIMUState));
if (IntegrateCurrentState())
{
- SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUIntegratedState, this, m_IntegratedIMUState));
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,
+ CIMUEvent::eOnIMUIntegratedState,
+ this,
+ m_IntegratedIMUState));
}
}
@@ -496,11 +625,17 @@ namespace IMU
case eGaussianFusion:
if (GaussianFuseCurrentState())
{
- SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUFusedCycle, this, m_FusedIMUState));
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,
+ CIMUEvent::eOnIMUFusedCycle,
+ this,
+ m_FusedIMUState));
if (IntegrateCurrentState())
{
- SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUIntegratedState, this, m_IntegratedIMUState));
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,
+ CIMUEvent::eOnIMUIntegratedState,
+ this,
+ m_IntegratedIMUState));
}
}
@@ -511,8 +646,14 @@ namespace IMU
if (IntegrateCurrentState())
{
- SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUFusedCycle, this, m_FusedIMUState));
- SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUIntegratedState, this, m_IntegratedIMUState));
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,
+ CIMUEvent::eOnIMUFusedCycle,
+ this,
+ m_FusedIMUState));
+ SendEvent(CIMUEvent(m_LastFrameTimeStamp,
+ CIMUEvent::eOnIMUIntegratedState,
+ this,
+ m_IntegratedIMUState));
}
break;
@@ -524,7 +665,8 @@ namespace IMU
return false;
}
- void CIMUDevice::SendEvent(const CIMUEvent& Event)
+ void
+ CIMUDevice::SendEvent(const CIMUEvent& Event)
{
_MINIMAL___LOCK(m_EventDispatchersMutex)
const unsigned long int TotalDispatchers = m_IMUEventDispatchers.size();
@@ -545,7 +687,8 @@ namespace IMU
_MINIMAL_UNLOCK(m_EventDispatchersMutex)
}
- void CIMUDevice::SetReferenceTimeStamps()
+ void
+ CIMUDevice::SetReferenceTimeStamps()
{
_MINIMAL___LOCK(m_EventDispatchersMutex)
gettimeofday(&m_ReferenceTimeStamp, nullptr);
@@ -560,7 +703,9 @@ namespace IMU
_MINIMAL_UNLOCK(m_EventDispatchersMutex)
}
- bool CIMUDevice::SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority)
+ bool
+ CIMUDevice::SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy,
+ const float NormalizedPriority)
{
if (m_IsActive)
{
@@ -572,9 +717,11 @@ namespace IMU
const int MaximalPriority = sched_get_priority_max(ThreadPolicy);
const int MinimalPriority = sched_get_priority_min(ThreadPolicy);
const int PriorityRange = MaximalPriority - MinimalPriority;
- const int Priority = int(round(float(PriorityRange) * NormalizedPriority + float(MinimalPriority)));
+ const int Priority =
+ int(round(float(PriorityRange) * NormalizedPriority + float(MinimalPriority)));
SchedulingParameters.sched_priority = Priority;
- const int Result = pthread_setschedparam(m_pInternalThreadHandel, ThreadPolicy, &SchedulingParameters);
+ const int Result = pthread_setschedparam(
+ m_pInternalThreadHandel, ThreadPolicy, &SchedulingParameters);
switch (Result)
{
@@ -583,19 +730,27 @@ namespace IMU
break;
case EINVAL:
- std::cerr << "[IMU Device error: SetThreadRunnigMode() returns EINVAL!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns "
+ "EINVAL!]\n[Source location: "
+ << __FILE__ << ":" << __LINE__ << "]" << std::endl;
break;
case ENOTSUP:
- std::cerr << "[IMU Device error: SetThreadRunnigMode() returns ENOTSUP!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns "
+ "ENOTSUP!]\n[Source location: "
+ << __FILE__ << ":" << __LINE__ << "]" << std::endl;
break;
case EPERM:
- std::cerr << "[IMU Device error: SetThreadRunnigMode() returns EPERM!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns "
+ "EPERM!]\n[Source location: "
+ << __FILE__ << ":" << __LINE__ << "]" << std::endl;
break;
case ESRCH:
- std::cerr << "[IMU Device error: SetThreadRunnigMode() returns ESRCH!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() returns "
+ "ESRCH!]\n[Source location: "
+ << __FILE__ << ":" << __LINE__ << "]" << std::endl;
break;
}
}
@@ -604,17 +759,20 @@ namespace IMU
}
else
{
- std::cerr << "[IMU Device error: SetThreadRunnigMode() cannot set running mode while thread is not active!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr << "[IMU Device error: SetThreadRunnigMode() cannot set running mode while "
+ "thread is not active!]\n[Source location: "
+ << __FILE__ << ":" << __LINE__ << "]" << std::endl;
return false;
}
}
- void* CIMUDevice::ThreadLoop(void* pData)
+ void*
+ CIMUDevice::ThreadLoop(void* pData)
{
if (pData)
{
- CIMUDevice* pIMUDevice = (CIMUDevice*) pData;
+ CIMUDevice* pIMUDevice = (CIMUDevice*)pData;
_MINIMAL___LOCK(pIMUDevice->m_IsActiveMutex)
pIMUDevice->m_IsActive = true;
@@ -638,10 +796,11 @@ namespace IMU
if (!DispatchingResult)
{
- std::cerr << "[IMU Device error: DispatchCylcle() returns false!]\n[Source location: " << __FILE__ << ":" << __LINE__ << "]" << std::endl;
+ std::cerr
+ << "[IMU Device error: DispatchCylcle() returns false!]\n[Source location: "
+ << __FILE__ << ":" << __LINE__ << "]" << std::endl;
break;
}
-
}
if (pIMUDevice->m_IsActive)
@@ -656,4 +815,4 @@ namespace IMU
return nullptr;
}
-}
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IMUDevice.h b/source/RobotAPI/drivers/XsensIMU/IMU/IMUDevice.h
index b3e925211346d9233138586c94c9c0018a9bcf59..62a3f3d795e8de4fe24fe2fd832259b3688eebb3 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IMUDevice.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IMUDevice.h
@@ -8,10 +8,10 @@
#pragma once
-#include "Includes.h"
-#include "IMUHelpers.h"
#include "IMUEvent.h"
+#include "IMUHelpers.h"
#include "IMUState.h"
+#include "Includes.h"
#ifdef _IMU_USE_XSENS_DEVICE_
@@ -41,13 +41,15 @@ namespace IMU
class CIMUDevice
{
public:
-
/*!
\brief Enum specifying the running thread policy.
*/
enum ThreadPolicyType
{
- eRealTime = SCHED_FIFO, eRoundRobinPriorityBased = SCHED_RR, eBatch = SCHED_BATCH, eIdle = SCHED_IDLE
+ eRealTime = SCHED_FIFO,
+ eRoundRobinPriorityBased = SCHED_RR,
+ eBatch = SCHED_BATCH,
+ eIdle = SCHED_IDLE
};
/*!
@@ -102,7 +104,9 @@ namespace IMU
enum FusionStrategy
{
- eNoFusion, eMeanFusion, eGaussianFusion
+ eNoFusion,
+ eMeanFusion,
+ eGaussianFusion
};
/*!
@@ -118,7 +122,8 @@ namespace IMU
uint64_t GetDeviceId() const;
- inline IMUState GetIMUState() const
+ inline IMUState
+ GetIMUState() const
{
#ifdef _IMU_USE_XSENS_DEVICE_
@@ -126,14 +131,14 @@ namespace IMU
return m_XsensMTiFrame.m_IMUState;
#endif
-
}
bool Connect(const std::string& PortName, const SamplingFrequency Frequency);
bool Start(const bool Blocking = true);
- bool SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority);
+ bool SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy,
+ const float NormalizedPriority);
void Stop(const bool Blocking = true);
@@ -145,13 +150,13 @@ namespace IMU
bool UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher);
- inline const timeval& GetReferenceTimeStamp() const
+ inline const timeval&
+ GetReferenceTimeStamp() const
{
return m_ReferenceTimeStamp;
}
protected:
-
bool LoadCurrentState();
bool MeanFuseCurrentState();
@@ -167,7 +172,6 @@ namespace IMU
bool IntegrateWithFusion();
private:
-
void UnregisterEventDispatchers();
bool InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency);
@@ -211,7 +215,5 @@ namespace IMU
Xsens::XsensMTiFrame m_XsensMTiFrame;
#endif
-
};
-}
-
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IMUEvent.cpp b/source/RobotAPI/drivers/XsensIMU/IMU/IMUEvent.cpp
index d9d6f712c7b2d5b7b2800edae2f8e9b0465a05d6..3a8b6501ba0c20ec5c664bd993ba095317b6a837 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IMUEvent.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IMUEvent.cpp
@@ -7,6 +7,7 @@
*/
#include "IMUEvent.h"
+
#include "IMUDevice.h"
namespace IMU
@@ -14,40 +15,64 @@ namespace IMU
uint32_t CIMUEvent::s_IdCounter = 0;
pthread_mutex_t CIMUEvent::s_IdCounterMutex = PTHREAD_MUTEX_INITIALIZER;
- CIMUEvent::CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice, const IMUState& EventState) :
+ CIMUEvent::CIMUEvent(const timeval& TimeStamp,
+ const EventType EventType,
+ const CIMUDevice* pIMUDevice,
+ const IMUState& EventState) :
m_Id(CreatId()),
- m_TimeStamp(TimeStamp), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(EventState)
+ m_TimeStamp(TimeStamp),
+ m_EventType(EventType),
+ m_pIMUDevice(pIMUDevice),
+ m_IMUState(EventState)
{
}
- CIMUEvent::CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice) :
- m_Id(CreatId()), m_TimeStamp(TimeStamp), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(pIMUDevice->GetIMUState())
+ CIMUEvent::CIMUEvent(const timeval& TimeStamp,
+ const EventType EventType,
+ const CIMUDevice* pIMUDevice) :
+ m_Id(CreatId()),
+ m_TimeStamp(TimeStamp),
+ m_EventType(EventType),
+ m_pIMUDevice(pIMUDevice),
+ m_IMUState(pIMUDevice->GetIMUState())
{
}
CIMUEvent::CIMUEvent(const EventType EventType, const CIMUDevice* pIMUDevice) :
- m_Id(CreatId()), m_TimeStamp(CTimeStamp::GetCurrentTimeStamp()), m_EventType(EventType), m_pIMUDevice(pIMUDevice), m_IMUState(pIMUDevice->GetIMUState())
+ m_Id(CreatId()),
+ m_TimeStamp(CTimeStamp::GetCurrentTimeStamp()),
+ m_EventType(EventType),
+ m_pIMUDevice(pIMUDevice),
+ m_IMUState(pIMUDevice->GetIMUState())
{
}
CIMUEvent::CIMUEvent(const CIMUEvent& Event) :
- m_Id(CreatId()), m_TimeStamp(Event.m_TimeStamp), m_EventType(Event.m_EventType), m_pIMUDevice(Event.m_pIMUDevice), m_IMUState(Event.m_IMUState)
+ m_Id(CreatId()),
+ m_TimeStamp(Event.m_TimeStamp),
+ m_EventType(Event.m_EventType),
+ m_pIMUDevice(Event.m_pIMUDevice),
+ m_IMUState(Event.m_IMUState)
{
}
CIMUEvent::CIMUEvent() :
- m_Id(CreatId()), m_TimeStamp(CTimeStamp::s_Zero), m_EventType(EventType(0x0)), m_pIMUDevice(nullptr), m_IMUState()
+ m_Id(CreatId()),
+ m_TimeStamp(CTimeStamp::s_Zero),
+ m_EventType(EventType(0x0)),
+ m_pIMUDevice(nullptr),
+ m_IMUState()
{
}
- CIMUEvent::~CIMUEvent()
- = default;
+ CIMUEvent::~CIMUEvent() = default;
- uint32_t CIMUEvent::CreatId()
+ uint32_t
+ CIMUEvent::CreatId()
{
pthread_mutex_lock(&s_IdCounterMutex);
uint32_t Id = CIMUEvent::s_IdCounter++;
pthread_mutex_unlock(&s_IdCounterMutex);
return Id;
}
-}
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IMUEvent.h b/source/RobotAPI/drivers/XsensIMU/IMU/IMUEvent.h
index ea70d2a91c457996f7db3440cabd18295ddfa08d..5c9b66088d171b8e2af7c5a6be1e60dff8abeb24 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IMUEvent.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IMUEvent.h
@@ -14,45 +14,58 @@
namespace IMU
{
class CIMUDevice;
+
class CIMUEvent
{
public:
-
enum EventType
{
- eOnIMUStart = 0X0001, eOnIMUStop = 0X0002, eOnIMUCycle = 0X0004, eOnIMUFusedCycle = 0X0008, eOnIMUIntegratedState = 0X0010, eOnIMUCustomEvent = 0X8000
+ eOnIMUStart = 0X0001,
+ eOnIMUStop = 0X0002,
+ eOnIMUCycle = 0X0004,
+ eOnIMUFusedCycle = 0X0008,
+ eOnIMUIntegratedState = 0X0010,
+ eOnIMUCustomEvent = 0X8000
};
- CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice, const IMUState& EventState);
- CIMUEvent(const timeval& TimeStamp, const EventType EventType, const CIMUDevice* pIMUDevice);
+ CIMUEvent(const timeval& TimeStamp,
+ const EventType EventType,
+ const CIMUDevice* pIMUDevice,
+ const IMUState& EventState);
+ CIMUEvent(const timeval& TimeStamp,
+ const EventType EventType,
+ const CIMUDevice* pIMUDevice);
CIMUEvent(const EventType EventType, const CIMUDevice* pIMUDevice);
CIMUEvent(const CIMUEvent& Event);
CIMUEvent();
virtual ~CIMUEvent();
- inline uint32_t GetId() const
+ inline uint32_t
+ GetId() const
{
return m_Id;
}
- inline EventType GetEventType() const
+ inline EventType
+ GetEventType() const
{
return m_EventType;
}
- inline const CIMUDevice* GetIMU() const
+ inline const CIMUDevice*
+ GetIMU() const
{
return m_pIMUDevice;
}
- inline const timeval& GetTimeStamp() const
+ inline const timeval&
+ GetTimeStamp() const
{
return m_TimeStamp;
}
protected:
-
uint32_t m_Id;
const timeval m_TimeStamp;
const EventType m_EventType;
@@ -60,10 +73,8 @@ namespace IMU
const IMUState m_IMUState;
private:
-
static uint32_t CreatId();
static uint32_t s_IdCounter;
static pthread_mutex_t s_IdCounterMutex;
};
-}
-
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IMUHelpers.cpp b/source/RobotAPI/drivers/XsensIMU/IMU/IMUHelpers.cpp
index 6a2d46c2f67ad6f134f125b6490009dec56a84f1..3a69894714bc4ba748ae982fff3d7dd96c6dcc15 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IMUHelpers.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IMUHelpers.cpp
@@ -10,9 +10,9 @@
namespace IMU
{
- const timeval CTimeStamp::s_Zero = { 0, 0 };
+ const timeval CTimeStamp::s_Zero = {0, 0};
const float CGeolocationInformation::s_G_LPoles = 9.832f;
const float CGeolocationInformation::s_G_L45 = 9.806f;
const float CGeolocationInformation::s_G_LEquator = 9.780f;
-}
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IMUHelpers.h b/source/RobotAPI/drivers/XsensIMU/IMU/IMUHelpers.h
index 40be79b0ace0b6594c5b3ef9688757b2d0a141dd..1bc7fc9841b41992788143f16ec8cc78a9103216 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IMUHelpers.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IMUHelpers.h
@@ -18,30 +18,34 @@ namespace IMU
class CTimeStamp
{
public:
-
- inline static timeval GetCurrentTimeStamp()
+ inline static timeval
+ GetCurrentTimeStamp()
{
timeval TimeStamp;
gettimeofday(&TimeStamp, NULL);
return TimeStamp;
}
- inline static float GetElapsedSeconds(const timeval& Post, const timeval& Pre)
+ inline static float
+ GetElapsedSeconds(const timeval& Post, const timeval& Pre)
{
return float(double(GetElapsedMicroseconds(Post, Pre)) / 1000000.0);
}
- inline static float GetElapsedMilliseconds(const timeval& Post, const timeval& Pre)
+ inline static float
+ GetElapsedMilliseconds(const timeval& Post, const timeval& Pre)
{
return float(double(GetElapsedMicroseconds(Post, Pre)) / 1000.0);
}
- inline static long GetElapsedMicroseconds(const timeval& Post, const timeval& Pre)
+ inline static long
+ GetElapsedMicroseconds(const timeval& Post, const timeval& Pre)
{
return ((Post.tv_sec - Pre.tv_sec) * 1000000) + (Post.tv_usec - Pre.tv_usec);
}
- inline static long GetElapsedMicroseconds(const timeval& Pre)
+ inline static long
+ GetElapsedMicroseconds(const timeval& Pre)
{
timeval Post;
gettimeofday(&Post, NULL);
@@ -54,7 +58,6 @@ namespace IMU
class CGeolocationInformation
{
public:
-
static const float s_G_LPoles;
static const float s_G_L45;
static const float s_G_LEquator;
@@ -63,13 +66,13 @@ namespace IMU
//http://www.mapsofworld.com/lat_long/germany-lat-long.html
//49.0167 Karlsruhe, Germany
- static float GetGravitationalAcceleration(const float LatitudeInDegrees = 49.0167f)
+ static float
+ GetGravitationalAcceleration(const float LatitudeInDegrees = 49.0167f)
{
- return s_G_L45 - (s_G_LPoles - s_G_LEquator) * std::cos((float(M_PI) / 90.0f) * LatitudeInDegrees);
+ return s_G_L45 - (s_G_LPoles - s_G_LEquator) *
+ std::cos((float(M_PI) / 90.0f) * LatitudeInDegrees);
}
};
-
-}
-
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/IMUState.h b/source/RobotAPI/drivers/XsensIMU/IMU/IMUState.h
index 0203e532f81187201a36c668b370c6d671ed5902..f190fa98008f3e6be4b1e850b2164ef375ba10cb 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/IMUState.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/IMUState.h
@@ -14,15 +14,17 @@ namespace IMU
{
struct IMUState
{
- inline IMUState() :
- m_ControlData(), m_PhysicalData()
+ inline IMUState() : m_ControlData(), m_PhysicalData()
{
}
struct ControlData
{
ControlData() :
- m_PreviousSampleCount(-1), m_CurrentSampleCount(0), m_MessageCounter(0), m_IsConsecutive(false)
+ m_PreviousSampleCount(-1),
+ m_CurrentSampleCount(0),
+ m_MessageCounter(0),
+ m_IsConsecutive(false)
{
memset(&m_TimeStamp, 0, sizeof(timeval));
}
@@ -36,8 +38,7 @@ namespace IMU
struct PhysicalData
{
- PhysicalData() :
- m_AccelerationMagnitud(0.0f)
+ PhysicalData() : m_AccelerationMagnitud(0.0f)
{
memset(m_Acceleration, 0, sizeof(float) * 3);
memset(m_GyroscopeRotation, 0, sizeof(float) * 3);
@@ -51,14 +52,16 @@ namespace IMU
float m_MagneticRotation[3];
float m_QuaternionRotation[4];
- inline void UpdateAccelerationMagnitud()
+ inline void
+ UpdateAccelerationMagnitud()
{
- m_AccelerationMagnitud = std::sqrt(m_Acceleration[0] * m_Acceleration[0] + m_Acceleration[1] * m_Acceleration[1] + m_Acceleration[2] * m_Acceleration[2]);
+ m_AccelerationMagnitud = std::sqrt(m_Acceleration[0] * m_Acceleration[0] +
+ m_Acceleration[1] * m_Acceleration[1] +
+ m_Acceleration[2] * m_Acceleration[2]);
}
};
ControlData m_ControlData;
PhysicalData m_PhysicalData;
};
-}
-
+} // namespace IMU
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/Includes.h b/source/RobotAPI/drivers/XsensIMU/IMU/Includes.h
index c934806a5001994c531aea93116e306da7a8177c..1e1a9252fb4e49ba70944f8f42207b315debc7e8 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/Includes.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/Includes.h
@@ -29,20 +29,23 @@
//GENERAL INCLUDES
/////////////////////////////////////////////////////////////////////////////
#include <stdint.h>
-#include <pthread.h>
#include <string.h>
-#include <set>
-#include <list>
-#include <iostream>
+
#include <cmath>
+#include <iostream>
+#include <list>
+#include <set>
+
+#include <pthread.h>
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
//LINUX INCLUDES
/////////////////////////////////////////////////////////////////////////////
-#include <sys/time.h>
-#include <sched.h>
#include <errno.h>
+
+#include <sched.h>
+#include <sys/time.h>
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
@@ -50,4 +53,3 @@
/////////////////////////////////////////////////////////////////////////////
#define _IMU_USE_XSENS_DEVICE_
/////////////////////////////////////////////////////////////////////////////
-
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/Xsens.h b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/Xsens.h
index 67d2a5e4adfb6e250012c9362d2c99339a5ec105..14435a51d66f053b5123a23a6bab4b1d632e60d9 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/Xsens.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/Xsens.h
@@ -7,8 +7,8 @@
#pragma once
-#include "../Includes.h"
#include "../IMUState.h"
+#include "../Includes.h"
#ifdef _IMU_USE_XSENS_DEVICE_
@@ -18,17 +18,15 @@ namespace IMU::Xsens
{
struct XsensMTiFrame
{
- XsensMTiFrame() :
- m_DataLength(0)
+ XsensMTiFrame() : m_DataLength(0)
{
memset(m_Data, 0, sizeof(unsigned char) * MAXMSGLEN);
}
short m_DataLength;
- unsigned char m_Data[MAXMSGLEN ];
+ unsigned char m_Data[MAXMSGLEN];
IMUState m_IMUState;
};
-}
+} // namespace IMU::Xsens
#endif
-
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
index 334aa4c34f4cfce8d189c7faba32973ffee785af..bb3aefffc4ba4f40aa332bafb9213da667142c41 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.cpp
@@ -132,13 +132,13 @@ namespace IMU::Xsens
m_handle = -1;
m_portOpen = false;
m_fileOpen = false;
- m_deviceError = 0; // No error
+ m_deviceError = 0; // No error
m_retVal = MTRV_OK;
m_timeOut = TO_DEFAULT;
m_nTempBufferLen = 0;
m_clkEnd = 0;
- for (int i = 0 ; i < MAXDEVICES + 1 ; i++)
+ for (int i = 0; i < MAXDEVICES + 1; i++)
{
m_storedOutputMode[i] = INVALIDSETTINGVALUE;
m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
@@ -162,11 +162,12 @@ namespace IMU::Xsens
// Output
// returns processor time in milliseconds
//
- clock_t CXsensMTiModule::clockms()
+ clock_t
+ CXsensMTiModule::clockms()
{
clock_t clk;
#ifdef WIN32
- clk = clock(); // Get current processor time
+ clk = clock(); // Get current processor time
#if (CLOCKS_PER_SEC != 1000)
clk /= (CLOCKS_PER_SEC / 1000);
// clk = (clk * 1000) / CLOCKS_PER_SEC;
@@ -194,7 +195,11 @@ namespace IMU::Xsens
// Output
// returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
//
- short CXsensMTiModule::openPort(const int portNumber, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
+ short
+ CXsensMTiModule::openPort(const int portNumber,
+ const unsigned long baudrate,
+ const unsigned long /*inqueueSize*/,
+ const unsigned long /*outqueueSize*/)
{
m_clkEnd = 0;
@@ -206,9 +211,10 @@ namespace IMU::Xsens
#ifdef WIN32
char pchFileName[10];
- sprintf(pchFileName, "\\\\.\\COM%d", portNumber); // Create file name
+ sprintf(pchFileName, "\\\\.\\COM%d", portNumber); // Create file name
- m_handle = CreateFile(pchFileName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
+ m_handle =
+ CreateFile(pchFileName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
if (m_handle == INVALID_HANDLE_VALUE)
{
@@ -221,14 +227,14 @@ namespace IMU::Xsens
//Get the current state & then change it
DCB dcb;
- GetCommState(m_handle, &dcb);// Get current state
+ GetCommState(m_handle, &dcb); // Get current state
- dcb.BaudRate = baudrate;// Setup the baud rate
- dcb.Parity = NOPARITY;// Setup the Parity
- dcb.ByteSize = 8;// Setup the data bits
- dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
- dcb.fDsrSensitivity = FALSE;// Setup the flow control
- dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
+ dcb.BaudRate = baudrate; // Setup the baud rate
+ dcb.Parity = NOPARITY; // Setup the Parity
+ dcb.ByteSize = 8; // Setup the data bits
+ dcb.StopBits = TWOSTOPBITS; // Setup the stop bits
+ dcb.fDsrSensitivity = FALSE; // Setup the flow control
+ dcb.fOutxCtsFlow = FALSE; // NoFlowControl:
dcb.fOutxDsrFlow = FALSE;
dcb.fOutX = FALSE;
dcb.fInX = FALSE;
@@ -264,10 +270,10 @@ namespace IMU::Xsens
// CommTimeouts.ReadIntervalTimeout = MAXDWORD;
// CommTimeouts.ReadTotalTimeoutMultiplier = 0;
- SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
+ SetCommTimeouts(m_handle, &CommTimeouts); // Set CommTimeouts structure
// Other initialization functions
- EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
+ EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use)
SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
// Remove any 'old' data in buffer
@@ -307,8 +313,8 @@ namespace IMU::Xsens
options.c_cflag |= (CLOCAL | CREAD);
// Set character size to data bits and set no parity Mask the characte size bits
options.c_cflag &= ~(CSIZE | PARENB);
- options.c_cflag |= CS8; // Select 8 data bits
- options.c_cflag |= CSTOPB; // send 2 stop bits
+ options.c_cflag |= CS8; // Select 8 data bits
+ options.c_cflag |= CSTOPB; // send 2 stop bits
// Disable hardware flow control
options.c_cflag &= ~CRTSCTS;
options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
@@ -343,7 +349,11 @@ namespace IMU::Xsens
// Output
// returns MTRV_OK if serial port is successfully opened, else MTRV_INPUTCANNOTBEOPENED
//
- short CXsensMTiModule::openPort(const char* portName, const unsigned long baudrate, const unsigned long /*inqueueSize*/, const unsigned long /*outqueueSize*/)
+ short
+ CXsensMTiModule::openPort(const char* portName,
+ const unsigned long baudrate,
+ const unsigned long /*inqueueSize*/,
+ const unsigned long /*outqueueSize*/)
{
m_clkEnd = 0;
@@ -353,7 +363,8 @@ namespace IMU::Xsens
}
#ifdef WIN32
- m_handle = CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
+ m_handle =
+ CreateFile(portName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
if (m_handle == INVALID_HANDLE_VALUE)
{
@@ -366,14 +377,14 @@ namespace IMU::Xsens
//Get the current state & then change it
DCB dcb;
- GetCommState(m_handle, &dcb);// Get current state
+ GetCommState(m_handle, &dcb); // Get current state
- dcb.BaudRate = baudrate;// Setup the baud rate
- dcb.Parity = NOPARITY;// Setup the Parity
- dcb.ByteSize = 8;// Setup the data bits
- dcb.StopBits = TWOSTOPBITS;// Setup the stop bits
- dcb.fDsrSensitivity = FALSE;// Setup the flow control
- dcb.fOutxCtsFlow = FALSE;// NoFlowControl:
+ dcb.BaudRate = baudrate; // Setup the baud rate
+ dcb.Parity = NOPARITY; // Setup the Parity
+ dcb.ByteSize = 8; // Setup the data bits
+ dcb.StopBits = TWOSTOPBITS; // Setup the stop bits
+ dcb.fDsrSensitivity = FALSE; // Setup the flow control
+ dcb.fOutxCtsFlow = FALSE; // NoFlowControl:
dcb.fOutxDsrFlow = FALSE;
dcb.fOutX = FALSE;
dcb.fInX = FALSE;
@@ -408,10 +419,10 @@ namespace IMU::Xsens
// CommTimeouts.ReadTotalTimeoutConstant = 0;
// CommTimeouts.ReadIntervalTimeout = MAXDWORD;
// CommTimeouts.ReadTotalTimeoutMultiplier = 0;
- SetCommTimeouts(m_handle, &CommTimeouts);// Set CommTimeouts structure
+ SetCommTimeouts(m_handle, &CommTimeouts); // Set CommTimeouts structure
// Other initialization functions
- EscapeCommFunction(m_handle, SETRTS);// Enable RTS (for Xbus Master use)
+ EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use)
SetupComm(m_handle, inqueueSize, outqueueSize); // Set queue size
// Remove any 'old' data in buffer
@@ -450,8 +461,8 @@ namespace IMU::Xsens
options.c_cflag |= (CLOCAL | CREAD);
// Set character size to data bits and set no parity Mask the characte size bits
options.c_cflag &= ~(CSIZE | PARENB);
- options.c_cflag |= CS8; // Select 8 data bits
- options.c_cflag |= CSTOPB; // send 2 stop bits
+ options.c_cflag |= CS8; // Select 8 data bits
+ options.c_cflag |= CSTOPB; // send 2 stop bits
// Disable hardware flow control
options.c_cflag &= ~CRTSCTS;
options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
@@ -487,7 +498,8 @@ namespace IMU::Xsens
// returns MTRV_INPUTCANNOTBEOPENED if the file can not be opened
// returns MTRV_ANINPUTALREADYOPEN if a serial port / file is already opened
//
- short CXsensMTiModule::openFile(const char* fileName, bool createAlways)
+ short
+ CXsensMTiModule::openFile(const char* fileName, bool createAlways)
{
m_clkEnd = 0;
@@ -504,7 +516,8 @@ namespace IMU::Xsens
disposition = CREATE_ALWAYS;
}
- m_handle = CreateFile(fileName, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ, NULL, disposition, 0, NULL);
+ m_handle = CreateFile(
+ fileName, GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ, NULL, disposition, 0, NULL);
if (m_handle == INVALID_HANDLE_VALUE)
{
@@ -527,10 +540,9 @@ namespace IMU::Xsens
}
#endif
- m_timeOut = 0; // No timeout when using file input
+ m_timeOut = 0; // No timeout when using file input
m_fileOpen = true;
return (m_retVal = MTRV_OK);
-
}
////////////////////////////////////////////////////////////////////
@@ -538,7 +550,8 @@ namespace IMU::Xsens
//
// Return if serial port is open or not
//
- bool CXsensMTiModule::isPortOpen()
+ bool
+ CXsensMTiModule::isPortOpen()
{
return m_portOpen;
}
@@ -548,7 +561,8 @@ namespace IMU::Xsens
//
// Return if serial port is open or not
//
- bool CXsensMTiModule::isFileOpen()
+ bool
+ CXsensMTiModule::isFileOpen()
{
return m_fileOpen;
}
@@ -565,7 +579,8 @@ namespace IMU::Xsens
//
// Output
// number of bytes actually read
- int CXsensMTiModule::readData(unsigned char* msgBuffer, const int nBytesToRead)
+ int
+ CXsensMTiModule::readData(unsigned char* msgBuffer, const int nBytesToRead)
{
//if(!m_fileOpen && !m_portOpen)
if (!(m_portOpen || m_fileOpen))
@@ -655,7 +670,8 @@ namespace IMU::Xsens
// number of bytes actually written
//
// The MTComm return value is != MTRV_OK if serial port is closed
- int CXsensMTiModule::writeData(const unsigned char* msgBuffer, const int nBytesToWrite)
+ int
+ CXsensMTiModule::writeData(const unsigned char* msgBuffer, const int nBytesToWrite)
{
if (!m_fileOpen && !m_portOpen)
{
@@ -679,7 +695,8 @@ namespace IMU::Xsens
// Remove any 'old' data in COM port buffer and flushes internal
// temporary buffer
//
- void CXsensMTiModule::flush()
+ void
+ CXsensMTiModule::flush()
{
if (m_portOpen)
{
@@ -703,7 +720,8 @@ namespace IMU::Xsens
// Input
// function : Windows define. Can be one of following:
// CLRDTR, CLRRTS, SETDTR, SETRTS, SETXOFF, SETXON, SETBREAK, CLRBREAK
- void CXsensMTiModule::escape(unsigned long /*function*/)
+ void
+ CXsensMTiModule::escape(unsigned long /*function*/)
{
#ifdef WIN32
EscapeCommFunction(m_handle, function);
@@ -716,7 +734,9 @@ namespace IMU::Xsens
//
// Set input and output buffer size of serial port
//
- void CXsensMTiModule::setPortQueueSize(const unsigned long /*inqueueSize = 4096 */, const unsigned long /*outqueueSize = 1024 */)
+ void
+ CXsensMTiModule::setPortQueueSize(const unsigned long /*inqueueSize = 4096 */,
+ const unsigned long /*outqueueSize = 1024 */)
{
if (m_portOpen)
{
@@ -743,7 +763,8 @@ namespace IMU::Xsens
//
// returns MTRV_OK if file pointer is successfully set
//
- short CXsensMTiModule::setFilePos(long relPos, unsigned long moveMethod)
+ short
+ CXsensMTiModule::setFilePos(long relPos, unsigned long moveMethod)
{
#ifdef WIN32
@@ -780,7 +801,8 @@ namespace IMU::Xsens
//
// returns MTRV_OK if successful
//
- short CXsensMTiModule::getFileSize(unsigned long& fileSize)
+ short
+ CXsensMTiModule::getFileSize(unsigned long& fileSize)
{
#ifdef WIN32
@@ -814,7 +836,8 @@ namespace IMU::Xsens
//
// Closes handle of file or serial port
//
- short CXsensMTiModule::close()
+ short
+ CXsensMTiModule::close()
{
if (m_portOpen || m_fileOpen)
{
@@ -833,12 +856,12 @@ namespace IMU::Xsens
}
m_fileOpen = m_portOpen = false;
- m_timeOut = TO_DEFAULT; // Restore timeout value (file input)
+ m_timeOut = TO_DEFAULT; // Restore timeout value (file input)
m_clkEnd = 0;
m_nTempBufferLen = 0;
- m_deviceError = 0; // No error
+ m_deviceError = 0; // No error
- for (int i = 0 ; i < MAXDEVICES + 1 ; i++)
+ for (int i = 0; i < MAXDEVICES + 1; i++)
{
m_storedOutputMode[i] = INVALIDSETTINGVALUE;
m_storedOutputSettings[i] = INVALIDSETTINGVALUE;
@@ -866,9 +889,13 @@ namespace IMU::Xsens
// Remarks
// allocate enough memory for message buffer
// use setTimeOut for different timeout value
- short CXsensMTiModule::readMessage(unsigned char& mid, unsigned char data[], short& dataLen, unsigned char* bid)
+ short
+ CXsensMTiModule::readMessage(unsigned char& mid,
+ unsigned char data[],
+ short& dataLen,
+ unsigned char* bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (!(m_fileOpen || m_portOpen))
@@ -915,14 +942,15 @@ namespace IMU::Xsens
// Remarks
// allocate enough memory for data buffer
// use setTimeOut for different timeout value
- short CXsensMTiModule::readDataMessage(unsigned char data[], short& dataLen)
+ short
+ CXsensMTiModule::readDataMessage(unsigned char data[], short& dataLen)
{
if (!(m_portOpen || m_fileOpen))
{
return (m_retVal = MTRV_NOINPUTINITIALIZED);
}
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short buflen;
if (readMessageRaw(buffer, &buflen) == MTRV_OK)
@@ -946,7 +974,7 @@ namespace IMU::Xsens
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
}
@@ -967,7 +995,8 @@ namespace IMU::Xsens
// Remarks
// allocate enough memory for message buffer
// use setTimeOut for different timeout value
- short CXsensMTiModule::readMessageRaw(unsigned char* msgBuffer, short* msgBufferLength)
+ short
+ CXsensMTiModule::readMessageRaw(unsigned char* msgBuffer, short* msgBufferLength)
{
int state = 0;
int nBytesToRead = 1;
@@ -991,8 +1020,8 @@ namespace IMU::Xsens
m_nTempBufferLen = 0;
}
- clock_t clkStart = clockms(); // Get current processor time
- clock_t clkEnd = m_clkEnd; // check if the end timer is already set
+ clock_t clkStart = clockms(); // Get current processor time
+ clock_t clkEnd = m_clkEnd; // check if the end timer is already set
if (clkEnd == 0)
{
@@ -1039,7 +1068,7 @@ namespace IMU::Xsens
{
switch (state)
{
- case 0: // Check preamble
+ case 0: // Check preamble
if (msgBuffer[IND_PREAMBLE] == PREAMBLE)
{
state++;
@@ -1053,28 +1082,31 @@ namespace IMU::Xsens
break;
- case 1: // Check ADDR, MID, LEN
+ case 1: // Check ADDR, MID, LEN
if (msgBuffer[IND_LEN] != 0xFF)
{
state = 3;
- nBytesToRead = (nMsgDataLen = msgBuffer[IND_LEN]) + 1; // Read LEN + 1 (chksum)
+ nBytesToRead =
+ (nMsgDataLen = msgBuffer[IND_LEN]) + 1; // Read LEN + 1 (chksum)
}
else
{
state = 2;
- nBytesToRead = 2; // Read extended length
+ nBytesToRead = 2; // Read extended length
}
break;
case 2:
state = 3;
- nBytesToRead = (nMsgDataLen = msgBuffer[IND_LENEXTH] * 256 + msgBuffer[IND_LENEXTL]) + 1; // Read LENEXT + CS
+ nBytesToRead = (nMsgDataLen = msgBuffer[IND_LENEXTH] * 256 +
+ msgBuffer[IND_LENEXTL]) +
+ 1; // Read LENEXT + CS
if (nMsgDataLen > MAXMSGLEN - LEN_MSGEXTHEADERCS)
{
// Not synced - check for preamble in the bytes read
- for (int i = 1 ; i < LEN_MSGEXTHEADER ; i++)
+ for (int i = 1; i < LEN_MSGEXTHEADER; i++)
if (msgBuffer[i] == PREAMBLE)
{
// Found a maybe preamble - 'fill buffer'
@@ -1086,16 +1118,16 @@ namespace IMU::Xsens
Synced = false;
nOffset = 0;
state = 0;
- nBytesToRead = 1; // Start looking for preamble
+ nBytesToRead = 1; // Start looking for preamble
}
break;
- case 3: // Check msg
+ case 3: // Check msg
chCheckSum = 0;
nMsgLen = nMsgDataLen + 5 + (msgBuffer[IND_LEN] == 0xFF ? 2 : 0);
- for (int i = 1 ; i < nMsgLen ; i++)
+ for (int i = 1; i < nMsgLen; i++)
{
chCheckSum += msgBuffer[i];
}
@@ -1117,7 +1149,9 @@ namespace IMU::Xsens
else
{
if (!Synced)
- for (int i = 1 ; i < nMsgLen ; i++) // Not synced - maybe recheck for preamble in this incorrect message
+ for (
+ int i = 1; i < nMsgLen;
+ i++) // Not synced - maybe recheck for preamble in this incorrect message
if (msgBuffer[i] == PREAMBLE)
{
// Found a maybe preamble - 'fill buffer'
@@ -1129,14 +1163,13 @@ namespace IMU::Xsens
Synced = false;
nOffset = 0;
state = 0;
- nBytesToRead = 1; // Start looking for preamble
+ nBytesToRead = 1; // Start looking for preamble
}
break;
}
}
- }
- while ((clkEnd >= clockms()) || m_timeOut == 0 || nBytesRead != 0);
+ } while ((clkEnd >= clockms()) || m_timeOut == 0 || nBytesRead != 0);
// Check if pending message has a valid message
if (state > 0)
@@ -1144,11 +1177,11 @@ namespace IMU::Xsens
int i;
// Search for preamble
- for (i = 1; i < nOffset ; i++)
+ for (i = 1; i < nOffset; i++)
if (msgBuffer[i] == PREAMBLE)
{
// Found a maybe preamble - 'fill buffer'
- nBytesRead = nOffset - i - 1; // no preamble
+ nBytesRead = nOffset - i - 1; // no preamble
memmove(msgBuffer + 1, msgBuffer + i + 1, nBytesRead);
break;
}
@@ -1158,7 +1191,7 @@ namespace IMU::Xsens
// Found preamble in message - recheck
nOffset = 1;
state = 1;
- nBytesToRead = 3; // Start looking for preamble
+ nBytesToRead = 3; // Start looking for preamble
continue;
}
}
@@ -1192,9 +1225,13 @@ namespace IMU::Xsens
// = MTRV_TIMEOUT if timeout occurred
// = MTRV_NOINPUTINITIALIZED
//
- short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned long dataValue, const unsigned char dataValueLen, const unsigned char bid)
+ short
+ CXsensMTiModule::writeMessage(const unsigned char mid,
+ const unsigned long dataValue,
+ const unsigned char dataValueLen,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (!(m_fileOpen || m_portOpen))
@@ -1209,7 +1246,7 @@ namespace IMU::Xsens
if (dataValueLen)
{
- swapEndian((const unsigned char*) &dataValue, &buffer[IND_DATA0], dataValueLen);
+ swapEndian((const unsigned char*)&dataValue, &buffer[IND_DATA0], dataValueLen);
}
calcChecksum(buffer, LEN_MSGHEADER + dataValueLen);
@@ -1227,7 +1264,7 @@ namespace IMU::Xsens
clock_t clkStart, clkOld;
bool msgRead = false;
- clkStart = clockms(); // Get current processor time
+ clkStart = clockms(); // Get current processor time
clkOld = m_clkEnd;
if (clkOld == 0)
@@ -1245,13 +1282,13 @@ namespace IMU::Xsens
if (buffer[IND_MID] == (mid + 1))
{
m_clkEnd = clkOld;
- return (m_retVal = MTRV_OK); // Acknowledge received
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
m_clkEnd = clkOld;
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
}
}
@@ -1288,9 +1325,13 @@ namespace IMU::Xsens
// = MTRV_TIMEOUT if timeout occurred
// = MTRV_NOINPUTINITIALIZED
//
- short CXsensMTiModule::writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short& dataLen, const unsigned char bid)
+ short
+ CXsensMTiModule::writeMessage(const unsigned char mid,
+ const unsigned char data[],
+ const unsigned short& dataLen,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
short headerLength;
@@ -1306,7 +1347,7 @@ namespace IMU::Xsens
if (dataLen < EXTLENCODE)
{
- buffer[IND_LEN] = (unsigned char) dataLen;
+ buffer[IND_LEN] = (unsigned char)dataLen;
headerLength = LEN_MSGHEADER;
}
else
@@ -1333,7 +1374,7 @@ namespace IMU::Xsens
bool msgRead = false;
clock_t clkStart, clkOld;
- clkStart = clockms(); // Get current processor time
+ clkStart = clockms(); // Get current processor time
clkOld = m_clkEnd;
if (clkOld == 0)
@@ -1351,13 +1392,13 @@ namespace IMU::Xsens
if (buffer[IND_MID] == (mid + 1))
{
m_clkEnd = clkOld;
- return (m_retVal = MTRV_OK); // Acknowledge received
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
m_clkEnd = clkOld;
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
}
}
@@ -1393,9 +1434,13 @@ namespace IMU::Xsens
// Remarks
// allocate enough memory for data message buffer
// use setTimeOut for different timeout value
- short CXsensMTiModule::waitForMessage(const unsigned char mid, unsigned char data[], short* dataLen, unsigned char* bid)
+ short
+ CXsensMTiModule::waitForMessage(const unsigned char mid,
+ unsigned char data[],
+ short* dataLen,
+ unsigned char* bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short buflen;
clock_t clkStart, clkOld;
@@ -1405,7 +1450,7 @@ namespace IMU::Xsens
return (m_retVal = MTRV_NOINPUTINITIALIZED);
}
- clkStart = clockms(); // Get current processor time
+ clkStart = clockms(); // Get current processor time
clkOld = m_clkEnd;
if (clkOld == 0)
@@ -1475,9 +1520,12 @@ namespace IMU::Xsens
//
// value contains the integer value of the data field of the ack message
//
- short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned long& value, const unsigned char bid)
+ short
+ CXsensMTiModule::reqSetting(const unsigned char mid,
+ unsigned long& value,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -1507,17 +1555,17 @@ namespace IMU::Xsens
{
// Acknowledge received
value = 0;
- swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
+ swapEndian(&buffer[IND_DATA0], (unsigned char*)&value, buffer[IND_LEN]);
return (m_retVal = MTRV_OK);
}
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
}
@@ -1543,9 +1591,13 @@ namespace IMU::Xsens
//
// value contains the integer value of the data field of the ack message
//
- short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned long& value, const unsigned char bid)
+ short
+ CXsensMTiModule::reqSetting(const unsigned char mid,
+ const unsigned char param,
+ unsigned long& value,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -1588,11 +1640,11 @@ namespace IMU::Xsens
if (param == 0xFF)
{
- swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
+ swapEndian(&buffer[IND_DATA0], (unsigned char*)&value, buffer[IND_LEN]);
}
else
{
- swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*) &value, buffer[IND_LEN] - 1);
+ swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*)&value, buffer[IND_LEN] - 1);
}
return (m_retVal = MTRV_OK);
@@ -1600,11 +1652,11 @@ namespace IMU::Xsens
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
}
@@ -1628,9 +1680,10 @@ namespace IMU::Xsens
//
// value contains the float value of the acknowledge data field
//
- short CXsensMTiModule::reqSetting(const unsigned char mid, float& value, const unsigned char bid)
+ short
+ CXsensMTiModule::reqSetting(const unsigned char mid, float& value, const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -1660,17 +1713,17 @@ namespace IMU::Xsens
{
// Acknowledge received
value = 0;
- swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
+ swapEndian(&buffer[IND_DATA0], (unsigned char*)&value, buffer[IND_LEN]);
return (m_retVal = MTRV_OK);
}
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
}
@@ -1695,9 +1748,13 @@ namespace IMU::Xsens
//
// value contains the float value of the acknowledge data field
//
- short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, float& value, const unsigned char bid)
+ short
+ CXsensMTiModule::reqSetting(const unsigned char mid,
+ const unsigned char param,
+ float& value,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -1740,11 +1797,11 @@ namespace IMU::Xsens
if (param == 0xFF)
{
- swapEndian(&buffer[IND_DATA0], (unsigned char*) &value, buffer[IND_LEN]);
+ swapEndian(&buffer[IND_DATA0], (unsigned char*)&value, buffer[IND_LEN]);
}
else
{
- swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*) &value, buffer[IND_LEN] - 1);
+ swapEndian(&buffer[IND_DATA0] + 1, (unsigned char*)&value, buffer[IND_LEN] - 1);
}
return (m_retVal = MTRV_OK);
@@ -1752,11 +1809,11 @@ namespace IMU::Xsens
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
}
@@ -1781,9 +1838,13 @@ namespace IMU::Xsens
// data[] contains the data of the acknowledge message
// dataLen contains the number bytes returned
//
- short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char data[], short& dataLen, const unsigned char bid)
+ short
+ CXsensMTiModule::reqSetting(const unsigned char mid,
+ unsigned char data[],
+ short& dataLen,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -1830,11 +1891,11 @@ namespace IMU::Xsens
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
}
@@ -1861,9 +1922,15 @@ namespace IMU::Xsens
// dataOut[] contains the data of the acknowledge message
// dataOutLen contains the number bytes returned
//
- short CXsensMTiModule::reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short& dataOutLen, const unsigned char bid)
+ short
+ CXsensMTiModule::reqSetting(const unsigned char mid,
+ unsigned char dataIn[],
+ short dataInLen,
+ unsigned char dataOut[],
+ short& dataOutLen,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short headerLength;
short msgLen;
@@ -1883,7 +1950,7 @@ namespace IMU::Xsens
if (dataInLen < EXTLENCODE)
{
- buffer[IND_LEN] = (unsigned char) dataInLen;
+ buffer[IND_LEN] = (unsigned char)dataInLen;
headerLength = LEN_MSGHEADER;
}
else
@@ -1925,11 +1992,11 @@ namespace IMU::Xsens
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
}
@@ -1955,9 +2022,14 @@ namespace IMU::Xsens
// data[] contains the data of the acknowledge message (including param!!)
// dataLen contains the number bytes returned
//
- short CXsensMTiModule::reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short& dataLen, const unsigned char bid)
+ short
+ CXsensMTiModule::reqSetting(const unsigned char mid,
+ const unsigned char param,
+ unsigned char data[],
+ short& dataLen,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -2014,11 +2086,11 @@ namespace IMU::Xsens
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
else
{
- return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
+ return (m_retVal = MTRV_UNEXPECTEDMSG); // Unexpected message
}
}
@@ -2044,9 +2116,13 @@ namespace IMU::Xsens
// = MTRV_TIMEOUT if timeout occurred
//
//
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
+ short
+ CXsensMTiModule::setSetting(const unsigned char mid,
+ const unsigned long value,
+ const unsigned short valuelen,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -2063,8 +2139,8 @@ namespace IMU::Xsens
buffer[IND_PREAMBLE] = PREAMBLE;
buffer[IND_BID] = bid;
buffer[IND_MID] = mid;
- buffer[IND_LEN] = (unsigned char) valuelen;
- swapEndian((unsigned char*) &value, &buffer[msgLen], valuelen);
+ buffer[IND_LEN] = (unsigned char)valuelen;
+ swapEndian((unsigned char*)&value, &buffer[msgLen], valuelen);
calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
// Send message
@@ -2076,12 +2152,12 @@ namespace IMU::Xsens
// Message received
if (buffer[IND_MID] == (mid + 1))
{
- return (m_retVal = MTRV_OK); // Acknowledge received
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
}
@@ -2108,9 +2184,14 @@ namespace IMU::Xsens
// = MTRV_TIMEOUT if timeout occurred
//
//
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid)
+ short
+ CXsensMTiModule::setSetting(const unsigned char mid,
+ const unsigned char param,
+ const unsigned long value,
+ const unsigned short valuelen,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -2136,10 +2217,10 @@ namespace IMU::Xsens
}
else
{
- buffer[IND_LEN] = (unsigned char) valuelen;
+ buffer[IND_LEN] = (unsigned char)valuelen;
}
- swapEndian((unsigned char*) &value, &buffer[msgLen], valuelen);
+ swapEndian((unsigned char*)&value, &buffer[msgLen], valuelen);
calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
// Send message
@@ -2151,12 +2232,12 @@ namespace IMU::Xsens
// Message received
if (buffer[IND_MID] == (mid + 1))
{
- return (m_retVal = MTRV_OK); // Acknowledge received
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
}
@@ -2179,9 +2260,10 @@ namespace IMU::Xsens
// = MTRV_RECVERRORMSG if an error message is received
// = MTRV_TIMEOUT if timeout occurred
//
- short CXsensMTiModule::setSetting(const unsigned char mid, const float value, const unsigned char bid)
+ short
+ CXsensMTiModule::setSetting(const unsigned char mid, const float value, const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -2199,7 +2281,7 @@ namespace IMU::Xsens
buffer[IND_BID] = bid;
buffer[IND_MID] = mid;
buffer[IND_LEN] = LEN_FLOAT;
- swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
+ swapEndian((unsigned char*)&value, &buffer[msgLen], LEN_FLOAT);
calcChecksum(buffer, LEN_MSGHEADER + LEN_FLOAT);
// Send message
@@ -2211,12 +2293,12 @@ namespace IMU::Xsens
// Message received
if (buffer[IND_MID] == (mid + 1))
{
- return (m_retVal = MTRV_OK); // Acknowledge received
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
}
@@ -2241,9 +2323,13 @@ namespace IMU::Xsens
// = MTRV_TIMEOUT if timeout occurred
//
//
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid)
+ short
+ CXsensMTiModule::setSetting(const unsigned char mid,
+ const unsigned char param,
+ const float value,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -2272,7 +2358,7 @@ namespace IMU::Xsens
buffer[IND_LEN] = LEN_FLOAT;
}
- swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
+ swapEndian((unsigned char*)&value, &buffer[msgLen], LEN_FLOAT);
calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
// Send message
@@ -2284,12 +2370,12 @@ namespace IMU::Xsens
// Message received
if (buffer[IND_MID] == (mid + 1))
{
- return (m_retVal = MTRV_OK); // Acknowledge received
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
}
@@ -2315,9 +2401,14 @@ namespace IMU::Xsens
// = MTRV_TIMEOUT if timeout occurred
//
//
- short CXsensMTiModule::setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid)
+ short
+ CXsensMTiModule::setSetting(const unsigned char mid,
+ const unsigned char param,
+ const float value,
+ const bool store,
+ const unsigned char bid)
{
- unsigned char buffer[MAXMSGLEN ];
+ unsigned char buffer[MAXMSGLEN];
short msgLen;
if (m_fileOpen)
@@ -2346,8 +2437,8 @@ namespace IMU::Xsens
buffer[IND_LEN] = LEN_FLOAT + 1;
}
- swapEndian((unsigned char*) &value, &buffer[msgLen], LEN_FLOAT);
- buffer[msgLen + LEN_FLOAT ] = store;
+ swapEndian((unsigned char*)&value, &buffer[msgLen], LEN_FLOAT);
+ buffer[msgLen + LEN_FLOAT] = store;
calcChecksum(buffer, LEN_MSGHEADER + buffer[IND_LEN]);
// Send message
@@ -2359,12 +2450,12 @@ namespace IMU::Xsens
// Message received
if (buffer[IND_MID] == (mid + 1))
{
- return (m_retVal = MTRV_OK); // Acknowledge received
+ return (m_retVal = MTRV_OK); // Acknowledge received
}
else if (buffer[IND_MID] == MID_ERROR)
{
m_deviceError = buffer[IND_DATA0];
- return (m_retVal = MTRV_RECVERRORMSG); // Error message received
+ return (m_retVal = MTRV_RECVERRORMSG); // Error message received
}
}
@@ -2391,9 +2482,10 @@ namespace IMU::Xsens
//
// returns MTRV_OK if the mode & settings are read
//
- short CXsensMTiModule::getDeviceMode(unsigned short* numDevices)
+ short
+ CXsensMTiModule::getDeviceMode(unsigned short* numDevices)
{
- unsigned char mid = 0, data[MAXMSGLEN ];
+ unsigned char mid = 0, data[MAXMSGLEN];
short datalen;
if (numDevices != nullptr)
@@ -2410,19 +2502,23 @@ namespace IMU::Xsens
}
// Retrieve outputmode + outputsettings
- for (int i = 0 ; i < datalen / LEN_DEVICEID ; i++)
+ for (int i = 0; i < datalen / LEN_DEVICEID; i++)
{
- if (reqSetting(MID_REQOUTPUTMODE, m_storedOutputMode[BID_MT + i], BID_MT + i) != MTRV_OK)
+ if (reqSetting(MID_REQOUTPUTMODE, m_storedOutputMode[BID_MT + i], BID_MT + i) !=
+ MTRV_OK)
{
return m_retVal;
}
- if (reqSetting(MID_REQOUTPUTSETTINGS, m_storedOutputSettings[BID_MT + i], BID_MT + i) != MTRV_OK)
+ if (reqSetting(MID_REQOUTPUTSETTINGS,
+ m_storedOutputSettings[BID_MT + i],
+ BID_MT + i) != MTRV_OK)
{
return m_retVal;
}
- if (reqSetting(MID_REQDATALENGTH, m_storedDataLength[BID_MT + i], BID_MT + i) != MTRV_OK)
+ if (reqSetting(MID_REQDATALENGTH, m_storedDataLength[BID_MT + i], BID_MT + i) !=
+ MTRV_OK)
{
return m_retVal;
}
@@ -2450,9 +2546,9 @@ namespace IMU::Xsens
}
else
{
- m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
- m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
- m_storedDataLength[0] = m_storedDataLength[BID_MT ];
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT];
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT];
+ m_storedDataLength[0] = m_storedDataLength[BID_MT];
}
return (m_retVal = MTRV_OK);
@@ -2467,18 +2563,22 @@ namespace IMU::Xsens
if (mid == MID_CONFIGURATION)
{
unsigned short _numDevices = 0;
- swapEndian(data + CONF_NUMDEVICES, (unsigned char*) &_numDevices, CONF_NUMDEVICESLEN);
+ swapEndian(
+ data + CONF_NUMDEVICES, (unsigned char*)&_numDevices, CONF_NUMDEVICESLEN);
- for (unsigned int i = 0 ; i < _numDevices ; i++)
+ for (unsigned int i = 0; i < _numDevices; i++)
{
m_storedOutputMode[BID_MT + i] = 0;
- swapEndian(data + CONF_OUTPUTMODE + i * CONF_BLOCKLEN, (unsigned char*)(m_storedOutputMode + BID_MT + i),
+ swapEndian(data + CONF_OUTPUTMODE + i * CONF_BLOCKLEN,
+ (unsigned char*)(m_storedOutputMode + BID_MT + i),
CONF_OUTPUTMODELEN);
m_storedOutputSettings[BID_MT + i] = 0;
- swapEndian(data + CONF_OUTPUTSETTINGS + i * CONF_BLOCKLEN, (unsigned char*)(m_storedOutputSettings + BID_MT + i),
+ swapEndian(data + CONF_OUTPUTSETTINGS + i * CONF_BLOCKLEN,
+ (unsigned char*)(m_storedOutputSettings + BID_MT + i),
CONF_OUTPUTSETTINGSLEN);
m_storedDataLength[BID_MT + i] = 0;
- swapEndian(data + CONF_DATALENGTH + i * CONF_BLOCKLEN, (unsigned char*)(m_storedDataLength + BID_MT + i),
+ swapEndian(data + CONF_DATALENGTH + i * CONF_BLOCKLEN,
+ (unsigned char*)(m_storedDataLength + BID_MT + i),
CONF_DATALENGTHLEN);
}
@@ -2496,9 +2596,9 @@ namespace IMU::Xsens
}
else
{
- m_storedOutputMode[0] = m_storedOutputMode[BID_MT ];
- m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ];
- m_storedDataLength[0] = m_storedDataLength[BID_MT ];
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT];
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT];
+ m_storedDataLength[0] = m_storedDataLength[BID_MT];
}
return (m_retVal = MTRV_OK);
@@ -2526,7 +2626,10 @@ namespace IMU::Xsens
//
// returns MTRV_OK if the mode & settings are read
//
- short CXsensMTiModule::setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
+ short
+ CXsensMTiModule::setDeviceMode(unsigned long OutputMode,
+ unsigned long OutputSettings,
+ const unsigned char bid)
{
// In case serial port is used (live XM / MT)
if (m_portOpen)
@@ -2539,7 +2642,7 @@ namespace IMU::Xsens
if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
{
- m_storedOutputMode[0] = m_storedOutputMode[BID_MT ] = OutputMode;
+ m_storedOutputMode[0] = m_storedOutputMode[BID_MT] = OutputMode;
}
else
{
@@ -2547,14 +2650,15 @@ namespace IMU::Xsens
}
// Set OutputSettings
- if (setSetting(MID_SETOUTPUTSETTINGS, OutputSettings, LEN_OUTPUTSETTINGS, bid) != MTRV_OK)
+ if (setSetting(MID_SETOUTPUTSETTINGS, OutputSettings, LEN_OUTPUTSETTINGS, bid) !=
+ MTRV_OK)
{
return m_retVal;
}
if (bid == BID_MASTER || (bid == BID_MT && m_storedOutputMode[0] != OUTPUTMODE_XM))
{
- m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT ] = OutputSettings;
+ m_storedOutputSettings[0] = m_storedOutputSettings[BID_MT] = OutputSettings;
}
else
{
@@ -2568,9 +2672,10 @@ namespace IMU::Xsens
if (reqSetting(MID_REQDATALENGTH, value, bid) == MTRV_OK)
{
- if ((bid == BID_MASTER) || ((bid == BID_MT) && (m_storedOutputMode[0] != OUTPUTMODE_XM)))
+ if ((bid == BID_MASTER) ||
+ ((bid == BID_MT) && (m_storedOutputMode[0] != OUTPUTMODE_XM)))
{
- m_storedDataLength[0] = m_storedDataLength[BID_MT ] = value;
+ m_storedDataLength[0] = m_storedDataLength[BID_MT] = value;
}
else
{
@@ -2603,12 +2708,16 @@ namespace IMU::Xsens
//
// returns always MTRV_OK
//
- short CXsensMTiModule::getMode(unsigned long& OutputMode, unsigned long& OutputSettings, unsigned short& dataLength, const unsigned char bid)
+ short
+ CXsensMTiModule::getMode(unsigned long& OutputMode,
+ unsigned long& OutputSettings,
+ unsigned short& dataLength,
+ const unsigned char bid)
{
unsigned char nbid = (bid == BID_MASTER) ? 0 : bid;
OutputMode = m_storedOutputMode[nbid];
OutputSettings = m_storedOutputSettings[nbid];
- dataLength = (unsigned short) m_storedDataLength[nbid];
+ dataLength = (unsigned short)m_storedDataLength[nbid];
return (m_retVal = MTRV_OK);
}
@@ -2626,7 +2735,10 @@ namespace IMU::Xsens
//
// returns always MTRV_OK
//
- short CXsensMTiModule::setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid)
+ short
+ CXsensMTiModule::setMode(unsigned long OutputMode,
+ unsigned long OutputSettings,
+ const unsigned char bid)
{
unsigned char nbid = bid;
@@ -2648,7 +2760,10 @@ namespace IMU::Xsens
if (OutputMode & OUTPUTMODE_MT9)
{
- dataLength = ((OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0 + LEN_RAWDATA;
+ dataLength = ((OutputSettings & OUTPUTSETTINGS_TIMESTAMP_MASK) ==
+ OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
+ ? LEN_SAMPLECNT
+ : 0 + LEN_RAWDATA;
}
else if (OutputMode == OUTPUTMODE_XM)
{
@@ -2707,9 +2822,9 @@ namespace IMU::Xsens
// If not XbusMaster store also in BID_MT
if (bid == BID_MASTER && OutputMode != OUTPUTMODE_XM)
{
- m_storedOutputMode[BID_MT ] = m_storedOutputMode[0];
- m_storedOutputSettings[BID_MT ] = m_storedOutputSettings[0];
- m_storedDataLength[BID_MT ] = m_storedDataLength[0];
+ m_storedOutputMode[BID_MT] = m_storedOutputMode[0];
+ m_storedOutputSettings[BID_MT] = m_storedOutputSettings[0];
+ m_storedDataLength[BID_MT] = m_storedDataLength[0];
}
return (m_retVal = MTRV_OK);
@@ -2740,7 +2855,11 @@ namespace IMU::Xsens
// MTRV_OK : value is successfully retrieved
// != MTRV_OK : not successful
//
- short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short& value, const unsigned char data[], const unsigned char bid)
+ short
+ CXsensMTiModule::getValue(const unsigned long valueSpec,
+ unsigned short& value,
+ const unsigned char data[],
+ const unsigned char bid)
{
short offset = 0;
unsigned char nbid = bid;
@@ -2751,7 +2870,8 @@ namespace IMU::Xsens
}
// Check for invalid mode/settings
- if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE ||
+ m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
{
return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
}
@@ -2763,7 +2883,7 @@ namespace IMU::Xsens
while (i < nbid)
{
- offset += (short) m_storedDataLength[i++];
+ offset += (short)m_storedDataLength[i++];
}
}
@@ -2774,21 +2894,27 @@ namespace IMU::Xsens
{
if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
{
- offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
- swapEndian(data + offset + valueSpec * LEN_UNSIGSHORT * 3, (unsigned char*) &value, LEN_RAW_TEMP);
+ offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 &&
+ m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
+ ? LEN_SAMPLECNT
+ : 0;
+ swapEndian(data + offset + valueSpec * LEN_UNSIGSHORT * 3,
+ (unsigned char*)&value,
+ LEN_RAW_TEMP);
m_retVal = MTRV_OK;
}
}
else if (valueSpec == VALUE_SAMPLECNT)
{
- if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_TIMESTAMP_MASK) == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
+ if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_TIMESTAMP_MASK) ==
+ OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
{
if (!(m_storedOutputMode[nbid] == OUTPUTMODE_MT9))
{
- offset += (short) m_storedDataLength[nbid] - LEN_SAMPLECNT;
+ offset += (short)m_storedDataLength[nbid] - LEN_SAMPLECNT;
}
- swapEndian(data + offset, (unsigned char*) &value, LEN_SAMPLECNT);
+ swapEndian(data + offset, (unsigned char*)&value, LEN_SAMPLECNT);
m_retVal = MTRV_OK;
}
}
@@ -2822,7 +2948,11 @@ namespace IMU::Xsens
// MTRV_OK : value is successfully retrieved
// != MTRV_OK : not successful
//
- short CXsensMTiModule::getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid)
+ short
+ CXsensMTiModule::getValue(const unsigned long valueSpec,
+ unsigned short value[],
+ const unsigned char data[],
+ const unsigned char bid)
{
short offset = 0;
unsigned char nbid = bid;
@@ -2833,7 +2963,8 @@ namespace IMU::Xsens
}
// Check for invalid mode/settings
- if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE ||
+ m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
{
return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
}
@@ -2845,7 +2976,7 @@ namespace IMU::Xsens
while (i < nbid)
{
- offset += (short) m_storedDataLength[i++];
+ offset += (short)m_storedDataLength[i++];
}
}
@@ -2858,11 +2989,16 @@ namespace IMU::Xsens
if (m_storedOutputMode[nbid] & (OUTPUTMODE_RAW | OUTPUTMODE_MT9))
{
offset += (short)(valueSpec * LEN_UNSIGSHORT * 3);
- offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 && m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) ? LEN_SAMPLECNT : 0;
+ offset += (m_storedOutputMode[nbid] == OUTPUTMODE_MT9 &&
+ m_storedOutputSettings[nbid] == OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT)
+ ? LEN_SAMPLECNT
+ : 0;
- for (int i = 0 ; i < 3 ; i++)
+ for (int i = 0; i < 3; i++)
{
- swapEndian(data + offset + i * LEN_UNSIGSHORT, (unsigned char*) value + i * LEN_UNSIGSHORT, LEN_UNSIGSHORT);
+ swapEndian(data + offset + i * LEN_UNSIGSHORT,
+ (unsigned char*)value + i * LEN_UNSIGSHORT,
+ LEN_UNSIGSHORT);
}
m_retVal = MTRV_OK;
@@ -2902,7 +3038,11 @@ namespace IMU::Xsens
// MTRV_OK : value is successfully retrieved
// != MTRV_OK : not successful
//
- short CXsensMTiModule::getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid)
+ short
+ CXsensMTiModule::getValue(const unsigned long valueSpec,
+ float value[],
+ const unsigned char data[],
+ const unsigned char bid)
{
short offset = 0;
int nElements = 0;
@@ -2914,7 +3054,8 @@ namespace IMU::Xsens
}
// Check for invalid mode/settings
- if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE || m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
+ if (m_storedOutputMode[nbid] == INVALIDSETTINGVALUE ||
+ m_storedOutputSettings[nbid] == INVALIDSETTINGVALUE)
{
return (m_retVal = MTRV_NOVALIDMODESPECIFIED);
}
@@ -2926,7 +3067,7 @@ namespace IMU::Xsens
while (i < nbid)
{
- offset += (short) m_storedDataLength[i++];
+ offset += (short)m_storedDataLength[i++];
}
}
@@ -2945,7 +3086,8 @@ namespace IMU::Xsens
{
offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) &&
+ (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
{
nElements = LEN_CALIB_ACCDATA / LEN_FLOAT;
m_retVal = MTRV_OK;
@@ -2955,9 +3097,12 @@ namespace IMU::Xsens
{
offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) &&
+ (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
{
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
+ ? LEN_CALIB_ACCX * 3
+ : 0;
nElements = LEN_CALIB_GYRDATA / LEN_FLOAT;
m_retVal = MTRV_OK;
}
@@ -2966,10 +3111,15 @@ namespace IMU::Xsens
{
offset += ((m_storedOutputMode[nbid] & OUTPUTMODE_TEMP) != 0) ? LEN_TEMPDATA : 0;
- if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) && (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
+ if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB) &&
+ (m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
{
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
+ ? LEN_CALIB_ACCX * 3
+ : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
+ ? LEN_CALIB_GYRX * 3
+ : 0;
nElements = LEN_CALIB_MAGDATA / LEN_FLOAT;
m_retVal = MTRV_OK;
}
@@ -2980,14 +3130,21 @@ namespace IMU::Xsens
if ((m_storedOutputMode[nbid] & OUTPUTMODE_CALIB))
{
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0) ? LEN_CALIB_ACCX * 3 : 0;
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0) ? LEN_CALIB_GYRX * 3 : 0;
- offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0) ? LEN_CALIB_MAGX * 3 : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_ACC_MASK) == 0)
+ ? LEN_CALIB_ACCX * 3
+ : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_GYR_MASK) == 0)
+ ? LEN_CALIB_GYRX * 3
+ : 0;
+ offset += ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_CALIBMODE_MAG_MASK) == 0)
+ ? LEN_CALIB_MAGX * 3
+ : 0;
}
if (m_storedOutputMode[nbid] & OUTPUTMODE_ORIENT)
{
- unsigned long orientmode = m_storedOutputSettings[nbid] & OUTPUTSETTINGS_ORIENTMODE_MASK;
+ unsigned long orientmode =
+ m_storedOutputSettings[nbid] & OUTPUTSETTINGS_ORIENTMODE_MASK;
switch (valueSpec)
{
@@ -3028,19 +3185,21 @@ namespace IMU::Xsens
{
if ((m_storedOutputSettings[nbid] & OUTPUTSETTINGS_DATAFORMAT_F1220) == 0)
{
- for (int i = 0 ; i < nElements ; i++)
+ for (int i = 0; i < nElements; i++)
{
- swapEndian(data + offset + i * LEN_FLOAT, (unsigned char*) value + i * LEN_FLOAT, LEN_FLOAT);
+ swapEndian(data + offset + i * LEN_FLOAT,
+ (unsigned char*)value + i * LEN_FLOAT,
+ LEN_FLOAT);
}
}
else
{
int temp;
- for (int i = 0 ; i < nElements ; i++)
+ for (int i = 0; i < nElements; i++)
{
- swapEndian(data + offset + i * LEN_FLOAT, (unsigned char*) &temp, 4);
- value[i] = (float) temp / 1048576;
+ swapEndian(data + offset + i * LEN_FLOAT, (unsigned char*)&temp, 4);
+ value[i] = (float)temp / 1048576;
}
}
}
@@ -3056,7 +3215,8 @@ namespace IMU::Xsens
//
// Output
// Error code
- short CXsensMTiModule::getLastDeviceError()
+ short
+ CXsensMTiModule::getLastDeviceError()
{
return m_deviceError;
}
@@ -3068,7 +3228,8 @@ namespace IMU::Xsens
//
// Output
// Return value
- short CXsensMTiModule::getLastRetVal()
+ short
+ CXsensMTiModule::getLastRetVal()
{
return m_retVal;
}
@@ -3081,7 +3242,8 @@ namespace IMU::Xsens
//
// Output
// MTRV_OK is set, MTRV_INVALIDTIMEOUT if time value < 0
- short CXsensMTiModule::setTimeOut(short timeOutMs)
+ short
+ CXsensMTiModule::setTimeOut(short timeOutMs)
{
if (timeOutMs >= 0)
{
@@ -3107,7 +3269,10 @@ namespace IMU::Xsens
// Remarks:
// Allocate enough bytes for output buffer
- void CXsensMTiModule::swapEndian(const unsigned char input[], unsigned char output[], const int length)
+ void
+ CXsensMTiModule::swapEndian(const unsigned char input[],
+ unsigned char output[],
+ const int length)
{
switch (length)
{
@@ -3128,7 +3293,7 @@ namespace IMU::Xsens
break;
default:
- for (int i = 0, j = length - 1 ; i < length ; i++, j--)
+ for (int i = 0, j = length - 1; i < length; i++, j--)
{
output[j] = input[i];
}
@@ -3142,17 +3307,18 @@ namespace IMU::Xsens
//
// Calculate and append checksum to msgBuffer
//
- void CXsensMTiModule::calcChecksum(unsigned char* msgBuffer, const int msgBufferLength)
+ void
+ CXsensMTiModule::calcChecksum(unsigned char* msgBuffer, const int msgBufferLength)
{
unsigned char checkSum = 0;
int i;
- for (i = 1; i < msgBufferLength ; i++)
+ for (i = 1; i < msgBufferLength; i++)
{
checkSum += msgBuffer[i];
}
- msgBuffer[msgBufferLength] = -checkSum; // Store chksum
+ msgBuffer[msgBufferLength] = -checkSum; // Store chksum
}
//////////////////////////////////////////////////////////////////////
@@ -3162,12 +3328,13 @@ namespace IMU::Xsens
//
// Output
// returns true checksum is OK
- bool CXsensMTiModule::checkChecksum(const unsigned char* msgBuffer, const int msgBufferLength)
+ bool
+ CXsensMTiModule::checkChecksum(const unsigned char* msgBuffer, const int msgBufferLength)
{
unsigned char checkSum = 0;
int i;
- for (i = 1; i < msgBufferLength ; i++)
+ for (i = 1; i < msgBufferLength; i++)
{
checkSum += msgBuffer[i];
}
@@ -3181,6 +3348,6 @@ namespace IMU::Xsens
return false;
}
}
-}
+} // namespace IMU::Xsens
#endif
diff --git a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
index 99fac5a3db993ed70d224e91c13681c939d579c5..c67b9b7718b4b0574403c9452f86b76363ae7703 100644
--- a/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
+++ b/source/RobotAPI/drivers/XsensIMU/IMU/Xsens/XsensMTiModule.h
@@ -110,18 +110,19 @@
#pragma once
#endif // _MSC_VER > 1000
-#include <string.h>
#include <stdio.h>
+#include <string.h>
#ifdef WIN32
-#include <windows.h>
-#include <conio.h>
#include <time.h>
+
+#include <conio.h>
+#include <windows.h>
#else
-#include <fcntl.h> /* POSIX Standard: 6.5 File Control Operations */
-#include <termios.h> /* terminal i/o system, talks to /dev/tty* ports */
-#include <unistd.h> /* Read function */
-#include <sys/time.h> /* gettimeofday function */
-#include <sys/stat.h> /* stat calls and structures*/
+#include <fcntl.h> /* POSIX Standard: 6.5 File Control Operations */
+#include <sys/stat.h> /* stat calls and structures*/
+#include <sys/time.h> /* gettimeofday function */
+#include <termios.h> /* terminal i/o system, talks to /dev/tty* ports */
+#include <unistd.h> /* Read function */
#endif
#include "../Includes.h"
@@ -132,795 +133,796 @@ namespace IMU::Xsens
{
#ifndef INVALID_SET_FILE_POINTER
-#define INVALID_SET_FILE_POINTER ((DWORD)(-1))
+#define INVALID_SET_FILE_POINTER ((DWORD)(-1))
#endif
// Field message indexes
-#define IND_PREAMBLE 0
-#define IND_BID 1
-#define IND_MID 2
-#define IND_LEN 3
-#define IND_DATA0 4
-#define IND_LENEXTH 4
-#define IND_LENEXTL 5
-#define IND_DATAEXT0 6
+#define IND_PREAMBLE 0
+#define IND_BID 1
+#define IND_MID 2
+#define IND_LEN 3
+#define IND_DATA0 4
+#define IND_LENEXTH 4
+#define IND_LENEXTL 5
+#define IND_DATAEXT0 6
// Maximum number of sensors supported
-#define MAXDEVICES 20
-
-#define PREAMBLE (unsigned char)0xFA
-#define BID_MASTER (unsigned char)0xFF
-#define BID_MT (unsigned char)0x01
-#define EXTLENCODE 0xFF
-
-#define LEN_MSGHEADER (unsigned short)4
-#define LEN_MSGEXTHEADER (unsigned short)6
-#define LEN_MSGHEADERCS (unsigned short)5
-#define LEN_MSGEXTHEADERCS (unsigned short)7
-#define LEN_CHECKSUM (unsigned short)1
-#define LEN_UNSIGSHORT (unsigned short)2
-#define LEN_UNSIGINT (unsigned short)4
-#define LEN_FLOAT (unsigned short)4
+#define MAXDEVICES 20
+
+#define PREAMBLE (unsigned char)0xFA
+#define BID_MASTER (unsigned char)0xFF
+#define BID_MT (unsigned char)0x01
+#define EXTLENCODE 0xFF
+
+#define LEN_MSGHEADER (unsigned short)4
+#define LEN_MSGEXTHEADER (unsigned short)6
+#define LEN_MSGHEADERCS (unsigned short)5
+#define LEN_MSGEXTHEADERCS (unsigned short)7
+#define LEN_CHECKSUM (unsigned short)1
+#define LEN_UNSIGSHORT (unsigned short)2
+#define LEN_UNSIGINT (unsigned short)4
+#define LEN_FLOAT (unsigned short)4
// Maximum message/data length
-#define MAXDATALEN (unsigned short)2048
-#define MAXSHORTDATALEN (unsigned short)254
-#define MAXMSGLEN (unsigned short)(MAXDATALEN+7)
-#define MAXSHORTMSGLEN (unsigned short)(MAXSHORTDATALEN+5)
+#define MAXDATALEN (unsigned short)2048
+#define MAXSHORTDATALEN (unsigned short)254
+#define MAXMSGLEN (unsigned short)(MAXDATALEN + 7)
+#define MAXSHORTMSGLEN (unsigned short)(MAXSHORTDATALEN + 5)
// DID Type (high nibble)
-#define DID_TYPEH_MASK (unsigned long)0x00F00000
-#define DID_TYPEH_MT (unsigned long)0x00000000
-#define DID_TYPEH_XM (unsigned long)0x00100000
-#define DID_TYPEH_MTI_MTX (unsigned long)0x00300000
+#define DID_TYPEH_MASK (unsigned long)0x00F00000
+#define DID_TYPEH_MT (unsigned long)0x00000000
+#define DID_TYPEH_XM (unsigned long)0x00100000
+#define DID_TYPEH_MTI_MTX (unsigned long)0x00300000
// All Message identifiers
// WakeUp state messages
-#define MID_WAKEUP (unsigned char)0x3E
-#define MID_WAKEUPACK (unsigned char)0x3F
+#define MID_WAKEUP (unsigned char)0x3E
+#define MID_WAKEUPACK (unsigned char)0x3F
// Config state messages
-#define MID_REQDID (unsigned char)0x00
-#define MID_DEVICEID (unsigned char)0x01
-#define LEN_DEVICEID (unsigned short)4
-#define MID_INITBUS (unsigned char)0x02
-#define MID_INITBUSRESULTS (unsigned char)0x03
-#define LEN_INITBUSRESULTS (unsigned short)4
-#define MID_REQPERIOD (unsigned char)0x04
-#define MID_REQPERIODACK (unsigned char)0x05
-#define LEN_PERIOD (unsigned short)2
-#define MID_SETPERIOD (unsigned char)0x04
-#define MID_SETPERIODACK (unsigned char)0x05
+#define MID_REQDID (unsigned char)0x00
+#define MID_DEVICEID (unsigned char)0x01
+#define LEN_DEVICEID (unsigned short)4
+#define MID_INITBUS (unsigned char)0x02
+#define MID_INITBUSRESULTS (unsigned char)0x03
+#define LEN_INITBUSRESULTS (unsigned short)4
+#define MID_REQPERIOD (unsigned char)0x04
+#define MID_REQPERIODACK (unsigned char)0x05
+#define LEN_PERIOD (unsigned short)2
+#define MID_SETPERIOD (unsigned char)0x04
+#define MID_SETPERIODACK (unsigned char)0x05
// XbusMaster
-#define MID_SETBID (unsigned char)0x06
-#define MID_SETBIDACK (unsigned char)0x07
-#define MID_AUTOSTART (unsigned char)0x06
-#define MID_AUTOSTARTACK (unsigned char)0x07
-#define MID_BUSPWROFF (unsigned char)0x08
-#define MID_BUSPWROFFACK (unsigned char)0x09
+#define MID_SETBID (unsigned char)0x06
+#define MID_SETBIDACK (unsigned char)0x07
+#define MID_AUTOSTART (unsigned char)0x06
+#define MID_AUTOSTARTACK (unsigned char)0x07
+#define MID_BUSPWROFF (unsigned char)0x08
+#define MID_BUSPWROFFACK (unsigned char)0x09
// End XbusMaster
-#define MID_REQDATALENGTH (unsigned char)0x0A
-#define MID_DATALENGTH (unsigned char)0x0B
-#define LEN_DATALENGTH (unsigned short)2
-#define MID_REQCONFIGURATION (unsigned char)0x0C
-#define MID_CONFIGURATION (unsigned char)0x0D
-#define LEN_CONFIGURATION (unsigned short)118
-#define MID_RESTOREFACTORYDEF (unsigned char)0x0E
-#define MID_RESTOREFACTORYDEFACK (unsigned char)0x0F
-
-#define MID_GOTOMEASUREMENT (unsigned char)0x10
-#define MID_GOTOMEASUREMENTACK (unsigned char)0x11
-#define MID_REQFWREV (unsigned char)0x12
-#define MID_FIRMWAREREV (unsigned char)0x13
-#define LEN_FIRMWAREREV (unsigned short)3
+#define MID_REQDATALENGTH (unsigned char)0x0A
+#define MID_DATALENGTH (unsigned char)0x0B
+#define LEN_DATALENGTH (unsigned short)2
+#define MID_REQCONFIGURATION (unsigned char)0x0C
+#define MID_CONFIGURATION (unsigned char)0x0D
+#define LEN_CONFIGURATION (unsigned short)118
+#define MID_RESTOREFACTORYDEF (unsigned char)0x0E
+#define MID_RESTOREFACTORYDEFACK (unsigned char)0x0F
+
+#define MID_GOTOMEASUREMENT (unsigned char)0x10
+#define MID_GOTOMEASUREMENTACK (unsigned char)0x11
+#define MID_REQFWREV (unsigned char)0x12
+#define MID_FIRMWAREREV (unsigned char)0x13
+#define LEN_FIRMWAREREV (unsigned short)3
// XbusMaster
-#define MID_REQBTDISABLE (unsigned char)0x14
-#define MID_REQBTDISABLEACK (unsigned char)0x15
-#define MID_DISABLEBT (unsigned char)0x14
-#define MID_DISABLEBTACK (unsigned char)0x15
-#define MID_REQOPMODE (unsigned char)0x16
-#define MID_REQOPMODEACK (unsigned char)0x17
-#define MID_SETOPMODE (unsigned char)0x16
-#define MID_SETOPMODEACK (unsigned char)0x17
+#define MID_REQBTDISABLE (unsigned char)0x14
+#define MID_REQBTDISABLEACK (unsigned char)0x15
+#define MID_DISABLEBT (unsigned char)0x14
+#define MID_DISABLEBTACK (unsigned char)0x15
+#define MID_REQOPMODE (unsigned char)0x16
+#define MID_REQOPMODEACK (unsigned char)0x17
+#define MID_SETOPMODE (unsigned char)0x16
+#define MID_SETOPMODEACK (unsigned char)0x17
// End XbusMaster
-#define MID_REQBAUDRATE (unsigned char)0x18
-#define MID_REQBAUDRATEACK (unsigned char)0x19
-#define LEN_BAUDRATE (unsigned short)1
-#define MID_SETBAUDRATE (unsigned char)0x18
-#define MID_SETBAUDRATEACK (unsigned char)0x19
+#define MID_REQBAUDRATE (unsigned char)0x18
+#define MID_REQBAUDRATEACK (unsigned char)0x19
+#define LEN_BAUDRATE (unsigned short)1
+#define MID_SETBAUDRATE (unsigned char)0x18
+#define MID_SETBAUDRATEACK (unsigned char)0x19
// XbusMaster
-#define MID_REQSYNCMODE (unsigned char)0x1A
-#define MID_REQSYNCMODEACK (unsigned char)0x1B
-#define MID_SETSYNCMODE (unsigned char)0x1A
-#define MID_SETSYNCMODEACK (unsigned char)0x1B
+#define MID_REQSYNCMODE (unsigned char)0x1A
+#define MID_REQSYNCMODEACK (unsigned char)0x1B
+#define MID_SETSYNCMODE (unsigned char)0x1A
+#define MID_SETSYNCMODEACK (unsigned char)0x1B
// End XbusMaster
-#define MID_REQPRODUCTCODE (unsigned char)0x1C
-#define MID_PRODUCTCODE (unsigned char)0x1D
-
-#define MID_REQOUTPUTMODE (unsigned char)0xD0
-#define MID_REQOUTPUTMODEACK (unsigned char)0xD1
-#define LEN_OUTPUTMODE (unsigned short)2
-#define MID_SETOUTPUTMODE (unsigned char)0xD0
-#define MID_SETOUTPUTMODEACK (unsigned char)0xD1
-
-#define MID_REQOUTPUTSETTINGS (unsigned char)0xD2
-#define MID_REQOUTPUTSETTINGSACK (unsigned char)0xD3
-#define LEN_OUTPUTSETTINGS (unsigned short)4
-#define MID_SETOUTPUTSETTINGS (unsigned char)0xD2
-#define MID_SETOUTPUTSETTINGSACK (unsigned char)0xD3
-
-#define MID_REQOUTPUTSKIPFACTOR (unsigned char)0xD4
-#define MID_REQOUTPUTSKIPFACTORACK (unsigned char)0xD5
-#define LEN_OUTPUTSKIPFACTOR (unsigned short)2
-#define MID_SETOUTPUTSKIPFACTOR (unsigned char)0xD4
-#define MID_SETOUTPUTSKIPFACTORACK (unsigned char)0xD5
-
-#define MID_REQSYNCINSETTINGS (unsigned char)0xD6
-#define MID_REQSYNCINSETTINGSACK (unsigned char)0xD7
-#define LEN_SYNCINMODE (unsigned short)2
-#define LEN_SYNCINSKIPFACTOR (unsigned short)2
-#define LEN_SYNCINOFFSET (unsigned short)4
-#define MID_SETSYNCINSETTINGS (unsigned char)0xD6
-#define MID_SETSYNCINSETTINGSACK (unsigned char)0xD7
-
-#define MID_REQSYNCOUTSETTINGS (unsigned char)0xD8
-#define MID_REQSYNCOUTSETTINGSACK (unsigned char)0xD9
-#define LEN_SYNCOUTMODE (unsigned short)2
-#define LEN_SYNCOUTSKIPFACTOR (unsigned short)2
-#define LEN_SYNCOUTOFFSET (unsigned short)4
-#define LEN_SYNCOUTPULSEWIDTH (unsigned short)4
-#define MID_SETSYNCOUTSETTINGS (unsigned char)0xD8
-#define MID_SETSYNCOUTSETTINGSACK (unsigned char)0xD9
-
-#define MID_REQERRORMODE (unsigned char)0xDA
-#define MID_REQERRORMODEACK (unsigned char)0xDB
-#define LEN_ERRORMODE (unsigned short)2
-#define MID_SETERRORMODE (unsigned char)0xDA
-#define MID_SETERRORMODEACK (unsigned char)0xDB
-
-#define MID_REQTRANSMITDELAY (unsigned char)0xDC
-#define MID_REQTRANSMITDELAYACK (unsigned char)0xDD
-#define LEN_TRANSMITDELAY (unsigned short)2
-#define MID_SETTRANSMITDELAY (unsigned char)0xDC
-#define MID_SETTRANSMITDELAYACK (unsigned char)0xDD
+#define MID_REQPRODUCTCODE (unsigned char)0x1C
+#define MID_PRODUCTCODE (unsigned char)0x1D
+
+#define MID_REQOUTPUTMODE (unsigned char)0xD0
+#define MID_REQOUTPUTMODEACK (unsigned char)0xD1
+#define LEN_OUTPUTMODE (unsigned short)2
+#define MID_SETOUTPUTMODE (unsigned char)0xD0
+#define MID_SETOUTPUTMODEACK (unsigned char)0xD1
+
+#define MID_REQOUTPUTSETTINGS (unsigned char)0xD2
+#define MID_REQOUTPUTSETTINGSACK (unsigned char)0xD3
+#define LEN_OUTPUTSETTINGS (unsigned short)4
+#define MID_SETOUTPUTSETTINGS (unsigned char)0xD2
+#define MID_SETOUTPUTSETTINGSACK (unsigned char)0xD3
+
+#define MID_REQOUTPUTSKIPFACTOR (unsigned char)0xD4
+#define MID_REQOUTPUTSKIPFACTORACK (unsigned char)0xD5
+#define LEN_OUTPUTSKIPFACTOR (unsigned short)2
+#define MID_SETOUTPUTSKIPFACTOR (unsigned char)0xD4
+#define MID_SETOUTPUTSKIPFACTORACK (unsigned char)0xD5
+
+#define MID_REQSYNCINSETTINGS (unsigned char)0xD6
+#define MID_REQSYNCINSETTINGSACK (unsigned char)0xD7
+#define LEN_SYNCINMODE (unsigned short)2
+#define LEN_SYNCINSKIPFACTOR (unsigned short)2
+#define LEN_SYNCINOFFSET (unsigned short)4
+#define MID_SETSYNCINSETTINGS (unsigned char)0xD6
+#define MID_SETSYNCINSETTINGSACK (unsigned char)0xD7
+
+#define MID_REQSYNCOUTSETTINGS (unsigned char)0xD8
+#define MID_REQSYNCOUTSETTINGSACK (unsigned char)0xD9
+#define LEN_SYNCOUTMODE (unsigned short)2
+#define LEN_SYNCOUTSKIPFACTOR (unsigned short)2
+#define LEN_SYNCOUTOFFSET (unsigned short)4
+#define LEN_SYNCOUTPULSEWIDTH (unsigned short)4
+#define MID_SETSYNCOUTSETTINGS (unsigned char)0xD8
+#define MID_SETSYNCOUTSETTINGSACK (unsigned char)0xD9
+
+#define MID_REQERRORMODE (unsigned char)0xDA
+#define MID_REQERRORMODEACK (unsigned char)0xDB
+#define LEN_ERRORMODE (unsigned short)2
+#define MID_SETERRORMODE (unsigned char)0xDA
+#define MID_SETERRORMODEACK (unsigned char)0xDB
+
+#define MID_REQTRANSMITDELAY (unsigned char)0xDC
+#define MID_REQTRANSMITDELAYACK (unsigned char)0xDD
+#define LEN_TRANSMITDELAY (unsigned short)2
+#define MID_SETTRANSMITDELAY (unsigned char)0xDC
+#define MID_SETTRANSMITDELAYACK (unsigned char)0xDD
// Xbus Master
-#define MID_REQXMERRORMODE (unsigned char)0x82
-#define MID_REQXMERRORMODEACK (unsigned char)0x83
-#define LEN_XMERRORMODE (unsigned short)2
-#define MID_SETXMERRORMODE (unsigned char)0x82
-#define MID_SETXMERRORMODEACK (unsigned char)0x83
-
-#define MID_REQBUFFERSIZE (unsigned char)0x84
-#define MID_REQBUFFERSIZEACK (unsigned char)0x85
-#define LEN_BUFFERSIZE (unsigned short)2
-#define MID_SETBUFFERSIZE (unsigned char)0x84
-#define MID_SETBUFFERSIZEACK (unsigned char)0x85
+#define MID_REQXMERRORMODE (unsigned char)0x82
+#define MID_REQXMERRORMODEACK (unsigned char)0x83
+#define LEN_XMERRORMODE (unsigned short)2
+#define MID_SETXMERRORMODE (unsigned char)0x82
+#define MID_SETXMERRORMODEACK (unsigned char)0x83
+
+#define MID_REQBUFFERSIZE (unsigned char)0x84
+#define MID_REQBUFFERSIZEACK (unsigned char)0x85
+#define LEN_BUFFERSIZE (unsigned short)2
+#define MID_SETBUFFERSIZE (unsigned char)0x84
+#define MID_SETBUFFERSIZEACK (unsigned char)0x85
// End Xbus Master
-#define MID_REQHEADING (unsigned char)0x82
-#define MID_REQHEADINGACK (unsigned char)0x83
-#define LEN_HEADING (unsigned short)4
-#define MID_SETHEADING (unsigned char)0x82
-#define MID_SETHEADINGACK (unsigned char)0x83
+#define MID_REQHEADING (unsigned char)0x82
+#define MID_REQHEADINGACK (unsigned char)0x83
+#define LEN_HEADING (unsigned short)4
+#define MID_SETHEADING (unsigned char)0x82
+#define MID_SETHEADINGACK (unsigned char)0x83
-#define MID_REQLOCATIONID (unsigned char)0x84
-#define MID_REQLOCATIONIDACK (unsigned char)0x85
-#define LEN_LOCATIONID (unsigned short)2
-#define MID_SETLOCATIONID (unsigned char)0x84
-#define MID_SETLOCATIONIDACK (unsigned char)0x85
+#define MID_REQLOCATIONID (unsigned char)0x84
+#define MID_REQLOCATIONIDACK (unsigned char)0x85
+#define LEN_LOCATIONID (unsigned short)2
+#define MID_SETLOCATIONID (unsigned char)0x84
+#define MID_SETLOCATIONIDACK (unsigned char)0x85
-#define MID_REQEXTOUTPUTMODE (unsigned char)0x86
-#define MID_REQEXTOUTPUTMODEACK (unsigned char)0x87
-#define LEN_EXTOUTPUTMODE (unsigned short)2
-#define MID_SETEXTOUTPUTMODE (unsigned char)0x86
-#define MID_SETEXTOUTPUTMODEACK (unsigned char)0x87
+#define MID_REQEXTOUTPUTMODE (unsigned char)0x86
+#define MID_REQEXTOUTPUTMODEACK (unsigned char)0x87
+#define LEN_EXTOUTPUTMODE (unsigned short)2
+#define MID_SETEXTOUTPUTMODE (unsigned char)0x86
+#define MID_SETEXTOUTPUTMODEACK (unsigned char)0x87
// XbusMaster
-#define MID_REQBATLEVEL (unsigned char)0x88
-#define MID_BATLEVEL (unsigned char)0x89
+#define MID_REQBATLEVEL (unsigned char)0x88
+#define MID_BATLEVEL (unsigned char)0x89
// End XbusMaster
-#define MID_REQINITTRACKMODE (unsigned char)0x88
-#define MID_REQINITTRACKMODEACK (unsigned char)0x89
-#define LEN_INITTRACKMODE (unsigned short)2
-#define MID_SETINITTRACKMODE (unsigned char)0x88
-#define MID_SETINITTRACKMODEACK (unsigned char)0x89
+#define MID_REQINITTRACKMODE (unsigned char)0x88
+#define MID_REQINITTRACKMODEACK (unsigned char)0x89
+#define LEN_INITTRACKMODE (unsigned short)2
+#define MID_SETINITTRACKMODE (unsigned char)0x88
+#define MID_SETINITTRACKMODEACK (unsigned char)0x89
-#define MID_STOREFILTERSTATE (unsigned char)0x8A
-#define MID_STOREFILTERSTATEACK (unsigned char)0x8B
+#define MID_STOREFILTERSTATE (unsigned char)0x8A
+#define MID_STOREFILTERSTATEACK (unsigned char)0x8B
// Measurement state
-#define MID_GOTOCONFIG (unsigned char)0x30
-#define MID_GOTOCONFIGACK (unsigned char)0x31
-#define MID_BUSDATA (unsigned char)0x32
-#define MID_MTDATA (unsigned char)0x32
+#define MID_GOTOCONFIG (unsigned char)0x30
+#define MID_GOTOCONFIGACK (unsigned char)0x31
+#define MID_BUSDATA (unsigned char)0x32
+#define MID_MTDATA (unsigned char)0x32
// Manual
-#define MID_PREPAREDATA (unsigned char)0x32
-#define MID_REQDATA (unsigned char)0x34
-#define MID_REQDATAACK (unsigned char)0x35
+#define MID_PREPAREDATA (unsigned char)0x32
+#define MID_REQDATA (unsigned char)0x34
+#define MID_REQDATAACK (unsigned char)0x35
// MTData defines
// Length of data blocks in bytes
-#define LEN_RAWDATA (unsigned short)20
-#define LEN_CALIBDATA (unsigned short)36
-#define LEN_CALIB_ACCDATA (unsigned short)12
-#define LEN_CALIB_GYRDATA (unsigned short)12
-#define LEN_CALIB_MAGDATA (unsigned short)12
-#define LEN_ORIENT_QUATDATA (unsigned short)16
-#define LEN_ORIENT_EULERDATA (unsigned short)12
-#define LEN_ORIENT_MATRIXDATA (unsigned short)36
-#define LEN_SAMPLECNT (unsigned short)2
-#define LEN_TEMPDATA (unsigned short)4
+#define LEN_RAWDATA (unsigned short)20
+#define LEN_CALIBDATA (unsigned short)36
+#define LEN_CALIB_ACCDATA (unsigned short)12
+#define LEN_CALIB_GYRDATA (unsigned short)12
+#define LEN_CALIB_MAGDATA (unsigned short)12
+#define LEN_ORIENT_QUATDATA (unsigned short)16
+#define LEN_ORIENT_EULERDATA (unsigned short)12
+#define LEN_ORIENT_MATRIXDATA (unsigned short)36
+#define LEN_SAMPLECNT (unsigned short)2
+#define LEN_TEMPDATA (unsigned short)4
// Length of data blocks in floats
-#define LEN_CALIBDATA_FLT (unsigned short)9
-#define LEN_ORIENT_QUATDATA_FLT (unsigned short)4
-#define LEN_ORIENT_EULERDATA_FLT (unsigned short)3
-#define LEN_ORIENT_MATRIXDATA_FLT (unsigned short)9
+#define LEN_CALIBDATA_FLT (unsigned short)9
+#define LEN_ORIENT_QUATDATA_FLT (unsigned short)4
+#define LEN_ORIENT_EULERDATA_FLT (unsigned short)3
+#define LEN_ORIENT_MATRIXDATA_FLT (unsigned short)9
// Indices of fields in DATA field of MTData message in bytes
// use in combination with LEN_CALIB etc
// Un-calibrated raw data
-#define IND_RAW_ACCX 0
-#define IND_RAW_ACCY 2
-#define IND_RAW_ACCZ 4
-#define IND_RAW_GYRX 6
-#define IND_RAW_GYRY 8
-#define IND_RAW_GYRZ 10
-#define IND_RAW_MAGX 12
-#define IND_RAW_MAGY 14
-#define IND_RAW_MAGZ 16
-#define IND_RAW_TEMP 18
+#define IND_RAW_ACCX 0
+#define IND_RAW_ACCY 2
+#define IND_RAW_ACCZ 4
+#define IND_RAW_GYRX 6
+#define IND_RAW_GYRY 8
+#define IND_RAW_GYRZ 10
+#define IND_RAW_MAGX 12
+#define IND_RAW_MAGY 14
+#define IND_RAW_MAGZ 16
+#define IND_RAW_TEMP 18
// Calibrated data
-#define IND_CALIB_ACCX 0
-#define IND_CALIB_ACCY 4
-#define IND_CALIB_ACCZ 8
-#define IND_CALIB_GYRX 12
-#define IND_CALIB_GYRY 16
-#define IND_CALIB_GYRZ 20
-#define IND_CALIB_MAGX 24
-#define IND_CALIB_MAGY 28
-#define IND_CALIB_MAGZ 32
+#define IND_CALIB_ACCX 0
+#define IND_CALIB_ACCY 4
+#define IND_CALIB_ACCZ 8
+#define IND_CALIB_GYRX 12
+#define IND_CALIB_GYRY 16
+#define IND_CALIB_GYRZ 20
+#define IND_CALIB_MAGX 24
+#define IND_CALIB_MAGY 28
+#define IND_CALIB_MAGZ 32
// Orientation data - quat
-#define IND_ORIENT_Q0 0
-#define IND_ORIENT_Q1 4
-#define IND_ORIENT_Q2 8
-#define IND_ORIENT_Q3 12
+#define IND_ORIENT_Q0 0
+#define IND_ORIENT_Q1 4
+#define IND_ORIENT_Q2 8
+#define IND_ORIENT_Q3 12
// Orientation data - euler
-#define IND_ORIENT_ROLL 0
-#define IND_ORIENT_PITCH 4
-#define IND_ORIENT_YAW 8
+#define IND_ORIENT_ROLL 0
+#define IND_ORIENT_PITCH 4
+#define IND_ORIENT_YAW 8
// Orientation data - matrix
-#define IND_ORIENT_A 0
-#define IND_ORIENT_B 4
-#define IND_ORIENT_C 8
-#define IND_ORIENT_D 12
-#define IND_ORIENT_E 16
-#define IND_ORIENT_F 20
-#define IND_ORIENT_G 24
-#define IND_ORIENT_H 28
-#define IND_ORIENT_I 32
+#define IND_ORIENT_A 0
+#define IND_ORIENT_B 4
+#define IND_ORIENT_C 8
+#define IND_ORIENT_D 12
+#define IND_ORIENT_E 16
+#define IND_ORIENT_F 20
+#define IND_ORIENT_G 24
+#define IND_ORIENT_H 28
+#define IND_ORIENT_I 32
// Orientation data - euler
-#define IND_SAMPLECNTH 0
-#define IND_SAMPLECNTL 1
+#define IND_SAMPLECNTH 0
+#define IND_SAMPLECNTL 1
// Indices of fields in DATA field of MTData message
// Un-calibrated raw data
-#define FLDNUM_RAW_ACCX 0
-#define FLDNUM_RAW_ACCY 1
-#define FLDNUM_RAW_ACCZ 2
-#define FLDNUM_RAW_GYRX 3
-#define FLDNUM_RAW_GYRY 4
-#define FLDNUM_RAW_GYRZ 5
-#define FLDNUM_RAW_MAGX 6
-#define FLDNUM_RAW_MAGY 7
-#define FLDNUM_RAW_MAGZ 8
-#define FLDNUM_RAW_TEMP 9
+#define FLDNUM_RAW_ACCX 0
+#define FLDNUM_RAW_ACCY 1
+#define FLDNUM_RAW_ACCZ 2
+#define FLDNUM_RAW_GYRX 3
+#define FLDNUM_RAW_GYRY 4
+#define FLDNUM_RAW_GYRZ 5
+#define FLDNUM_RAW_MAGX 6
+#define FLDNUM_RAW_MAGY 7
+#define FLDNUM_RAW_MAGZ 8
+#define FLDNUM_RAW_TEMP 9
// Calibrated data
-#define FLDNUM_CALIB_ACCX 0
-#define FLDNUM_CALIB_ACCY 1
-#define FLDNUM_CALIB_ACCZ 2
-#define FLDNUM_CALIB_GYRX 3
-#define FLDNUM_CALIB_GYRY 4
-#define FLDNUM_CALIB_GYRZ 5
-#define FLDNUM_CALIB_MAGX 6
-#define FLDNUM_CALIB_MAGY 7
-#define FLDNUM_CALIB_MAGZ 8
+#define FLDNUM_CALIB_ACCX 0
+#define FLDNUM_CALIB_ACCY 1
+#define FLDNUM_CALIB_ACCZ 2
+#define FLDNUM_CALIB_GYRX 3
+#define FLDNUM_CALIB_GYRY 4
+#define FLDNUM_CALIB_GYRZ 5
+#define FLDNUM_CALIB_MAGX 6
+#define FLDNUM_CALIB_MAGY 7
+#define FLDNUM_CALIB_MAGZ 8
// Orientation data - quat
-#define FLDNUM_ORIENT_Q0 0
-#define FLDNUM_ORIENT_Q1 1
-#define FLDNUM_ORIENT_Q2 2
-#define FLDNUM_ORIENT_Q3 3
+#define FLDNUM_ORIENT_Q0 0
+#define FLDNUM_ORIENT_Q1 1
+#define FLDNUM_ORIENT_Q2 2
+#define FLDNUM_ORIENT_Q3 3
// Orientation data - euler
-#define FLDNUM_ORIENT_ROLL 0
-#define FLDNUM_ORIENT_PITCH 1
-#define FLDNUM_ORIENT_YAW 2
+#define FLDNUM_ORIENT_ROLL 0
+#define FLDNUM_ORIENT_PITCH 1
+#define FLDNUM_ORIENT_YAW 2
// Orientation data - matrix
-#define FLDNUM_ORIENT_A 0
-#define FLDNUM_ORIENT_B 1
-#define FLDNUM_ORIENT_C 2
-#define FLDNUM_ORIENT_D 3
-#define FLDNUM_ORIENT_E 4
-#define FLDNUM_ORIENT_F 5
-#define FLDNUM_ORIENT_G 6
-#define FLDNUM_ORIENT_H 7
-#define FLDNUM_ORIENT_I 8
+#define FLDNUM_ORIENT_A 0
+#define FLDNUM_ORIENT_B 1
+#define FLDNUM_ORIENT_C 2
+#define FLDNUM_ORIENT_D 3
+#define FLDNUM_ORIENT_E 4
+#define FLDNUM_ORIENT_F 5
+#define FLDNUM_ORIENT_G 6
+#define FLDNUM_ORIENT_H 7
+#define FLDNUM_ORIENT_I 8
// Length
// Uncalibrated raw data
-#define LEN_RAW_ACCX 2
-#define LEN_RAW_ACCY 2
-#define LEN_RAW_ACCZ 2
-#define LEN_RAW_GYRX 2
-#define LEN_RAW_GYRY 2
-#define LEN_RAW_GYRZ 2
-#define LEN_RAW_MAGX 2
-#define LEN_RAW_MAGY 2
-#define LEN_RAW_MAGZ 2
-#define LEN_RAW_TEMP 2
+#define LEN_RAW_ACCX 2
+#define LEN_RAW_ACCY 2
+#define LEN_RAW_ACCZ 2
+#define LEN_RAW_GYRX 2
+#define LEN_RAW_GYRY 2
+#define LEN_RAW_GYRZ 2
+#define LEN_RAW_MAGX 2
+#define LEN_RAW_MAGY 2
+#define LEN_RAW_MAGZ 2
+#define LEN_RAW_TEMP 2
// Calibrated data
-#define LEN_CALIB_ACCX 4
-#define LEN_CALIB_ACCY 4
-#define LEN_CALIB_ACCZ 4
-#define LEN_CALIB_GYRX 4
-#define LEN_CALIB_GYRY 4
-#define LEN_CALIB_GYRZ 4
-#define LEN_CALIB_MAGX 4
-#define LEN_CALIB_MAGY 4
-#define LEN_CALIB_MAGZ 4
+#define LEN_CALIB_ACCX 4
+#define LEN_CALIB_ACCY 4
+#define LEN_CALIB_ACCZ 4
+#define LEN_CALIB_GYRX 4
+#define LEN_CALIB_GYRY 4
+#define LEN_CALIB_GYRZ 4
+#define LEN_CALIB_MAGX 4
+#define LEN_CALIB_MAGY 4
+#define LEN_CALIB_MAGZ 4
// Orientation data - quat
-#define LEN_ORIENT_Q0 4
-#define LEN_ORIENT_Q1 4
-#define LEN_ORIENT_Q2 4
-#define LEN_ORIENT_Q3 4
+#define LEN_ORIENT_Q0 4
+#define LEN_ORIENT_Q1 4
+#define LEN_ORIENT_Q2 4
+#define LEN_ORIENT_Q3 4
// Orientation data - euler
-#define LEN_ORIENT_ROLL 4
-#define LEN_ORIENT_PITCH 4
-#define LEN_ORIENT_YAW 4
+#define LEN_ORIENT_ROLL 4
+#define LEN_ORIENT_PITCH 4
+#define LEN_ORIENT_YAW 4
// Orientation data - matrix
-#define LEN_ORIENT_A 4
-#define LEN_ORIENT_B 4
-#define LEN_ORIENT_C 4
-#define LEN_ORIENT_D 4
-#define LEN_ORIENT_E 4
-#define LEN_ORIENT_F 4
-#define LEN_ORIENT_G 4
-#define LEN_ORIENT_H 4
-#define LEN_ORIENT_I 4
+#define LEN_ORIENT_A 4
+#define LEN_ORIENT_B 4
+#define LEN_ORIENT_C 4
+#define LEN_ORIENT_D 4
+#define LEN_ORIENT_E 4
+#define LEN_ORIENT_F 4
+#define LEN_ORIENT_G 4
+#define LEN_ORIENT_H 4
+#define LEN_ORIENT_I 4
// Defines for getDataValue
-#define VALUE_RAW_ACC 0
-#define VALUE_RAW_GYR 1
-#define VALUE_RAW_MAG 2
-#define VALUE_RAW_TEMP 3
-#define VALUE_CALIB_ACC 4
-#define VALUE_CALIB_GYR 5
-#define VALUE_CALIB_MAG 6
-#define VALUE_ORIENT_QUAT 7
-#define VALUE_ORIENT_EULER 8
-#define VALUE_ORIENT_MATRIX 9
-#define VALUE_SAMPLECNT 10
-#define VALUE_TEMP 11
-
-#define INVALIDSETTINGVALUE 0xFFFFFFFF
+#define VALUE_RAW_ACC 0
+#define VALUE_RAW_GYR 1
+#define VALUE_RAW_MAG 2
+#define VALUE_RAW_TEMP 3
+#define VALUE_CALIB_ACC 4
+#define VALUE_CALIB_GYR 5
+#define VALUE_CALIB_MAG 6
+#define VALUE_ORIENT_QUAT 7
+#define VALUE_ORIENT_EULER 8
+#define VALUE_ORIENT_MATRIX 9
+#define VALUE_SAMPLECNT 10
+#define VALUE_TEMP 11
+
+#define INVALIDSETTINGVALUE 0xFFFFFFFF
// Valid in all states
-#define MID_RESET (unsigned char)0x40
-#define MID_RESETACK (unsigned char)0x41
-#define MID_ERROR (unsigned char)0x42
-#define LEN_ERROR (unsigned short)1
+#define MID_RESET (unsigned char)0x40
+#define MID_RESETACK (unsigned char)0x41
+#define MID_ERROR (unsigned char)0x42
+#define LEN_ERROR (unsigned short)1
// XbusMaster
-#define MID_XMPWROFF (unsigned char)0x44
+#define MID_XMPWROFF (unsigned char)0x44
// End XbusMaster
-#define MID_REQFILTERSETTINGS (unsigned char)0xA0
-#define MID_REQFILTERSETTINGSACK (unsigned char)0xA1
-#define LEN_FILTERSETTINGS (unsigned short)4
-#define MID_SETFILTERSETTINGS (unsigned char)0xA0
-#define MID_SETFILTERSETTINGSACK (unsigned char)0xA1
-#define MID_REQAMD (unsigned char)0xA2
-#define MID_REQAMDACK (unsigned char)0xA3
-#define LEN_AMD (unsigned short)2
-#define MID_SETAMD (unsigned char)0xA2
-#define MID_SETAMDACK (unsigned char)0xA3
-#define MID_RESETORIENTATION (unsigned char)0xA4
-#define MID_RESETORIENTATIONACK (unsigned char)0xA5
-#define LEN_RESETORIENTATION (unsigned short)2
+#define MID_REQFILTERSETTINGS (unsigned char)0xA0
+#define MID_REQFILTERSETTINGSACK (unsigned char)0xA1
+#define LEN_FILTERSETTINGS (unsigned short)4
+#define MID_SETFILTERSETTINGS (unsigned char)0xA0
+#define MID_SETFILTERSETTINGSACK (unsigned char)0xA1
+#define MID_REQAMD (unsigned char)0xA2
+#define MID_REQAMDACK (unsigned char)0xA3
+#define LEN_AMD (unsigned short)2
+#define MID_SETAMD (unsigned char)0xA2
+#define MID_SETAMDACK (unsigned char)0xA3
+#define MID_RESETORIENTATION (unsigned char)0xA4
+#define MID_RESETORIENTATIONACK (unsigned char)0xA5
+#define LEN_RESETORIENTATION (unsigned short)2
// All Messages
// WakeUp state messages
-#define MSG_WAKEUPLEN 5
-#define MSG_WAKEUPACK (const unsigned char *)"\xFA\xFF\x3F\x00"
-#define MSG_WAKEUPACKLEN 4
+#define MSG_WAKEUPLEN 5
+#define MSG_WAKEUPACK (const unsigned char*)"\xFA\xFF\x3F\x00"
+#define MSG_WAKEUPACKLEN 4
// Config state messages
-#define MSG_REQDID (const unsigned char *)"\xFA\xFF\x00\x00"
-#define MSG_REQDIDLEN 4
-#define MSG_DEVICEIDLEN 9
-#define MSG_INITBUS (const unsigned char *)"\xFA\xFF\x02\x00"
-#define MSG_INITBUSLEN 4
-#define MSG_INITBUSRESMAXLEN (5 + 4 * MAXSENSORS)
-#define MSG_REQPERIOD (const unsigned char *)"\xFA\xFF\x04\x00"
-#define MSG_REQPERIODLEN 4
-#define MSG_REQPERIODACKLEN 7
-#define MSG_SETPERIOD (const unsigned char *)"\xFA\xFF\x04\x02"
-#define MSG_SETPERIODLEN 6
-#define MSG_SETPERIODACKLEN 5
-#define MSG_SETBID (const unsigned char *)"\xFA\xFF\x06\x05"
-#define MSG_SETBIDLEN 9
-#define MSG_SETBIDACKLEN 5
-#define MSG_AUTOSTART (const unsigned char *)"\xFA\xFF\x06\x00"
-#define MSG_AUTOSTARTLEN 4
-#define MSG_AUTOSTARTACKLEN 5
-#define MSG_BUSPWROFF (const unsigned char *)"\xFA\xFF\x08\x00"
-#define MSG_BUSPWROFFLEN 4
-#define MSG_BUSPWROFFACKLEN 5
-#define MSG_RESTOREFACTORYDEF (const unsigned char *)"\xFA\xFF\x0E\x00"
-#define MSG_RESTOREFACTORYDEFLEN 4
+#define MSG_REQDID (const unsigned char*)"\xFA\xFF\x00\x00"
+#define MSG_REQDIDLEN 4
+#define MSG_DEVICEIDLEN 9
+#define MSG_INITBUS (const unsigned char*)"\xFA\xFF\x02\x00"
+#define MSG_INITBUSLEN 4
+#define MSG_INITBUSRESMAXLEN (5 + 4 * MAXSENSORS)
+#define MSG_REQPERIOD (const unsigned char*)"\xFA\xFF\x04\x00"
+#define MSG_REQPERIODLEN 4
+#define MSG_REQPERIODACKLEN 7
+#define MSG_SETPERIOD (const unsigned char*)"\xFA\xFF\x04\x02"
+#define MSG_SETPERIODLEN 6
+#define MSG_SETPERIODACKLEN 5
+#define MSG_SETBID (const unsigned char*)"\xFA\xFF\x06\x05"
+#define MSG_SETBIDLEN 9
+#define MSG_SETBIDACKLEN 5
+#define MSG_AUTOSTART (const unsigned char*)"\xFA\xFF\x06\x00"
+#define MSG_AUTOSTARTLEN 4
+#define MSG_AUTOSTARTACKLEN 5
+#define MSG_BUSPWROFF (const unsigned char*)"\xFA\xFF\x08\x00"
+#define MSG_BUSPWROFFLEN 4
+#define MSG_BUSPWROFFACKLEN 5
+#define MSG_RESTOREFACTORYDEF (const unsigned char*)"\xFA\xFF\x0E\x00"
+#define MSG_RESTOREFACTORYDEFLEN 4
#define MSG_RESTOREFACTORYDEFACKLEN 5
-#define MSG_REQDATALENGTH (const unsigned char *)"\xFA\xFF\x0A\x00"
-#define MSG_REQDATALENGTHLEN 4
-#define MSG_DATALENGTHLEN 7
-#define MSG_REQCONFIGURATION (const unsigned char *)"\xFA\xFF\x0C\x00"
-#define MSG_REQCONFIGURATIONLEN 4
-#define MSG_GOTOMEASUREMENT (const unsigned char *)"\xFA\xFF\x10\x00"
-#define MSG_GOTOMEASUREMENTLEN 4
-#define MSG_GOTOMEASMAN (const unsigned char *)"\xFA\x01\x10\x00"
-#define MSG_GOTOMEASMANLEN 4
-#define MSG_GOTOMEASACKLEN 5
-#define MSG_REQFWREV (const unsigned char *)"\xFA\xFF\x12\x00"
-#define MSG_REQFWREVLEN 4
-#define MSG_FIRMWAREREVLEN 8
-#define MSG_REQBTDISABLED (const unsigned char *)"\xFA\xFF\x14\x00"
-#define MSG_REQBTDISABLEDLEN 4
-#define MSG_REQBTDISABLEDACKLEN 6
-#define MSG_DISABLEBT (const unsigned char *)"\xFA\xFF\x14\x01"
-#define MSG_DISABLEBTLEN 5
-#define MSG_DISABLEBTACKLEN 5
-#define MSG_REQOPMODE (const unsigned char *)"\xFA\xFF\x16\x00"
-#define MSG_REQOPMODELEN 4
-#define MSG_REQOPMODEACKLEN 6
-#define MSG_SETOPMODE (const unsigned char *)"\xFA\xFF\x16\x01"
-#define MSG_SETOPMODELEN 5
-#define MSG_SETOPMODEACKLEN 5
-#define MSG_REQBAUDRATE (const unsigned char *)"\xFA\xFF\x18\x00"
-#define MSG_REQBAUDRATELEN 4
-#define MSG_REQBAUDRATEACKLEN 6
-#define MSG_SETBAUDRATE (const unsigned char *)"\xFA\xFF\x18\x01"
-#define MSG_SETBAUDRATELEN 5
-#define MSG_SETBAUDRATEACKLEN 5
-#define MSG_REQSYNCMODE (const unsigned char *)"\xFA\xFF\x1A\x00"
-#define MSG_REQSYNCMODELEN 4
-#define MSG_REQSYNCMODEACKLEN 6
-#define MSG_SETSYNCMODE (const unsigned char *)"\xFA\xFF\x1A\x01"
-#define MSG_SETSYNCMODELEN 5
-#define MSG_SETSYNCMODEACKLEN 6
-#define MSG_REQMTS (const unsigned char *)"\xFA\xFF\x90\x01"
-#define MSG_REQMTSLEN 5
-#define MSG_MTSDATA 61
-#define MSG_STORECUSMTS (const unsigned char *)"\xFA\xFF\x92\x58"
-#define MSG_STORECUSMTSLEN 92
-#define MSG_STORECUSMTSACKLEN 5
-#define MSG_REVTOORGMTS (const unsigned char *)"\xFA\xFF\x94\x00"
-#define MSG_REVTOORGMTSLEN 4
-#define MSG_REVTOORGMTSACKLEN 5
-#define MSG_STOREMTS (const unsigned char *)"\xFA\xFF\x96\x41"
-#define MSG_STOREMTSLEN 69
-#define MSG_STOREMTSACKLEN 5
-#define MSG_REQSYNCOUTMODE (const unsigned char *)"\xFA\xFF\xD8\x01\x00"
-#define MSG_REQSYNCOUTMODELEN 5
-#define MSG_REQSYNCOUTSKIPFACTOR (const unsigned char *)"\xFA\xFF\xD8\x01\x01"
+#define MSG_REQDATALENGTH (const unsigned char*)"\xFA\xFF\x0A\x00"
+#define MSG_REQDATALENGTHLEN 4
+#define MSG_DATALENGTHLEN 7
+#define MSG_REQCONFIGURATION (const unsigned char*)"\xFA\xFF\x0C\x00"
+#define MSG_REQCONFIGURATIONLEN 4
+#define MSG_GOTOMEASUREMENT (const unsigned char*)"\xFA\xFF\x10\x00"
+#define MSG_GOTOMEASUREMENTLEN 4
+#define MSG_GOTOMEASMAN (const unsigned char*)"\xFA\x01\x10\x00"
+#define MSG_GOTOMEASMANLEN 4
+#define MSG_GOTOMEASACKLEN 5
+#define MSG_REQFWREV (const unsigned char*)"\xFA\xFF\x12\x00"
+#define MSG_REQFWREVLEN 4
+#define MSG_FIRMWAREREVLEN 8
+#define MSG_REQBTDISABLED (const unsigned char*)"\xFA\xFF\x14\x00"
+#define MSG_REQBTDISABLEDLEN 4
+#define MSG_REQBTDISABLEDACKLEN 6
+#define MSG_DISABLEBT (const unsigned char*)"\xFA\xFF\x14\x01"
+#define MSG_DISABLEBTLEN 5
+#define MSG_DISABLEBTACKLEN 5
+#define MSG_REQOPMODE (const unsigned char*)"\xFA\xFF\x16\x00"
+#define MSG_REQOPMODELEN 4
+#define MSG_REQOPMODEACKLEN 6
+#define MSG_SETOPMODE (const unsigned char*)"\xFA\xFF\x16\x01"
+#define MSG_SETOPMODELEN 5
+#define MSG_SETOPMODEACKLEN 5
+#define MSG_REQBAUDRATE (const unsigned char*)"\xFA\xFF\x18\x00"
+#define MSG_REQBAUDRATELEN 4
+#define MSG_REQBAUDRATEACKLEN 6
+#define MSG_SETBAUDRATE (const unsigned char*)"\xFA\xFF\x18\x01"
+#define MSG_SETBAUDRATELEN 5
+#define MSG_SETBAUDRATEACKLEN 5
+#define MSG_REQSYNCMODE (const unsigned char*)"\xFA\xFF\x1A\x00"
+#define MSG_REQSYNCMODELEN 4
+#define MSG_REQSYNCMODEACKLEN 6
+#define MSG_SETSYNCMODE (const unsigned char*)"\xFA\xFF\x1A\x01"
+#define MSG_SETSYNCMODELEN 5
+#define MSG_SETSYNCMODEACKLEN 6
+#define MSG_REQMTS (const unsigned char*)"\xFA\xFF\x90\x01"
+#define MSG_REQMTSLEN 5
+#define MSG_MTSDATA 61
+#define MSG_STORECUSMTS (const unsigned char*)"\xFA\xFF\x92\x58"
+#define MSG_STORECUSMTSLEN 92
+#define MSG_STORECUSMTSACKLEN 5
+#define MSG_REVTOORGMTS (const unsigned char*)"\xFA\xFF\x94\x00"
+#define MSG_REVTOORGMTSLEN 4
+#define MSG_REVTOORGMTSACKLEN 5
+#define MSG_STOREMTS (const unsigned char*)"\xFA\xFF\x96\x41"
+#define MSG_STOREMTSLEN 69
+#define MSG_STOREMTSACKLEN 5
+#define MSG_REQSYNCOUTMODE (const unsigned char*)"\xFA\xFF\xD8\x01\x00"
+#define MSG_REQSYNCOUTMODELEN 5
+#define MSG_REQSYNCOUTSKIPFACTOR (const unsigned char*)"\xFA\xFF\xD8\x01\x01"
#define MSG_REQSYNCOUTSKIPFACTORLEN 5
-#define MSG_REQSYNCOUTOFFSET (const unsigned char *)"\xFA\xFF\xD8\x01\x02"
-#define MSG_REQSYNCOUTOFFSETLEN 5
-#define MSG_REQSYNCOUTPULSEWIDTH (const unsigned char *)"\xFA\xFF\xD8\x01\x03"
+#define MSG_REQSYNCOUTOFFSET (const unsigned char*)"\xFA\xFF\xD8\x01\x02"
+#define MSG_REQSYNCOUTOFFSETLEN 5
+#define MSG_REQSYNCOUTPULSEWIDTH (const unsigned char*)"\xFA\xFF\xD8\x01\x03"
#define MSG_REQSYNCOUTPULSEWIDTHLEN 5
-#define MSG_REQERRORMODE (const unsigned char *)"\xFA\xFF\xDA\x00"
-#define MSG_REQERRORMODELEN 4
-#define MSG_REQERRORMODEACKLEN 7
+#define MSG_REQERRORMODE (const unsigned char*)"\xFA\xFF\xDA\x00"
+#define MSG_REQERRORMODELEN 4
+#define MSG_REQERRORMODEACKLEN 7
// Measurement state - auto messages
-#define MSG_GOTOCONFIG (const unsigned char *)"\xFA\xFF\x30\x00"
-#define MSG_GOTOCONFIGLEN 4
-#define MSG_GOTOCONFIGACKLEN 5
+#define MSG_GOTOCONFIG (const unsigned char*)"\xFA\xFF\x30\x00"
+#define MSG_GOTOCONFIGLEN 4
+#define MSG_GOTOCONFIGACKLEN 5
// Measurement state - manual messages (Use DID = 0x01)
-#define MSG_GOTOCONFIGM (const unsigned char *)"\xFA\x01\x30\x00"
-#define MSG_GOTOCONFIGMLEN 4
-#define MSG_GOTOCONFIGMACKLEN 5
-#define MSG_PREPAREDATA (const unsigned char *)"\xFA\x01\x32\x00"
-#define MSG_PREPAREDATALEN 4
-#define MSG_REQDATA (const unsigned char *)"\xFA\x01\x34\x00"
-#define MSG_REQDATALEN 4
+#define MSG_GOTOCONFIGM (const unsigned char*)"\xFA\x01\x30\x00"
+#define MSG_GOTOCONFIGMLEN 4
+#define MSG_GOTOCONFIGMACKLEN 5
+#define MSG_PREPAREDATA (const unsigned char*)"\xFA\x01\x32\x00"
+#define MSG_PREPAREDATALEN 4
+#define MSG_REQDATA (const unsigned char*)"\xFA\x01\x34\x00"
+#define MSG_REQDATALEN 4
// Valid in all states
-#define MSG_RESET (const unsigned char *)"\xFA\xFF\x40\x00"
-#define MSG_RESETLEN 4
-#define MSG_RESETACKLEN 5
-#define MSG_XMPWROFF (const unsigned char *)"\xFA\xFF\x44\x00"
-#define MSG_XMPWROFFLEN 4
-#define MSG_XMPWROFFACKLEN 5
+#define MSG_RESET (const unsigned char*)"\xFA\xFF\x40\x00"
+#define MSG_RESETLEN 4
+#define MSG_RESETACKLEN 5
+#define MSG_XMPWROFF (const unsigned char*)"\xFA\xFF\x44\x00"
+#define MSG_XMPWROFFLEN 4
+#define MSG_XMPWROFFACKLEN 5
// Baudrate defines for SetBaudrate message
-#define BAUDRATE_9K6 0x09
-#define BAUDRATE_14K4 0x08
-#define BAUDRATE_19K2 0x07
-#define BAUDRATE_28K8 0x06
-#define BAUDRATE_38K4 0x05
-#define BAUDRATE_57K6 0x04
-#define BAUDRATE_76K8 0x03
-#define BAUDRATE_115K2 0x02
-#define BAUDRATE_230K4 0x01
-#define BAUDRATE_460K8 0x00
-#define BAUDRATE_921K6 0x80
+#define BAUDRATE_9K6 0x09
+#define BAUDRATE_14K4 0x08
+#define BAUDRATE_19K2 0x07
+#define BAUDRATE_28K8 0x06
+#define BAUDRATE_38K4 0x05
+#define BAUDRATE_57K6 0x04
+#define BAUDRATE_76K8 0x03
+#define BAUDRATE_115K2 0x02
+#define BAUDRATE_230K4 0x01
+#define BAUDRATE_460K8 0x00
+#define BAUDRATE_921K6 0x80
// Xbus protocol error codes (Error)
-#define ERROR_NOBUSCOMM 0x01
-#define ERROR_BUSNOTREADY 0x02
-#define ERROR_PERIODINVALID 0x03
-#define ERROR_MESSAGEINVALID 0x04
-#define ERROR_INITOFBUSFAILED1 0x10
-#define ERROR_INITOFBUSFAILED2 0x11
-#define ERROR_INITOFBUSFAILED3 0x12
-#define ERROR_SETBIDPROCFAILED1 0x14
-#define ERROR_SETBIDPROCFAILED2 0x15
-#define ERROR_MEASUREMENTFAILED1 0x18
-#define ERROR_MEASUREMENTFAILED2 0x19
-#define ERROR_MEASUREMENTFAILED3 0x1A
-#define ERROR_MEASUREMENTFAILED4 0x1B
-#define ERROR_MEASUREMENTFAILED5 0x1C
-#define ERROR_MEASUREMENTFAILED6 0x1D
-#define ERROR_TIMEROVERFLOW 0x1E
-#define ERROR_BAUDRATEINVALID 0x20
-#define ERROR_PARAMETERINVALID 0x21
-#define ERROR_MEASUREMENTFAILED7 0x23
+#define ERROR_NOBUSCOMM 0x01
+#define ERROR_BUSNOTREADY 0x02
+#define ERROR_PERIODINVALID 0x03
+#define ERROR_MESSAGEINVALID 0x04
+#define ERROR_INITOFBUSFAILED1 0x10
+#define ERROR_INITOFBUSFAILED2 0x11
+#define ERROR_INITOFBUSFAILED3 0x12
+#define ERROR_SETBIDPROCFAILED1 0x14
+#define ERROR_SETBIDPROCFAILED2 0x15
+#define ERROR_MEASUREMENTFAILED1 0x18
+#define ERROR_MEASUREMENTFAILED2 0x19
+#define ERROR_MEASUREMENTFAILED3 0x1A
+#define ERROR_MEASUREMENTFAILED4 0x1B
+#define ERROR_MEASUREMENTFAILED5 0x1C
+#define ERROR_MEASUREMENTFAILED6 0x1D
+#define ERROR_TIMEROVERFLOW 0x1E
+#define ERROR_BAUDRATEINVALID 0x20
+#define ERROR_PARAMETERINVALID 0x21
+#define ERROR_MEASUREMENTFAILED7 0x23
// Error modes (SetErrorMode)
-#define ERRORMODE_IGNORE 0x0000
-#define ERRORMODE_INCSAMPLECNT 0x0001
-#define ERRORMODE_INCSAMPLECNT_SENDERROR 0x0002
-#define ERRORMODE_SENDERROR_GOTOCONFIG 0x0003
+#define ERRORMODE_IGNORE 0x0000
+#define ERRORMODE_INCSAMPLECNT 0x0001
+#define ERRORMODE_INCSAMPLECNT_SENDERROR 0x0002
+#define ERRORMODE_SENDERROR_GOTOCONFIG 0x0003
// Configuration message defines
-#define CONF_MASTERDID 0
-#define CONF_PERIOD 4
-#define CONF_OUTPUTSKIPFACTOR 6
-#define CONF_SYNCIN_MODE 8
-#define CONF_SYNCIN_SKIPFACTOR 10
-#define CONF_SYNCIN_OFFSET 12
-#define CONF_DATE 16
-#define CONF_TIME 24
-#define CONF_NUMDEVICES 96
+#define CONF_MASTERDID 0
+#define CONF_PERIOD 4
+#define CONF_OUTPUTSKIPFACTOR 6
+#define CONF_SYNCIN_MODE 8
+#define CONF_SYNCIN_SKIPFACTOR 10
+#define CONF_SYNCIN_OFFSET 12
+#define CONF_DATE 16
+#define CONF_TIME 24
+#define CONF_NUMDEVICES 96
// Configuration sensor block properties
-#define CONF_DID 98
-#define CONF_DATALENGTH 102
-#define CONF_OUTPUTMODE 104
-#define CONF_OUTPUTSETTINGS 106
-#define CONF_BLOCKLEN 20
+#define CONF_DID 98
+#define CONF_DATALENGTH 102
+#define CONF_OUTPUTMODE 104
+#define CONF_OUTPUTSETTINGS 106
+#define CONF_BLOCKLEN 20
// To calculate the offset in data field for output mode of sensor #2 use
// CONF_OUTPUTMODE + 1*CONF_BLOCKLEN
-#define CONF_MASTERDIDLEN 4
-#define CONF_PERIODLEN 2
-#define CONF_OUTPUTSKIPFACTORLEN 2
-#define CONF_SYNCIN_MODELEN 2
-#define CONF_SYNCIN_SKIPFACTORLEN 2
-#define CONF_SYNCIN_OFFSETLEN 4
-#define CONF_DATELEN 8
-#define CONF_TIMELEN 8
-#define CONF_RESERVED_CLIENTLEN 32
-#define CONF_RESERVED_HOSTLEN 32
-#define CONF_NUMDEVICESLEN 2
+#define CONF_MASTERDIDLEN 4
+#define CONF_PERIODLEN 2
+#define CONF_OUTPUTSKIPFACTORLEN 2
+#define CONF_SYNCIN_MODELEN 2
+#define CONF_SYNCIN_SKIPFACTORLEN 2
+#define CONF_SYNCIN_OFFSETLEN 4
+#define CONF_DATELEN 8
+#define CONF_TIMELEN 8
+#define CONF_RESERVED_CLIENTLEN 32
+#define CONF_RESERVED_HOSTLEN 32
+#define CONF_NUMDEVICESLEN 2
// Configuration sensor block properties
-#define CONF_DIDLEN 4
-#define CONF_DATALENGTHLEN 2
-#define CONF_OUTPUTMODELEN 2
-#define CONF_OUTPUTSETTINGSLEN 4
+#define CONF_DIDLEN 4
+#define CONF_DATALENGTHLEN 2
+#define CONF_OUTPUTMODELEN 2
+#define CONF_OUTPUTSETTINGSLEN 4
// Clock frequency for offset & pulse width
-#define SYNC_CLOCKFREQ 29.4912e6
+#define SYNC_CLOCKFREQ 29.4912e6
// SyncIn params
-#define PARAM_SYNCIN_MODE (const unsigned char)0x00
-#define PARAM_SYNCIN_SKIPFACTOR (const unsigned char)0x01
-#define PARAM_SYNCIN_OFFSET (const unsigned char)0x02
+#define PARAM_SYNCIN_MODE (const unsigned char)0x00
+#define PARAM_SYNCIN_SKIPFACTOR (const unsigned char)0x01
+#define PARAM_SYNCIN_OFFSET (const unsigned char)0x02
// SyncIn mode
-#define SYNCIN_DISABLED 0x0000
-#define SYNCIN_EDGE_RISING 0x0001
-#define SYNCIN_EDGE_FALLING 0x0002
-#define SYNCIN_EDGE_BOTH 0x0003
-#define SYNCIN_TYPE_SENDLASTDATA 0x0004
-#define SYNCIN_TYPE_DOSAMPLING 0x0000
-#define SYNCIN_EDGE_MASK 0x0003
-#define SYNCIN_TYPE_MASK 0x000C
+#define SYNCIN_DISABLED 0x0000
+#define SYNCIN_EDGE_RISING 0x0001
+#define SYNCIN_EDGE_FALLING 0x0002
+#define SYNCIN_EDGE_BOTH 0x0003
+#define SYNCIN_TYPE_SENDLASTDATA 0x0004
+#define SYNCIN_TYPE_DOSAMPLING 0x0000
+#define SYNCIN_EDGE_MASK 0x0003
+#define SYNCIN_TYPE_MASK 0x000C
// SyncOut params
-#define PARAM_SYNCOUT_MODE (const unsigned char)0x00
-#define PARAM_SYNCOUT_SKIPFACTOR (const unsigned char)0x01
-#define PARAM_SYNCOUT_OFFSET (const unsigned char)0x02
-#define PARAM_SYNCOUT_PULSEWIDTH (const unsigned char)0x03
+#define PARAM_SYNCOUT_MODE (const unsigned char)0x00
+#define PARAM_SYNCOUT_SKIPFACTOR (const unsigned char)0x01
+#define PARAM_SYNCOUT_OFFSET (const unsigned char)0x02
+#define PARAM_SYNCOUT_PULSEWIDTH (const unsigned char)0x03
// SyncOut mode
-#define SYNCOUT_DISABLED 0x0000
-#define SYNCOUT_TYPE_TOGGLE 0x0001
-#define SYNCOUT_TYPE_PULSE 0x0002
-#define SYNCOUT_POL_NEG 0x0000
-#define SYNCOUT_POL_POS 0x0010
-#define SYNCOUT_TYPE_MASK 0x000F
-#define SYNCOUT_POL_MASK 0x0010
+#define SYNCOUT_DISABLED 0x0000
+#define SYNCOUT_TYPE_TOGGLE 0x0001
+#define SYNCOUT_TYPE_PULSE 0x0002
+#define SYNCOUT_POL_NEG 0x0000
+#define SYNCOUT_POL_POS 0x0010
+#define SYNCOUT_TYPE_MASK 0x000F
+#define SYNCOUT_POL_MASK 0x0010
// Sample frequencies (SetPeriod)
-#define PERIOD_10HZ 0x2D00
-#define PERIOD_12HZ 0x2580
-#define PERIOD_15HZ 0x1E00
-#define PERIOD_16HZ 0x1C20
-#define PERIOD_18HZ 0x1900
-#define PERIOD_20HZ 0x1680
-#define PERIOD_24HZ 0x12C0
-#define PERIOD_25HZ 0x1200
-#define PERIOD_30HZ 0x0F00
-#define PERIOD_32HZ 0x0E10
-#define PERIOD_36HZ 0x0C80
-#define PERIOD_40HZ 0x0B40
-#define PERIOD_45HZ 0x0A00
-#define PERIOD_48HZ 0x0960
-#define PERIOD_50HZ 0x0900
-#define PERIOD_60HZ 0x0780
-#define PERIOD_64HZ 0x0708
-#define PERIOD_72HZ 0x0640
-#define PERIOD_75HZ 0x0600
-#define PERIOD_80HZ 0x05A0
-#define PERIOD_90HZ 0x0500
-#define PERIOD_96HZ 0x04B0
-#define PERIOD_100HZ 0x0480
-#define PERIOD_120HZ 0x03C0
-#define PERIOD_128HZ 0x0384
-#define PERIOD_144HZ 0x0320
-#define PERIOD_150HZ 0x0300
-#define PERIOD_160HZ 0x02D0
-#define PERIOD_180HZ 0x0280
-#define PERIOD_192HZ 0x0258
-#define PERIOD_200HZ 0x0240
-#define PERIOD_225HZ 0x0200
-#define PERIOD_240HZ 0x01E0
-#define PERIOD_256HZ 0x01C2
-#define PERIOD_288HZ 0x0190
-#define PERIOD_300HZ 0x0180
-#define PERIOD_320HZ 0x0168
-#define PERIOD_360HZ 0x0140
-#define PERIOD_384HZ 0x012C
-#define PERIOD_400HZ 0x0120
-#define PERIOD_450HZ 0x0100
-#define PERIOD_480HZ 0x00F0
-#define PERIOD_512HZ 0x00E1
+#define PERIOD_10HZ 0x2D00
+#define PERIOD_12HZ 0x2580
+#define PERIOD_15HZ 0x1E00
+#define PERIOD_16HZ 0x1C20
+#define PERIOD_18HZ 0x1900
+#define PERIOD_20HZ 0x1680
+#define PERIOD_24HZ 0x12C0
+#define PERIOD_25HZ 0x1200
+#define PERIOD_30HZ 0x0F00
+#define PERIOD_32HZ 0x0E10
+#define PERIOD_36HZ 0x0C80
+#define PERIOD_40HZ 0x0B40
+#define PERIOD_45HZ 0x0A00
+#define PERIOD_48HZ 0x0960
+#define PERIOD_50HZ 0x0900
+#define PERIOD_60HZ 0x0780
+#define PERIOD_64HZ 0x0708
+#define PERIOD_72HZ 0x0640
+#define PERIOD_75HZ 0x0600
+#define PERIOD_80HZ 0x05A0
+#define PERIOD_90HZ 0x0500
+#define PERIOD_96HZ 0x04B0
+#define PERIOD_100HZ 0x0480
+#define PERIOD_120HZ 0x03C0
+#define PERIOD_128HZ 0x0384
+#define PERIOD_144HZ 0x0320
+#define PERIOD_150HZ 0x0300
+#define PERIOD_160HZ 0x02D0
+#define PERIOD_180HZ 0x0280
+#define PERIOD_192HZ 0x0258
+#define PERIOD_200HZ 0x0240
+#define PERIOD_225HZ 0x0200
+#define PERIOD_240HZ 0x01E0
+#define PERIOD_256HZ 0x01C2
+#define PERIOD_288HZ 0x0190
+#define PERIOD_300HZ 0x0180
+#define PERIOD_320HZ 0x0168
+#define PERIOD_360HZ 0x0140
+#define PERIOD_384HZ 0x012C
+#define PERIOD_400HZ 0x0120
+#define PERIOD_450HZ 0x0100
+#define PERIOD_480HZ 0x00F0
+#define PERIOD_512HZ 0x00E1
// OutputModes
-#define OUTPUTMODE_MT9 0x8000
-#define OUTPUTMODE_XM 0x0000
-#define OUTPUTMODE_RAW 0x4000
-#define OUTPUTMODE_TEMP 0x0001
-#define OUTPUTMODE_CALIB 0x0002
-#define OUTPUTMODE_ORIENT 0x0004
+#define OUTPUTMODE_MT9 0x8000
+#define OUTPUTMODE_XM 0x0000
+#define OUTPUTMODE_RAW 0x4000
+#define OUTPUTMODE_TEMP 0x0001
+#define OUTPUTMODE_CALIB 0x0002
+#define OUTPUTMODE_ORIENT 0x0004
// OutputSettings
-#define OUTPUTSETTINGS_XM 0x00000001
-#define OUTPUTSETTINGS_TIMESTAMP_NONE 0x00000000
-#define OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT 0x00000001
-#define OUTPUTSETTINGS_ORIENTMODE_QUATERNION 0x00000000
-#define OUTPUTSETTINGS_ORIENTMODE_EULER 0x00000004
-#define OUTPUTSETTINGS_ORIENTMODE_MATRIX 0x00000008
-#define OUTPUTSETTINGS_CALIBMODE_ACCGYRMAG 0x00000000
-#define OUTPUTSETTINGS_CALIBMODE_ACC 0x00000060
-#define OUTPUTSETTINGS_CALIBMODE_ACCGYR 0x00000040
-#define OUTPUTSETTINGS_CALIBMODE_ACCMAG 0x00000020
-#define OUTPUTSETTINGS_CALIBMODE_GYR 0x00000050
-#define OUTPUTSETTINGS_CALIBMODE_GYRMAG 0x00000010
-#define OUTPUTSETTINGS_CALIBMODE_MAG 0x00000030
-#define OUTPUTSETTINGS_DATAFORMAT_FLOAT 0x00000000
-#define OUTPUTSETTINGS_DATAFORMAT_F1220 0x00000100
-#define OUTPUTSETTINGS_TIMESTAMP_MASK 0x00000003
-#define OUTPUTSETTINGS_ORIENTMODE_MASK 0x0000000C
-#define OUTPUTSETTINGS_CALIBMODE_ACC_MASK 0x00000010
-#define OUTPUTSETTINGS_CALIBMODE_GYR_MASK 0x00000020
-#define OUTPUTSETTINGS_CALIBMODE_MAG_MASK 0x00000040
-#define OUTPUTSETTINGS_CALIBMODE_MASK 0x00000070
-#define OUTPUTSETTINGS_DATAFORMAT_MASK 0x00000300
+#define OUTPUTSETTINGS_XM 0x00000001
+#define OUTPUTSETTINGS_TIMESTAMP_NONE 0x00000000
+#define OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT 0x00000001
+#define OUTPUTSETTINGS_ORIENTMODE_QUATERNION 0x00000000
+#define OUTPUTSETTINGS_ORIENTMODE_EULER 0x00000004
+#define OUTPUTSETTINGS_ORIENTMODE_MATRIX 0x00000008
+#define OUTPUTSETTINGS_CALIBMODE_ACCGYRMAG 0x00000000
+#define OUTPUTSETTINGS_CALIBMODE_ACC 0x00000060
+#define OUTPUTSETTINGS_CALIBMODE_ACCGYR 0x00000040
+#define OUTPUTSETTINGS_CALIBMODE_ACCMAG 0x00000020
+#define OUTPUTSETTINGS_CALIBMODE_GYR 0x00000050
+#define OUTPUTSETTINGS_CALIBMODE_GYRMAG 0x00000010
+#define OUTPUTSETTINGS_CALIBMODE_MAG 0x00000030
+#define OUTPUTSETTINGS_DATAFORMAT_FLOAT 0x00000000
+#define OUTPUTSETTINGS_DATAFORMAT_F1220 0x00000100
+#define OUTPUTSETTINGS_TIMESTAMP_MASK 0x00000003
+#define OUTPUTSETTINGS_ORIENTMODE_MASK 0x0000000C
+#define OUTPUTSETTINGS_CALIBMODE_ACC_MASK 0x00000010
+#define OUTPUTSETTINGS_CALIBMODE_GYR_MASK 0x00000020
+#define OUTPUTSETTINGS_CALIBMODE_MAG_MASK 0x00000040
+#define OUTPUTSETTINGS_CALIBMODE_MASK 0x00000070
+#define OUTPUTSETTINGS_DATAFORMAT_MASK 0x00000300
// Extended Output Modes
-#define EXTOUTPUTMODE_DISABLED 0x0000
-#define EXTOUTPUTMODE_EULER 0x0001
+#define EXTOUTPUTMODE_DISABLED 0x0000
+#define EXTOUTPUTMODE_EULER 0x0001
// Factory Output Mode
-#define FACTORYOUTPUTMODE_DISABLE 0x0000
-#define FACTORYOUTPUTMODE_DEFAULT 0x0001
-#define FACTORYOUTPUTMODE_CUSTOM 0x0002
+#define FACTORYOUTPUTMODE_DISABLE 0x0000
+#define FACTORYOUTPUTMODE_DEFAULT 0x0001
+#define FACTORYOUTPUTMODE_CUSTOM 0x0002
// Initial tracking mode (SetInitTrackMode)
-#define INITTRACKMODE_DISABLED 0x0000
-#define INITTRACKMODE_ENABLED 0x0001
+#define INITTRACKMODE_DISABLED 0x0000
+#define INITTRACKMODE_ENABLED 0x0001
// Filter settings params
-#define PARAM_FILTER_GAIN (const unsigned char)0x00
-#define PARAM_FILTER_RHO (const unsigned char)0x01
-#define DONOTSTORE 0x00
-#define STORE 0x01
+#define PARAM_FILTER_GAIN (const unsigned char)0x00
+#define PARAM_FILTER_RHO (const unsigned char)0x01
+#define DONOTSTORE 0x00
+#define STORE 0x01
// AMDSetting (SetAMD)
-#define AMDSETTING_DISABLED 0x0000
-#define AMDSETTING_ENABLED 0x0001
+#define AMDSETTING_DISABLED 0x0000
+#define AMDSETTING_ENABLED 0x0001
// Reset orientation message type
-#define RESETORIENTATION_STORE 0
-#define RESETORIENTATION_HEADING 1
-#define RESETORIENTATION_GLOBAL 2
-#define RESETORIENTATION_OBJECT 3
-#define RESETORIENTATION_ALIGN 4
+#define RESETORIENTATION_STORE 0
+#define RESETORIENTATION_HEADING 1
+#define RESETORIENTATION_GLOBAL 2
+#define RESETORIENTATION_OBJECT 3
+#define RESETORIENTATION_ALIGN 4
// Send raw string mode
-#define SENDRAWSTRING_INIT 0
-#define SENDRAWSTRING_DEFAULT 1
-#define SENDRAWSTRING_SEND 2
+#define SENDRAWSTRING_INIT 0
+#define SENDRAWSTRING_DEFAULT 1
+#define SENDRAWSTRING_SEND 2
// Timeouts
-#define TO_DEFAULT 500
-#define TO_INIT 250
-#define TO_RETRY 50
+#define TO_DEFAULT 500
+#define TO_INIT 250
+#define TO_RETRY 50
// openPort baudrates
#ifdef WIN32
-#define PBR_9600 CBR_9600
-#define PBR_14K4 CBR_14400
-#define PBR_19K2 CBR_19200
-#define PBR_28K8 28800
-#define PBR_38K4 CBR_38400
-#define PBR_57K6 CBR_57600
-#define PBR_76K8 76800
-#define PBR_115K2 CBR_115200
-#define PBR_230K4 230400
-#define PBR_460K8 460800
-#define PBR_921K6 921600
+#define PBR_9600 CBR_9600
+#define PBR_14K4 CBR_14400
+#define PBR_19K2 CBR_19200
+#define PBR_28K8 28800
+#define PBR_38K4 CBR_38400
+#define PBR_57K6 CBR_57600
+#define PBR_76K8 76800
+#define PBR_115K2 CBR_115200
+#define PBR_230K4 230400
+#define PBR_460K8 460800
+#define PBR_921K6 921600
#else
-#define PBR_9600 B9600
-#define PBR_14K4 B14400
-#define PBR_19K2 B19200
-#define PBR_28K8 B28800
-#define PBR_38K4 B38400
-#define PBR_57K6 B57600
-#define PBR_76K8 B76800
-#define PBR_115K2 B115200
-#define PBR_230K4 B230400
-#define PBR_460K8 B460800
-#define PBR_921K6 B921600
+#define PBR_9600 B9600
+#define PBR_14K4 B14400
+#define PBR_19K2 B19200
+#define PBR_28K8 B28800
+#define PBR_38K4 B38400
+#define PBR_57K6 B57600
+#define PBR_76K8 B76800
+#define PBR_115K2 B115200
+#define PBR_230K4 B230400
+#define PBR_460K8 B460800
+#define PBR_921K6 B921600
#endif
// setFilePos defines
#ifdef WIN32
-#define FILEPOS_BEGIN FILE_BEGIN
-#define FILEPOS_CURRENT FILE_CURRENT
-#define FILEPOS_END FILE_END
+#define FILEPOS_BEGIN FILE_BEGIN
+#define FILEPOS_CURRENT FILE_CURRENT
+#define FILEPOS_END FILE_END
#else
-#define FILEPOS_BEGIN SEEK_SET
-#define FILEPOS_CURRENT SEEK_CUR
-#define FILEPOS_END SEEK_END
+#define FILEPOS_BEGIN SEEK_SET
+#define FILEPOS_CURRENT SEEK_CUR
+#define FILEPOS_END SEEK_END
#endif
// Return values
-#define MTRV_OK 0 // Operation successful
-#define MTRV_NOTSUCCESSFUL 1 // General no success return value
-#define MTRV_TIMEOUT 2 // Operation aborted because of a timeout
-#define MTRV_TIMEOUTNODATA 3 // Operation aborted because of no data read
-#define MTRV_CHECKSUMFAULT 4 // Checksum fault occured
-#define MTRV_NODATA 5 // No data is received
-#define MTRV_RECVERRORMSG 6 // A error message is received
-#define MTRV_OUTOFMEMORY 7 // No internal memory available
-#define MTRV_UNKNOWDATA 8 // An invalid message is read
-#define MTRV_INVALIDTIMEOUT 9 // An invalid value is used to set the timeout
-#define MTRV_UNEXPECTEDMSG 10 // Unexpected message received (e.g. no acknowledge message received)
-#define MTRV_INPUTCANNOTBEOPENED 11 // The specified file / serial port can not be opened
-#define MTRV_ANINPUTALREADYOPEN 12 // File and serial port can not be opened at same time
-#define MTRV_ENDOFFILE 13 // End of file is reached
-#define MTRV_NOINPUTINITIALIZED 14 // No file or serial port opened for reading/writing
-#define MTRV_NOVALIDMODESPECIFIED 15 // No valid outputmode or outputsettings are specified (use
+#define MTRV_OK 0 // Operation successful
+#define MTRV_NOTSUCCESSFUL 1 // General no success return value
+#define MTRV_TIMEOUT 2 // Operation aborted because of a timeout
+#define MTRV_TIMEOUTNODATA 3 // Operation aborted because of no data read
+#define MTRV_CHECKSUMFAULT 4 // Checksum fault occured
+#define MTRV_NODATA 5 // No data is received
+#define MTRV_RECVERRORMSG 6 // A error message is received
+#define MTRV_OUTOFMEMORY 7 // No internal memory available
+#define MTRV_UNKNOWDATA 8 // An invalid message is read
+#define MTRV_INVALIDTIMEOUT 9 // An invalid value is used to set the timeout
+#define MTRV_UNEXPECTEDMSG 10 // Unexpected message received (e.g. no acknowledge message received)
+#define MTRV_INPUTCANNOTBEOPENED 11 // The specified file / serial port can not be opened
+#define MTRV_ANINPUTALREADYOPEN 12 // File and serial port can not be opened at same time
+#define MTRV_ENDOFFILE 13 // End of file is reached
+#define MTRV_NOINPUTINITIALIZED 14 // No file or serial port opened for reading/writing
+#define MTRV_NOVALIDMODESPECIFIED 15 // No valid outputmode or outputsettings are specified (use
// mtGetDeviceMode or mtSetMode)
-#define MTRV_INVALIDVALUESPEC 16 // Value specifier does not match value type or not available in data
-#define MTRV_INVALIDFORFILEINPUT 17 // Function is not valid for file based interfaces
+#define MTRV_INVALIDVALUESPEC \
+ 16 // Value specifier does not match value type or not available in data
+#define MTRV_INVALIDFORFILEINPUT 17 // Function is not valid for file based interfaces
class CXsensMTiModule
{
@@ -932,8 +934,14 @@ namespace IMU::Xsens
clock_t clockms();
// Low level COM port / file functions
- short openPort(const int portNumber, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
- short openPort(const char* portName, const unsigned long baudrate = PBR_115K2, const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ short openPort(const int portNumber,
+ const unsigned long baudrate = PBR_115K2,
+ const unsigned long inqueueSize = 4096,
+ const unsigned long outqueueSize = 1024);
+ short openPort(const char* portName,
+ const unsigned long baudrate = PBR_115K2,
+ const unsigned long inqueueSize = 4096,
+ const unsigned long outqueueSize = 1024);
short openFile(const char* fileName, bool createAlways = false);
bool isPortOpen();
bool isFileOpen();
@@ -941,48 +949,116 @@ namespace IMU::Xsens
int writeData(const unsigned char* msgBuffer, const int nBytesToWrite);
void flush();
void escape(unsigned long function);
- void setPortQueueSize(const unsigned long inqueueSize = 4096, const unsigned long outqueueSize = 1024);
+ void setPortQueueSize(const unsigned long inqueueSize = 4096,
+ const unsigned long outqueueSize = 1024);
short setFilePos(long relPos, unsigned long moveMethod = FILEPOS_BEGIN);
short getFileSize(unsigned long& fileSize);
short close();
// Read & write message functions
- short readMessage(unsigned char& mid, unsigned char data[], short& dataLen, unsigned char* bid = NULL);
+ short readMessage(unsigned char& mid,
+ unsigned char data[],
+ short& dataLen,
+ unsigned char* bid = NULL);
short readDataMessage(unsigned char data[], short& dataLen);
short readMessageRaw(unsigned char* msgBuffer, short* msgBufferLength);
- short writeMessage(const unsigned char mid, const unsigned long dataValue = 0, const unsigned char dataValueLen = 0, const unsigned char bid = BID_MASTER);
- short writeMessage(const unsigned char mid, const unsigned char data[], const unsigned short& dataLen, const unsigned char bid = BID_MASTER);
- short waitForMessage(const unsigned char mid, unsigned char data[] = NULL, short* dataLen = NULL, unsigned char* bid = NULL);
+ short writeMessage(const unsigned char mid,
+ const unsigned long dataValue = 0,
+ const unsigned char dataValueLen = 0,
+ const unsigned char bid = BID_MASTER);
+ short writeMessage(const unsigned char mid,
+ const unsigned char data[],
+ const unsigned short& dataLen,
+ const unsigned char bid = BID_MASTER);
+ short waitForMessage(const unsigned char mid,
+ unsigned char data[] = NULL,
+ short* dataLen = NULL,
+ unsigned char* bid = NULL);
// Request & set setting functions
- short reqSetting(const unsigned char mid, unsigned long& value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, const unsigned char param, unsigned long& value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, float& value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, const unsigned char param, float& value, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, unsigned char data[], short& dataLen, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, unsigned char dataIn[], short dataInLen, unsigned char dataOut[], short& dataOutLen, const unsigned char bid = BID_MASTER);
- short reqSetting(const unsigned char mid, const unsigned char param, unsigned char data[], short& dataLen, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned char param, const unsigned long value, const unsigned short valuelen, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const float value, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned char param, const float value, const unsigned char bid = BID_MASTER);
- short setSetting(const unsigned char mid, const unsigned char param, const float value, const bool store, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid,
+ unsigned long& value,
+ const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid,
+ const unsigned char param,
+ unsigned long& value,
+ const unsigned char bid = BID_MASTER);
+ short
+ reqSetting(const unsigned char mid, float& value, const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid,
+ const unsigned char param,
+ float& value,
+ const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid,
+ unsigned char data[],
+ short& dataLen,
+ const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid,
+ unsigned char dataIn[],
+ short dataInLen,
+ unsigned char dataOut[],
+ short& dataOutLen,
+ const unsigned char bid = BID_MASTER);
+ short reqSetting(const unsigned char mid,
+ const unsigned char param,
+ unsigned char data[],
+ short& dataLen,
+ const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid,
+ const unsigned long value,
+ const unsigned short valuelen,
+ const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid,
+ const unsigned char param,
+ const unsigned long value,
+ const unsigned short valuelen,
+ const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid,
+ const float value,
+ const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid,
+ const unsigned char param,
+ const float value,
+ const unsigned char bid = BID_MASTER);
+ short setSetting(const unsigned char mid,
+ const unsigned char param,
+ const float value,
+ const bool store,
+ const unsigned char bid = BID_MASTER);
// Data-related functions
short getDeviceMode(unsigned short* numDevices = NULL);
- short setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
- short getMode(unsigned long& OutputMode, unsigned long& OutputSettings, unsigned short& dataLength, const unsigned char bid = BID_MASTER);
- short setMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid = BID_MASTER);
- short getValue(const unsigned long valueSpec, unsigned short& value, const unsigned char data[], const unsigned char bid = BID_MT);
- short getValue(const unsigned long valueSpec, unsigned short value[], const unsigned char data[], const unsigned char bid = BID_MT);
- short getValue(const unsigned long valueSpec, float value[], const unsigned char data[], const unsigned char bid = BID_MT);
+ short setDeviceMode(unsigned long OutputMode,
+ unsigned long OutputSettings,
+ const unsigned char bid = BID_MASTER);
+ short getMode(unsigned long& OutputMode,
+ unsigned long& OutputSettings,
+ unsigned short& dataLength,
+ const unsigned char bid = BID_MASTER);
+ short setMode(unsigned long OutputMode,
+ unsigned long OutputSettings,
+ const unsigned char bid = BID_MASTER);
+ short getValue(const unsigned long valueSpec,
+ unsigned short& value,
+ const unsigned char data[],
+ const unsigned char bid = BID_MT);
+ short getValue(const unsigned long valueSpec,
+ unsigned short value[],
+ const unsigned char data[],
+ const unsigned char bid = BID_MT);
+ short getValue(const unsigned long valueSpec,
+ float value[],
+ const unsigned char data[],
+ const unsigned char bid = BID_MT);
// Generic MTComm functions
short getLastDeviceError();
short getLastRetVal();
short setTimeOut(short timeOutMs);
- static void swapEndian(const unsigned char input[], unsigned char output[], const int length);
+ static void
+ swapEndian(const unsigned char input[], unsigned char output[], const int length);
void calcChecksum(unsigned char* msgBuffer, const int msgBufferLength);
bool checkChecksum(const unsigned char* msgBuffer, const int msgBufferLength);
+
protected:
// member variables
#ifdef WIN32
@@ -1003,12 +1079,12 @@ namespace IMU::Xsens
unsigned long m_storedDataLength[MAXDEVICES + 1];
// Temporary buffer for excess bytes read in ReadMessageRaw
- unsigned char m_tempBuffer[MAXMSGLEN ];
+ unsigned char m_tempBuffer[MAXMSGLEN];
int m_nTempBufferLen;
private:
};
-}
+} // namespace IMU::Xsens
#endif
diff --git a/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp b/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
index 16b3e89e0a87346ddcaf5e0576bd6eb2b979248f..e00baf761646b4f613e03f89c10cdae9d20a8fae 100644
--- a/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
@@ -24,17 +24,18 @@
#include "XsensIMU.h"
-
namespace armarx
{
using namespace IMU;
- PropertyDefinitionsPtr XsensIMU::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ XsensIMU::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new XsensIMUPropertyDefinitions(getConfigIdentifier()));
}
- void XsensIMU::frameAcquisitionTaskLoop()
+ void
+ XsensIMU::frameAcquisitionTaskLoop()
{
std::vector<float> offset(3, 0.0);
@@ -46,7 +47,8 @@ namespace armarx
if (getProperty<bool>("EnableSimpleCalibration").getValue())
{
ARMARX_WARNING << "Estimation of the offset for the IMU, please do not move the IMU";
- while (((armarx::TimeUtil::GetTime() - startTime) < IceUtil::Time::seconds(5)) && !sensorTask->isStopped())
+ while (((armarx::TimeUtil::GetTime() - startTime) < IceUtil::Time::seconds(5)) &&
+ !sensorTask->isStopped())
{
while (HasPendingEvents())
@@ -57,14 +59,13 @@ namespace armarx
offset[1] += m_GyroscopeRotation[1];
offset[2] += m_GyroscopeRotation[2];
- count ++;
+ count++;
}
}
offset[0] /= count;
offset[1] /= count;
offset[2] /= count;
-
}
@@ -99,14 +100,12 @@ namespace armarx
data.orientationQuaternion.push_back(m_OrientationQuaternion[3]);
IMUTopicPrx->reportSensorValues("XsensIMU", "XsensIMU", data, now);
-
}
usleep(10000);
}
}
-
/*
void XsensIMU::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
{
@@ -138,7 +137,8 @@ namespace armarx
*/
- void XsensIMU::onInitIMU()
+ void
+ XsensIMU::onInitIMU()
{
sensorTask = new RunningTask<XsensIMU>(this, &XsensIMU::frameAcquisitionTaskLoop);
@@ -148,18 +148,21 @@ namespace armarx
//IMUDevice.SetFusion(IMU::CIMUDevice::eGaussianFusion, 2);
IMUDevice.RegisterEventDispatcher(this);
- IMUDevice.Connect(_IMU_DEVICE_DEFAUL_CONNECTION_, IMU::CIMUDevice::eSamplingFrequency_120HZ);
+ IMUDevice.Connect(_IMU_DEVICE_DEFAUL_CONNECTION_,
+ IMU::CIMUDevice::eSamplingFrequency_120HZ);
}
- void XsensIMU::onStartIMU()
+ void
+ XsensIMU::onStartIMU()
{
IMUDevice.Start(false);
sensorTask->start();
}
- void XsensIMU::onExitIMU()
+ void
+ XsensIMU::onExitIMU()
{
IMUDevice.Stop();
sensorTask->stop();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/drivers/XsensIMU/XsensIMU.h b/source/RobotAPI/drivers/XsensIMU/XsensIMU.h
index 017a53ba1177caaa2a6ef0274a919fdc95c63295..149bb77b336812bc9be16184228eadc3e5200651 100644
--- a/source/RobotAPI/drivers/XsensIMU/XsensIMU.h
+++ b/source/RobotAPI/drivers/XsensIMU/XsensIMU.h
@@ -29,7 +29,6 @@
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
#include <RobotAPI/components/units/InertialMeasurementUnit.h>
#include "IMU/IMU.h"
@@ -40,11 +39,10 @@ namespace armarx
* @class XsensIMUPropertyDefinitions
* @brief
*/
- class XsensIMUPropertyDefinitions:
- public InertialMeasurementUnitPropertyDefinitions
+ class XsensIMUPropertyDefinitions : public InertialMeasurementUnitPropertyDefinitions
{
public:
- XsensIMUPropertyDefinitions(std::string prefix):
+ XsensIMUPropertyDefinitions(std::string prefix) :
InertialMeasurementUnitPropertyDefinitions(prefix)
{
defineOptionalProperty<std::string>("deviceConnection", "/dev/ttyUSB0", "");
@@ -66,23 +64,20 @@ namespace armarx
virtual public IMU::CIMUDeducedReckoning
{
public:
-
-
- XsensIMU(): CIMUDeducedReckoning(false)
+ XsensIMU() : CIMUDeducedReckoning(false)
{
-
}
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "XsensIMU";
}
protected:
-
/**
* @see PropertyUser::createPropertyDefinitions()
*/
@@ -98,9 +93,7 @@ namespace armarx
private:
-
RunningTask<XsensIMU>::pointer_type sensorTask;
IMU::CIMUDevice IMUDevice;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.h b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.h
index ca4db2ac23e5756bd96726ef830c103327dd1d5e..90fc616ef001a6671e4a96f244f836a8c9df221c 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.h
@@ -25,15 +25,13 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <RobotAPI/libraries/armem_gui/lifecycle.h>
-#include <RobotAPI/libraries/armem_gui/MemoryViewer.h>
-
#include <RobotAPI/gui-plugins/ArMemMemoryViewer/ui_ArMemMemoryViewerWidget.h>
-
+#include <RobotAPI/libraries/armem_gui/MemoryViewer.h>
+#include <RobotAPI/libraries/armem_gui/lifecycle.h>
namespace armarx
{
@@ -62,16 +60,14 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- ArMemMemoryViewerWidgetController :
- public armarx::ArmarXComponentWidgetControllerTemplate < ArMemMemoryViewerWidgetController >
+ class ARMARXCOMPONENT_IMPORT_EXPORT ArMemMemoryViewerWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<ArMemMemoryViewerWidgetController>
{
Q_OBJECT
using This = ArMemMemoryViewerWidgetController;
using MemoryViewer = armarx::armem::gui::MemoryViewer;
public:
-
static QString GetWidgetName();
static QIcon GetWidgetIcon();
@@ -102,7 +98,6 @@ namespace armarx
private:
-
/// Widget Form
Ui::ArMemMemoryViewerWidget widget;
armarx::gui::LifecycleServer lifecycleServer;
@@ -110,8 +105,5 @@ namespace armarx
QPointer<SimpleConfigDialog> configDialog;
std::unique_ptr<MemoryViewer> viewer;
-
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.cpp
index c73c555c8a67d1bc5ce25ec727b8838372b5a116..93b98fb5145903eeb365b7658160945cb1122d96 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.cpp
@@ -28,6 +28,6 @@ namespace armarx
{
ArVizGuiPlugin::ArVizGuiPlugin()
{
- addWidget < ArVizWidgetController > ();
+ addWidget<ArVizWidgetController>();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.h b/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.h
index 5b2ab4dd6c94b58497be62880a3eac674aab38be..6d7a7d434378799d248c4dbf3637314210e7b033 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizGuiPlugin.h
@@ -22,8 +22,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -34,8 +35,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT ArVizGuiPlugin:
- public armarx::ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT ArVizGuiPlugin : public armarx::ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -48,4 +48,4 @@ namespace armarx
*/
ArVizGuiPlugin();
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index fc014c5adab046771815191a4c1e997602dca519..d83a140a69225228b258284c4a328f582332b7dc 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -22,15 +22,14 @@
#include "ArVizWidgetController.h"
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-#include <ArmarXCore/observers/variant/Variant.h>
-
+#include <QFileDialog>
#include <QLineEdit>
#include <QMessageBox>
#include <QTimer>
-#include <QFileDialog>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <ArmarXCore/observers/variant/Variant.h>
namespace armarx
{
@@ -38,15 +37,16 @@ namespace armarx
{
struct ArVizWidgetController* this_;
- void onSuccess(viz::data::RecordingBatch const& batch)
+ void
+ onSuccess(viz::data::RecordingBatch const& batch)
{
this_->onGetBatchAsync(batch);
}
- void onFailure(Ice::Exception const& ex)
+ void
+ onFailure(Ice::Exception const& ex)
{
- ARMARX_WARNING << "Failed to get batch async.\nReason:"
- << ex;
+ ARMARX_WARNING << "Failed to get batch async.\nReason:" << ex;
}
};
@@ -74,11 +74,23 @@ namespace armarx
connect(widget.recordingStartButton, &QPushButton::clicked, this, &This::onStartRecording);
connect(widget.recordingStopButton, &QPushButton::clicked, this, &This::onStopRecording);
- connect(widget.refreshRecordingsButton, &QPushButton::clicked, this, &This::onRefreshRecordings);
- connect(widget.recordingList, &QListWidget::currentItemChanged, this, &This::onRecordingSelectionChanged);
-
- connect(widget.replayRevisionSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, &This::onReplaySpinChanged);
- connect(widget.replayRevisionSlider, QOverload<int>::of(&QSlider::valueChanged), this, &This::onReplaySliderChanged);
+ connect(widget.refreshRecordingsButton,
+ &QPushButton::clicked,
+ this,
+ &This::onRefreshRecordings);
+ connect(widget.recordingList,
+ &QListWidget::currentItemChanged,
+ this,
+ &This::onRecordingSelectionChanged);
+
+ connect(widget.replayRevisionSpinBox,
+ QOverload<int>::of(&QSpinBox::valueChanged),
+ this,
+ &This::onReplaySpinChanged);
+ connect(widget.replayRevisionSlider,
+ QOverload<int>::of(&QSlider::valueChanged),
+ this,
+ &This::onReplaySliderChanged);
connect(widget.replayStartButton, &QPushButton::clicked, this, &This::onReplayStart);
connect(widget.replayStopButton, &QPushButton::clicked, this, &This::onReplayStop);
@@ -87,17 +99,26 @@ namespace armarx
connect(widget.exportToVRMLButton, &QPushButton::clicked, this, &This::exportToVRML);
connect(widget.deselectButton, &QPushButton::clicked, this, &This::onDeselectElement);
- connect(widget.listInteractiveElements, &QListWidget::itemDoubleClicked, this, &This::onInteractiveElementSelected);
+ connect(widget.listInteractiveElements,
+ &QListWidget::itemDoubleClicked,
+ this,
+ &This::onInteractiveElementSelected);
connect(this, &This::connectGui, this, &This::onConnectGui, Qt::QueuedConnection);
connect(this, &This::disconnectGui, this, &This::onDisconnectGui, Qt::QueuedConnection);
// Layer info tree.
- connect(widget.layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer);
-
- connect(widget.layerInfoTreeGroupBox, &QGroupBox::toggled, &layerInfoTree, &LayerInfoTree::setEnabled);
- connect(widget.defaultShowLimitSpinBox, qOverload<int>(&QSpinBox::valueChanged),
- &layerInfoTree, &LayerInfoTree::setMaxElementCountDefault);
+ connect(
+ widget.layerTree, &QTreeWidget::currentItemChanged, this, &This::updateSelectedLayer);
+
+ connect(widget.layerInfoTreeGroupBox,
+ &QGroupBox::toggled,
+ &layerInfoTree,
+ &LayerInfoTree::setEnabled);
+ connect(widget.defaultShowLimitSpinBox,
+ qOverload<int>(&QSpinBox::valueChanged),
+ &layerInfoTree,
+ &LayerInfoTree::setMaxElementCountDefault);
layerInfoTree.setMaxElementCountDefault(widget.defaultShowLimitSpinBox->value());
layerInfoTree.setWidget(widget.layerInfoTree);
@@ -106,26 +127,30 @@ namespace armarx
// We need a callback from the visualizer, when the layers have changed
// So we can update the tree accordingly
- visualizer.layersChangedCallback = [this](std::vector<viz::CoinLayerID> const & layers)
- {
- layersChanged(layers);
- };
+ visualizer.layersChangedCallback = [this](std::vector<viz::CoinLayerID> const& layers)
+ { layersChanged(layers); };
// Arviz Profile tree.
arvizeProfileLayerWidget = new ArvizProfileManagerWidget();
widget.verticalLayout->addWidget(arvizeProfileLayerWidget);
- connect(arvizeProfileLayerWidget, &ArvizProfileManagerWidget::publishUpdate, this, &This::updateLayersFromProfile);
- connect(arvizeProfileLayerWidget, &ArvizProfileManagerWidget::fetchUpdate, this, &This::updateLayers);
+ connect(arvizeProfileLayerWidget,
+ &ArvizProfileManagerWidget::publishUpdate,
+ this,
+ &This::updateLayersFromProfile);
+ connect(arvizeProfileLayerWidget,
+ &ArvizProfileManagerWidget::fetchUpdate,
+ this,
+ &This::updateLayers);
replayTimer->start(33);
}
ArVizWidgetController::~ArVizWidgetController()
{
-
}
- void ArVizWidgetController::onInitComponent()
+ void
+ ArVizWidgetController::onInitComponent()
{
if (storageName.size() > 0)
{
@@ -139,17 +164,19 @@ namespace armarx
callbackData = new ArVizWidgetBatchCallback();
callbackData->this_ = this;
callback = viz::newCallback_StorageInterface_getRecordingBatch(
- callbackData,
- &ArVizWidgetBatchCallback::onSuccess,
- &ArVizWidgetBatchCallback::onFailure);
+ callbackData,
+ &ArVizWidgetBatchCallback::onSuccess,
+ &ArVizWidgetBatchCallback::onFailure);
}
- void ArVizWidgetController::onExitComponent()
+ void
+ ArVizWidgetController::onExitComponent()
{
visualizer.clearCache();
}
- void ArVizWidgetController::onConnectComponent()
+ void
+ ArVizWidgetController::onConnectComponent()
{
getProxy(storage, storageName);
// DebugObserver is optional (check for null on every call)
@@ -165,19 +192,22 @@ namespace armarx
emit connectGui();
}
- void ArVizWidgetController::onDisconnectComponent()
+ void
+ ArVizWidgetController::onDisconnectComponent()
{
// Changes to UI elements are only allowed in the GUI thread
emit disconnectGui();
}
- void ArVizWidgetController::updateLayers()
+ void
+ ArVizWidgetController::updateLayers()
{
// Update shown/hidden layers
layerTreeChanged(nullptr, 0);
}
- void ArVizWidgetController::updateLayersFromProfile()
+ void
+ ArVizWidgetController::updateLayersFromProfile()
{
{
QSignalBlocker blocker(widget.layerTree);
@@ -194,16 +224,19 @@ namespace armarx
const std::string layer = layerItem->text(0).toStdString();
const viz::CoinLayerID layerID(component, layer);
const bool visible = not arvizeProfileLayerWidget->isHidden(layerID);
- layerItem->setCheckState(0, visible ? Qt::CheckState::Checked : Qt::CheckState::Unchecked);
+ layerItem->setCheckState(
+ 0, visible ? Qt::CheckState::Checked : Qt::CheckState::Unchecked);
parentChecked &= visible;
visualizer.showLayer(layerID, visible);
}
- componentItem->setCheckState(0, parentChecked ? Qt::CheckState::Checked : Qt::CheckState::Unchecked);
+ componentItem->setCheckState(
+ 0, parentChecked ? Qt::CheckState::Checked : Qt::CheckState::Unchecked);
}
}
}
- void ArVizWidgetController::onConnectGui()
+ void
+ ArVizWidgetController::onConnectGui()
{
onRefreshRecordings();
currentRecordingSelected = false;
@@ -212,14 +245,16 @@ namespace armarx
updateTimer->start(33);
}
- void ArVizWidgetController::onDisconnectGui()
+ void
+ ArVizWidgetController::onDisconnectGui()
{
visualizer.stop();
updateTimer->stop();
changeMode(ArVizWidgetMode::NotConnected);
}
- void ArVizWidgetController::layerTreeChanged(QTreeWidgetItem* item, int /*column*/)
+ void
+ ArVizWidgetController::layerTreeChanged(QTreeWidgetItem* item, int /*column*/)
{
// Iterate over all items and activate/deactivate layers accordingly
int componentCount = widget.layerTree->topLevelItemCount();
@@ -235,7 +270,8 @@ namespace armarx
if (componentItem == item)
{
// The parent was selected or deselected, so all children should be set accordingly
- ARMARX_VERBOSE << "Setting all children of " << component << " to " << (componentCheck == Qt::Checked);
+ ARMARX_VERBOSE << "Setting all children of " << component << " to "
+ << (componentCheck == Qt::Checked);
for (int layerIndex = 0; layerIndex < layerCount; ++layerIndex)
{
QTreeWidgetItem* layerItem = componentItem->child(layerIndex);
@@ -262,7 +298,8 @@ namespace armarx
}
}
- void ArVizWidgetController::layersChanged(std::vector<viz::CoinLayerID> const& layers)
+ void
+ ArVizWidgetController::layersChanged(std::vector<viz::CoinLayerID> const& layers)
{
QTreeWidget* tree = widget.layerTree;
@@ -301,10 +338,14 @@ namespace armarx
if (iter == currentComponents.end())
{
// Create a new item
- QSignalBlocker blocker(tree); // otherwise added as unchecked and directly updated
+ QSignalBlocker blocker(
+ tree); // otherwise added as unchecked and directly updated
currentItem = new QTreeWidgetItem(tree);
currentItem->setText(0, QString::fromStdString(component));
- currentItem->setCheckState(0, arvizeProfileLayerWidget->isHidden(component) ? Qt::Unchecked : Qt::Checked);
+ currentItem->setCheckState(0,
+ arvizeProfileLayerWidget->isHidden(component)
+ ? Qt::Unchecked
+ : Qt::Checked);
componentWasNew = true;
}
@@ -326,7 +367,8 @@ namespace armarx
std::string const& layer = entry.second;
QTreeWidgetItem* layerItem = new QTreeWidgetItem;
layerItem->setText(0, QString::fromStdString(layer));
- layerItem->setCheckState(0, arvizeProfileLayerWidget->isHidden(entry) ? Qt::Unchecked : Qt::Checked);
+ layerItem->setCheckState(
+ 0, arvizeProfileLayerWidget->isHidden(entry) ? Qt::Unchecked : Qt::Checked);
if (currentItem)
{
@@ -341,14 +383,15 @@ namespace armarx
else
{
ARMARX_WARNING << deactivateSpam(10, entry.first + entry.second)
- << "Invalid component/layer ID: "
- << entry.first << " / " << entry.second;
+ << "Invalid component/layer ID: " << entry.first << " / "
+ << entry.second;
}
}
else
{
// Item exists already ==> make sure that it is not shown if necessary
- if (arvizeProfileLayerWidget->isHidden(iter->first) or iter->second->checkState(0) == Qt::Unchecked)
+ if (arvizeProfileLayerWidget->isHidden(iter->first) or
+ iter->second->checkState(0) == Qt::Unchecked)
{
visualizer.showLayer(iter->first, false);
}
@@ -356,9 +399,10 @@ namespace armarx
}
}
- void ArVizWidgetController::updateSelectedLayer(QTreeWidgetItem* current, QTreeWidgetItem* previous)
+ void
+ ArVizWidgetController::updateSelectedLayer(QTreeWidgetItem* current, QTreeWidgetItem* previous)
{
- (void) previous;
+ (void)previous;
if (!current->parent())
{
@@ -367,12 +411,14 @@ namespace armarx
}
// A layer was selected.
- viz::CoinLayerID id(current->parent()->text(0).toStdString(), current->text(0).toStdString());
+ viz::CoinLayerID id(current->parent()->text(0).toStdString(),
+ current->text(0).toStdString());
viz::CoinLayer* layer = visualizer.layers.findLayer(id);
if (layer == nullptr)
{
- ARMARX_WARNING << "Could not find layer (" << id.first << " / " << id.second << ") in Visualizer.";
+ ARMARX_WARNING << "Could not find layer (" << id.first << " / " << id.second
+ << ") in Visualizer.";
}
else
{
@@ -380,37 +426,44 @@ namespace armarx
}
}
- void ArVizWidgetController::onCollapseAll(bool)
+ void
+ ArVizWidgetController::onCollapseAll(bool)
{
widget.layerTree->collapseAll();
}
- void ArVizWidgetController::onExpandAll(bool)
+ void
+ ArVizWidgetController::onExpandAll(bool)
{
widget.layerTree->expandAll();
}
- void ArVizWidgetController::onHideAll(bool)
+ void
+ ArVizWidgetController::onHideAll(bool)
{
showAllLayers(false);
}
- void ArVizWidgetController::onShowAll(bool)
+ void
+ ArVizWidgetController::onShowAll(bool)
{
showAllLayers(true);
}
- void ArVizWidgetController::onHideFiltered(bool)
+ void
+ ArVizWidgetController::onHideFiltered(bool)
{
showFilteredLayers(false);
}
- void ArVizWidgetController::onShowFiltered(bool)
+ void
+ ArVizWidgetController::onShowFiltered(bool)
{
showFilteredLayers(true);
}
- void ArVizWidgetController::onFilterTextChanged(const QString& filter)
+ void
+ ArVizWidgetController::onFilterTextChanged(const QString& filter)
{
// Now we need to show these matches and hide the other items
// Is there a better way? Probably, via QSortFilterProxyModel...
@@ -429,7 +482,8 @@ namespace armarx
}
}
- void ArVizWidgetController::showAllLayers(bool visible)
+ void
+ ArVizWidgetController::showAllLayers(bool visible)
{
widget.layerTree->blockSignals(true);
@@ -444,7 +498,8 @@ namespace armarx
updateLayers();
}
- void ArVizWidgetController::showFilteredLayers(bool visible)
+ void
+ ArVizWidgetController::showFilteredLayers(bool visible)
{
widget.layerTree->blockSignals(true);
@@ -467,14 +522,16 @@ namespace armarx
updateLayers();
}
- void ArVizWidgetController::onDeselectElement()
+ void
+ ArVizWidgetController::onDeselectElement()
{
// We just deselect all elements.
// Maybe we need to be more specific for strange use cases (?)
visualizer.selection->deselectAll();
}
- void ArVizWidgetController::onContextMenuClicked()
+ void
+ ArVizWidgetController::onContextMenuClicked()
{
viz::ElementInteractionData* selected = visualizer.selectedElement;
if (selected == nullptr)
@@ -492,7 +549,8 @@ namespace armarx
QPushButton* button = static_cast<QPushButton*>(layout->itemAt(i)->widget());
if (button == clickedButton)
{
- viz::data::InteractionFeedback& interaction = visualizer.interactionFeedbackBuffer.emplace_back();
+ viz::data::InteractionFeedback& interaction =
+ visualizer.interactionFeedbackBuffer.emplace_back();
interaction.component = selected->layer.first;
interaction.layer = selected->layer.second;
interaction.element = selected->element;
@@ -503,7 +561,8 @@ namespace armarx
}
}
- void ArVizWidgetController::onInteractiveElementSelected(QListWidgetItem* item)
+ void
+ ArVizWidgetController::onInteractiveElementSelected(QListWidgetItem* item)
{
int index = widget.listInteractiveElements->row(item);
if (index < 0)
@@ -514,15 +573,15 @@ namespace armarx
visualizer.selectElement(index);
}
- static QString toQString(viz::ElementInteractionData const& inter)
+ static QString
+ toQString(viz::ElementInteractionData const& inter)
{
- std::string id = inter.layer.first + "/"
- + inter.layer.second + "/"
- + inter.element;
+ std::string id = inter.layer.first + "/" + inter.layer.second + "/" + inter.element;
return QString::fromStdString(id);
}
- void ArVizWidgetController::onUpdate()
+ void
+ ArVizWidgetController::onUpdate()
{
visualizer.update();
@@ -546,7 +605,7 @@ namespace armarx
{
// Remove all items
QLayoutItem* item;
- while ((item = contextMenuLayout->takeAt( 0 )) != nullptr)
+ while ((item = contextMenuLayout->takeAt(0)) != nullptr)
{
delete item->widget();
delete item;
@@ -554,10 +613,12 @@ namespace armarx
for (std::string const& option : options)
{
- QPushButton* button = new QPushButton(
- QString::fromStdString(option),
- widget.groupBoxContextMenu);
- connect(button, &QPushButton::clicked, this, &ArVizWidgetController::onContextMenuClicked);
+ QPushButton* button =
+ new QPushButton(QString::fromStdString(option), widget.groupBoxContextMenu);
+ connect(button,
+ &QPushButton::clicked,
+ this,
+ &ArVizWidgetController::onContextMenuClicked);
contextMenuLayout->addWidget(button);
}
}
@@ -574,7 +635,7 @@ namespace armarx
else
{
QLayoutItem* item;
- while ((item = contextMenuLayout->takeAt( 0 )) != nullptr)
+ while ((item = contextMenuLayout->takeAt(0)) != nullptr)
{
delete item->widget();
delete item;
@@ -598,7 +659,8 @@ namespace armarx
onTimingObserverUpdate();
}
- void ArVizWidgetController::onTimingObserverUpdate()
+ void
+ ArVizWidgetController::onTimingObserverUpdate()
{
viz::CoinVisualizer_UpdateTiming timing = visualizer.getTiming();
//if (timing.counter > lastTiming.counter)
@@ -606,12 +668,18 @@ namespace armarx
if (debugObserver)
{
timingMap["0. pull (ms)"] = new Variant(timing.pull.toMilliSecondsDouble());
- timingMap["1. apply (ms)"] = new Variant(timing.applyTotal.total.toMilliSecondsDouble());
- timingMap["1.1 apply, addLayer (ms)"] = new Variant(timing.applyTotal.addLayer.toMilliSecondsDouble());
- timingMap["1.2 apply, updateElements (ms)"] = new Variant(timing.applyTotal.updateElements.toMilliSecondsDouble());
- timingMap["1.3 apply, removeElements (ms)"] = new Variant(timing.applyTotal.removeElements.toMilliSecondsDouble());
- timingMap["2. layers (ms)"] = new Variant(timing.layersChanged.toMilliSecondsDouble());
- timingMap["3. wait duration (ms)"] = new Variant(timing.waitDuration.toMilliSecondsDouble());
+ timingMap["1. apply (ms)"] =
+ new Variant(timing.applyTotal.total.toMilliSecondsDouble());
+ timingMap["1.1 apply, addLayer (ms)"] =
+ new Variant(timing.applyTotal.addLayer.toMilliSecondsDouble());
+ timingMap["1.2 apply, updateElements (ms)"] =
+ new Variant(timing.applyTotal.updateElements.toMilliSecondsDouble());
+ timingMap["1.3 apply, removeElements (ms)"] =
+ new Variant(timing.applyTotal.removeElements.toMilliSecondsDouble());
+ timingMap["2. layers (ms)"] =
+ new Variant(timing.layersChanged.toMilliSecondsDouble());
+ timingMap["3. wait duration (ms)"] =
+ new Variant(timing.waitDuration.toMilliSecondsDouble());
timingMap["4. update toggle"] = new Variant(timing.updateToggle);
timingMap["total (ms)"] = new Variant(timing.total.toMilliSecondsDouble());
@@ -621,7 +689,8 @@ namespace armarx
{
timings.erase(timings.begin());
}
- double averageTime = std::accumulate(timings.begin(), timings.end(), 0.0) / numTimings;
+ double averageTime =
+ std::accumulate(timings.begin(), timings.end(), 0.0) / numTimings;
timingMap["total avg (ms)"] = new Variant(averageTime);
debugObserver->begin_setDebugChannel("ArViz_Timing", timingMap);
@@ -629,7 +698,8 @@ namespace armarx
}
}
- void ArVizWidgetController::onStartRecording()
+ void
+ ArVizWidgetController::onStartRecording()
{
std::string recordingID = widget.recordingIdTextBox->text().toStdString();
@@ -639,20 +709,18 @@ namespace armarx
if (runningID.size() > 0)
{
- std::string message = "There is already a recording running with ID '"
- + runningID + "'. \n"
- + "You have to stop the running recording first";
- QMessageBox::information(widget.tabWidget,
- "Recording running",
- QString::fromStdString(message));
-
+ std::string message = "There is already a recording running with ID '" + runningID +
+ "'. \n" + "You have to stop the running recording first";
+ QMessageBox::information(
+ widget.tabWidget, "Recording running", QString::fromStdString(message));
return;
}
}
- void ArVizWidgetController::onStopRecording()
+ void
+ ArVizWidgetController::onStopRecording()
{
storage->stopRecording();
@@ -660,14 +728,14 @@ namespace armarx
changeMode(ArVizWidgetMode::Live);
}
- void ArVizWidgetController::onRefreshRecordings()
+ void
+ ArVizWidgetController::onRefreshRecordings()
{
allRecordings = storage->getAllRecordings().recordings;
- std::sort(allRecordings.begin(), allRecordings.end(),
- [](viz::data::Recording const & lhs, viz::data::Recording const & rhs)
- {
- return lhs.id < rhs.id;
- });
+ std::sort(allRecordings.begin(),
+ allRecordings.end(),
+ [](viz::data::Recording const& lhs, viz::data::Recording const& rhs)
+ { return lhs.id < rhs.id; });
std::string currentText;
QListWidgetItem* currentItem = widget.recordingList->currentItem();
@@ -695,7 +763,9 @@ namespace armarx
}
}
- void ArVizWidgetController::onRecordingSelectionChanged(QListWidgetItem* current, QListWidgetItem* previous)
+ void
+ ArVizWidgetController::onRecordingSelectionChanged(QListWidgetItem* current,
+ QListWidgetItem* previous)
{
if (!current)
{
@@ -713,7 +783,8 @@ namespace armarx
}
}
- static std::string timestampToString(long timestampInMicroSeconds, bool showMS = false)
+ static std::string
+ timestampToString(long timestampInMicroSeconds, bool showMS = false)
{
IceUtil::Time time = IceUtil::Time::microSeconds(timestampInMicroSeconds);
std::string timeString = time.toDateTime();
@@ -724,12 +795,14 @@ namespace armarx
return timeString;
}
- void ArVizWidgetController::onReplaySpinChanged(int newValue)
+ void
+ ArVizWidgetController::onReplaySpinChanged(int newValue)
{
widget.replayRevisionSlider->setValue(newValue);
}
- void ArVizWidgetController::onReplaySliderChanged(int newValue)
+ void
+ ArVizWidgetController::onReplaySliderChanged(int newValue)
{
if (currentRevision != newValue)
{
@@ -741,9 +814,8 @@ namespace armarx
widget.replayRevisionSpinBox->setValue(newValue);
- widget.replayTimestampLabel->setText(QString::fromStdString(
- timestampToString(timestamp, true)
- ));
+ widget.replayTimestampLabel->setText(
+ QString::fromStdString(timestampToString(timestamp, true)));
}
else
{
@@ -752,34 +824,30 @@ namespace armarx
}
}
- void ArVizWidgetController::selectRecording(const viz::data::Recording& recording)
+ void
+ ArVizWidgetController::selectRecording(const viz::data::Recording& recording)
{
// Update recording description
widget.recordingIdLabel->setText(QString::fromStdString(recording.id));
- widget.recordingRevisionLabel->setText(QString::fromStdString(
- std::to_string(recording.firstRevision) +
- " - " +
- std::to_string(recording.lastRevision)));
+ widget.recordingRevisionLabel->setText(
+ QString::fromStdString(std::to_string(recording.firstRevision) + " - " +
+ std::to_string(recording.lastRevision)));
std::string firstTimeString = timestampToString(recording.firstTimestampInMicroSeconds);
std::string lastTimeString = timestampToString(recording.lastTimestampInMicroSeconds);
- widget.recordingTimestampLabel->setText(QString::fromStdString(
- firstTimeString +
- " - " +
- lastTimeString));
+ widget.recordingTimestampLabel->setText(
+ QString::fromStdString(firstTimeString + " - " + lastTimeString));
IceUtil::Time duration = IceUtil::Time::microSeconds(
- recording.lastTimestampInMicroSeconds -
- recording.firstTimestampInMicroSeconds);
+ recording.lastTimestampInMicroSeconds - recording.firstTimestampInMicroSeconds);
int durationSeconds = duration.toSeconds();
- widget.recordingDurationLabel->setText(QString::fromStdString(
- std::to_string(durationSeconds) + " s"));
+ widget.recordingDurationLabel->setText(
+ QString::fromStdString(std::to_string(durationSeconds) + " s"));
- widget.recordingBatchesLabel->setText(QString::fromStdString(
- std::to_string(recording.batchHeaders.size())
- ));
+ widget.recordingBatchesLabel->setText(
+ QString::fromStdString(std::to_string(recording.batchHeaders.size())));
// Update replay control
currentRecording = recording;
@@ -787,7 +855,8 @@ namespace armarx
enableWidgetAccordingToMode();
}
- void ArVizWidgetController::onReplayStart(bool)
+ void
+ ArVizWidgetController::onReplayStart(bool)
{
if (!currentRecordingSelected)
{
@@ -819,27 +888,28 @@ namespace armarx
onReplaySliderChanged(widget.replayRevisionSlider->value());
}
- void ArVizWidgetController::onReplayStop(bool)
+ void
+ ArVizWidgetController::onReplayStop(bool)
{
visualizer.startAsync(storage);
changeMode(ArVizWidgetMode::Live);
}
- long ArVizWidgetController::replayToRevision(long revision)
+ long
+ ArVizWidgetController::replayToRevision(long revision)
{
if (mode != ArVizWidgetMode::ReplayingManual && mode != ArVizWidgetMode::ReplayingTimed)
{
ARMARX_WARNING << "Cannot call replayToRevision, when not in replaying mode.\n"
- << "Actual mode: " << int(mode);
+ << "Actual mode: " << int(mode);
return -1;
}
viz::data::RecordingBatchHeader* matchingBatchHeader = nullptr;
for (auto& batchHeader : currentRecording.batchHeaders)
{
- if (batchHeader.firstRevision <= revision &&
- revision <= batchHeader.lastRevision)
+ if (batchHeader.firstRevision <= revision && revision <= batchHeader.lastRevision)
{
matchingBatchHeader = &batchHeader;
break;
@@ -854,18 +924,18 @@ namespace armarx
viz::data::RecordingBatch const& batch = getRecordingBatch(matchingBatchHeader->index);
ARMARX_VERBOSE << "Replaying to revision : " << revision
- << "\nGot batch: " << batch.header.firstRevision << " - " << batch.header.lastRevision
- << "\nUpdates: " << batch.updates.size()
+ << "\nGot batch: " << batch.header.firstRevision << " - "
+ << batch.header.lastRevision << "\nUpdates: " << batch.updates.size()
<< "\nInitial state: " << batch.initialState.size();
- auto revisionLess = [](viz::data::TimestampedLayerUpdate const & lhs, viz::data::TimestampedLayerUpdate const & rhs)
- {
- return lhs.revision < rhs.revision;
- };
+ auto revisionLess = [](viz::data::TimestampedLayerUpdate const& lhs,
+ viz::data::TimestampedLayerUpdate const& rhs)
+ { return lhs.revision < rhs.revision; };
viz::data::TimestampedLayerUpdate pivot;
pivot.revision = revision;
- auto updateBegin = std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, revisionLess);
+ auto updateBegin =
+ std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, revisionLess);
auto updateEnd = std::upper_bound(updateBegin, batch.updates.end(), pivot, revisionLess);
// TODO: Optimize: Only start from the last update position
@@ -894,18 +964,20 @@ namespace armarx
return updateBegin->timestampInMicroseconds;
}
- long ArVizWidgetController::getRevisionForTimestamp(long timestamp)
+ long
+ ArVizWidgetController::getRevisionForTimestamp(long timestamp)
{
if (mode != ArVizWidgetMode::ReplayingManual && mode != ArVizWidgetMode::ReplayingTimed)
{
ARMARX_WARNING << "Cannot call replayToTimestamp, when not in replaying mode.\n"
- << "Actual mode: " << int(mode);
+ << "Actual mode: " << int(mode);
return -1;
}
if (timestamp < currentRecording.firstTimestampInMicroSeconds)
{
- ARMARX_INFO << "Requested timestamp is earlier than recording: " << timestampToString(timestamp);
+ ARMARX_INFO << "Requested timestamp is earlier than recording: "
+ << timestampToString(timestamp);
return -1;
}
@@ -921,13 +993,13 @@ namespace armarx
viz::data::RecordingBatch const& batch = getRecordingBatch(matchingBatchHeader->index);
- auto timestampLess = [](viz::data::TimestampedLayerUpdate const & lhs, viz::data::TimestampedLayerUpdate const & rhs)
- {
- return lhs.timestampInMicroseconds < rhs.timestampInMicroseconds;
- };
+ auto timestampLess = [](viz::data::TimestampedLayerUpdate const& lhs,
+ viz::data::TimestampedLayerUpdate const& rhs)
+ { return lhs.timestampInMicroseconds < rhs.timestampInMicroseconds; };
viz::data::TimestampedLayerUpdate pivot;
pivot.timestampInMicroseconds = timestamp;
- auto updateEnd = std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, timestampLess);
+ auto updateEnd =
+ std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, timestampLess);
if (updateEnd == batch.updates.end())
{
return -2;
@@ -943,7 +1015,8 @@ namespace armarx
return revisionBeforeTimestamp;
}
- void ArVizWidgetController::onReplayTimedStart(bool checked)
+ void
+ ArVizWidgetController::onReplayTimedStart(bool checked)
{
if (checked)
{
@@ -957,7 +1030,8 @@ namespace armarx
}
}
- void ArVizWidgetController::onReplayTimerTick()
+ void
+ ArVizWidgetController::onReplayTimerTick()
{
if (mode == ArVizWidgetMode::ReplayingTimed)
{
@@ -989,14 +1063,16 @@ namespace armarx
lastReplayRealTime = IceUtil::Time::now().toMicroSeconds();
}
- void ArVizWidgetController::changeMode(ArVizWidgetMode newMode)
+ void
+ ArVizWidgetController::changeMode(ArVizWidgetMode newMode)
{
mode = newMode;
enableWidgetAccordingToMode();
}
- void ArVizWidgetController::enableWidgetAccordingToMode()
+ void
+ ArVizWidgetController::enableWidgetAccordingToMode()
{
switch (mode)
{
@@ -1061,7 +1137,8 @@ namespace armarx
}
}
- void ArVizWidgetController::onGetBatchAsync(const viz::data::RecordingBatch& batch)
+ void
+ ArVizWidgetController::onGetBatchAsync(const viz::data::RecordingBatch& batch)
{
// We received a batch asynchronously ==> Update the cache
ARMARX_INFO << "Received async batch: " << batch.header.index;
@@ -1076,7 +1153,8 @@ namespace armarx
recordingWaitingForBatchIndex = -1;
}
- viz::data::RecordingBatch const& ArVizWidgetController::getRecordingBatch(long index)
+ viz::data::RecordingBatch const&
+ ArVizWidgetController::getRecordingBatch(long index)
{
ARMARX_TRACE;
@@ -1091,12 +1169,13 @@ namespace armarx
bool asyncPrefetchIsRunning = callbackResult && !callbackResult->isCompleted();
if (!asyncPrefetchIsRunning && recordingWaitingForBatchIndex == -1)
{
- if (index + 1 < long(currentRecording.batchHeaders.size())
- && recordingBatchCache.count(index + 1) == 0)
+ if (index + 1 < long(currentRecording.batchHeaders.size()) &&
+ recordingBatchCache.count(index + 1) == 0)
{
// ARMARX_WARNING << "after begin_getRecordingBatch: " << (index + 1)
// << " waiting for " << recordingWaitingForBatchIndex;
- callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index + 1, callback);
+ callbackResult =
+ storage->begin_getRecordingBatch(currentRecording.id, index + 1, callback);
recordingWaitingForBatchIndex = index + 1;
ARMARX_INFO << "Now waiting for " << recordingWaitingForBatchIndex;
}
@@ -1104,10 +1183,10 @@ namespace armarx
{
// ARMARX_WARNING << "before begin_getRecordingBatch: " << (index - 1)
// << " waiting for " << recordingWaitingForBatchIndex;
- callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index - 1, callback);
+ callbackResult =
+ storage->begin_getRecordingBatch(currentRecording.id, index - 1, callback);
recordingWaitingForBatchIndex = index - 1;
}
-
}
TimestampedRecordingBatch& entry = iter->second;
@@ -1128,7 +1207,8 @@ namespace armarx
return getRecordingBatch(index);
}
- ARMARX_INFO << "Batch #" << index << " is not in the cache. Getting synchronously, blocking GUI...";
+ ARMARX_INFO << "Batch #" << index
+ << " is not in the cache. Getting synchronously, blocking GUI...";
// Entry is not in the cache, we have to get it from ArVizStorage
auto& newEntry = recordingBatchCache[index];
@@ -1140,14 +1220,14 @@ namespace armarx
return newEntry.data;
}
- void ArVizWidgetController::limitRecordingBatchCacheSize()
+ void
+ ArVizWidgetController::limitRecordingBatchCacheSize()
{
if (recordingBatchCache.size() > recordingBatchCacheMaxSize)
{
// Remove the entry with the oldest last used timestamp
auto oldestIter = recordingBatchCache.begin();
- for (auto iter = recordingBatchCache.begin();
- iter != recordingBatchCache.end(); ++iter)
+ for (auto iter = recordingBatchCache.begin(); iter != recordingBatchCache.end(); ++iter)
{
TimestampedRecordingBatch& entry = iter->second;
if (entry.lastUsed < oldestIter->second.lastUsed)
@@ -1160,7 +1240,8 @@ namespace armarx
}
}
- SoNode* ArVizWidgetController::getScene()
+ SoNode*
+ ArVizWidgetController::getScene()
{
return visualizer.root;
}
@@ -1168,15 +1249,20 @@ namespace armarx
static const std::string CONFIG_KEY_STORAGE = "Storage";
static const std::string CONFIG_KEY_DEBUG_OBSERVER = "DebugObserver";
- void ArVizWidgetController::loadSettings(QSettings* settings)
+ void
+ ArVizWidgetController::loadSettings(QSettings* settings)
{
- storageName = settings->value(QString::fromStdString(CONFIG_KEY_STORAGE),
- "ArVizStorage").toString().toStdString();
- debugObserverName = settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER),
- "DebugObserver").toString().toStdString();
+ storageName = settings->value(QString::fromStdString(CONFIG_KEY_STORAGE), "ArVizStorage")
+ .toString()
+ .toStdString();
+ debugObserverName =
+ settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), "DebugObserver")
+ .toString()
+ .toStdString();
}
- void ArVizWidgetController::saveSettings(QSettings* settings)
+ void
+ ArVizWidgetController::saveSettings(QSettings* settings)
{
settings->setValue(QString::fromStdString(CONFIG_KEY_STORAGE),
QString::fromStdString(storageName));
@@ -1184,18 +1270,22 @@ namespace armarx
QString::fromStdString(debugObserverName));
}
- QPointer<QDialog> ArVizWidgetController::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ ArVizWidgetController::getConfigDialog(QWidget* parent)
{
if (!configDialog)
{
configDialog = new SimpleConfigDialog(parent);
- configDialog->addProxyFinder<armarx::viz::StorageInterfacePrx>({CONFIG_KEY_STORAGE, "ArViz Storage", "ArViz*"});
- configDialog->addProxyFinder<armarx::DebugObserverInterfacePrx>({CONFIG_KEY_DEBUG_OBSERVER, "Debug observer", "DebugObserver"});
+ configDialog->addProxyFinder<armarx::viz::StorageInterfacePrx>(
+ {CONFIG_KEY_STORAGE, "ArViz Storage", "ArViz*"});
+ configDialog->addProxyFinder<armarx::DebugObserverInterfacePrx>(
+ {CONFIG_KEY_DEBUG_OBSERVER, "Debug observer", "DebugObserver"});
}
return qobject_cast<QDialog*>(configDialog);
}
- void ArVizWidgetController::configured()
+ void
+ ArVizWidgetController::configured()
{
if (configDialog)
{
@@ -1204,11 +1294,12 @@ namespace armarx
}
}
-
- void ArVizWidgetController::exportToVRML()
+ void
+ ArVizWidgetController::exportToVRML()
{
- QString fi = QFileDialog::getSaveFileName(Q_NULLPTR, tr("VRML 2.0 File"), QString(), tr("VRML Files (*.wrl)"));
+ QString fi = QFileDialog::getSaveFileName(
+ Q_NULLPTR, tr("VRML 2.0 File"), QString(), tr("VRML Files (*.wrl)"));
std::string s = std::string(fi.toLatin1());
if (s.empty())
@@ -1222,4 +1313,4 @@ namespace armarx
visualizer.exportToVRML(s);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index 065d04253da98d4c84c9d6f96313b094cfd95237..2fd4a5b85556cf6a403a88ff919269614c8cba40 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -23,21 +23,19 @@
#include <unordered_set>
-#include <RobotAPI/gui-plugins/ArViz/ui_ArVizWidget.h>
-
-#include <RobotAPI/components/ArViz/Coin/Visualizer.h>
-#include <RobotAPI/interface/ArViz/Component.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <RobotAPI/components/ArViz/Coin/Visualizer.h>
+#include <RobotAPI/gui-plugins/ArViz/ui_ArVizWidget.h>
+#include <RobotAPI/interface/ArViz/Component.h>
-#include "LayerInfoTree.h"
#include "ArvizProfileManagerWidget.h"
-
+#include "LayerInfoTree.h"
namespace armarx
{
@@ -52,7 +50,6 @@ namespace armarx
struct ArVizWidgetBatchCallback;
-
/**
\page RobotAPI-GuiPlugins-ArViz ArViz
\brief The ArViz allows visualizing ...
@@ -71,14 +68,12 @@ namespace armarx
*
* Detailed description
*/
- struct ARMARXCOMPONENT_IMPORT_EXPORT
- ArVizWidgetController
- : armarx::ArmarXComponentWidgetControllerTemplate < ArVizWidgetController >
+ struct ARMARXCOMPONENT_IMPORT_EXPORT ArVizWidgetController :
+ armarx::ArmarXComponentWidgetControllerTemplate<ArVizWidgetController>
{
Q_OBJECT
public:
-
/// Controller Constructor
explicit ArVizWidgetController();
@@ -94,7 +89,8 @@ namespace armarx
SoNode* getScene() override;
/// Returns the Widget name displayed in the ArmarXGui to create an instance of this class.
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Visualization.ArViz";
}
@@ -122,7 +118,6 @@ namespace armarx
private:
-
void onConnectGui();
void onDisconnectGui();
@@ -177,7 +172,6 @@ namespace armarx
private:
-
Ui::ArVizWidget widget;
QPointer<SimpleConfigDialog> configDialog;
@@ -232,11 +226,10 @@ namespace armarx
ArvizProfileManagerWidget* arvizeProfileLayerWidget;
public:
- static QIcon GetWidgetIcon()
+ static QIcon
+ GetWidgetIcon()
{
return QIcon(":icons/ArViz.png");
}
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.cpp b/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.cpp
index 4af5a0fd4db95eb36733d5bcb501d1453bdfc5b6..6b32e9688be5480b5833f351cec40707ed48f5de 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.cpp
@@ -2,361 +2,392 @@
#include <string>
-#include <QVBoxLayout>
-#include <QMessageBox>
#include <QBoxLayout>
-#include <QLabel>
-#include <QFormLayout>
#include <QDialogButtonBox>
+#include <QFormLayout>
+#include <QLabel>
+#include <QMessageBox>
#include <QPushButton>
-#include <QBoxLayout>
#include <QRadioButton>
+#include <QVBoxLayout>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
namespace armarx
{
-const QString ArvizProfileManagerWidget::SETTINGS_DEFAULT_PROFILE_KEY = "defaultProfileName";
-
-
-ProfileDialog::ProfileDialog(QWidget *parent, const QString &name, bool additive, bool addDialog, bool isDefault)
- : QDialog(parent)
-{
- setWindowTitle(addDialog ? "Add ArViz Profile" : "Edit ArViz Profile");
-
- // Set up UI components
- nameInput = new QLineEdit(this);
- nameInput->setText(name);
-
- additiveRadioButton = new QRadioButton("Save added layers", this);
- additiveRadioButton->setChecked(additive);
- substractiveRadioButton = new QRadioButton("Save removed layers", this);
- substractiveRadioButton->setChecked(not additive);
-
- defaultCheckBox = new QCheckBox("Mark default", this);
- defaultCheckBox->setChecked(isDefault);
-
- // Save/Cancel buttons
- QDialogButtonBox *buttonBox = new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel, this);
- connect(buttonBox, &QDialogButtonBox::accepted, this, &ProfileDialog::accept);
- connect(buttonBox, &QDialogButtonBox::rejected, this, &ProfileDialog::reject);
- buttonBox->setCenterButtons(true);
-
- // Delete button
- deleteButton = new QPushButton("Delete", this);
- connect(deleteButton, &QPushButton::clicked, this, &ProfileDialog::deleteProfile);
-
- // Layouts
- QFormLayout *formLayout = new QFormLayout;
- formLayout->addRow("Profile Name:", nameInput);
- formLayout->addRow(additiveRadioButton);
- formLayout->addRow(substractiveRadioButton);
- formLayout->addRow(defaultCheckBox);
- formLayout->addRow(buttonBox);
- formLayout->addRow(deleteButton);
-
- setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum);
- setLayout(formLayout);
- adjustSize();
-}
-
-ProfileDialog::~ProfileDialog()
-{
-}
-
-QString ProfileDialog::getName() const
-{
- return nameInput->text();
-}
-
-bool ProfileDialog::isAdditive() const
-{
- return additiveRadioButton->isChecked();
-}
-
-bool ProfileDialog::isDefaultProfile() const
-{
- return defaultCheckBox->isChecked();
-}
-
-void ProfileDialog::deleteProfile()
-{
- // Handle the profile deletion
- emit deleteProfileSignal();
- reject(); // Close the dialog after deletion
-}
-
-
-
-ArvizProfileManagerWidget::ArvizProfileManagerWidget(QWidget* parent) :
- QWidget(parent),
- settings(QString((armarx::ArmarXDataPath::GetDefaultUserConfigPath() + "/ArvizProfileManager.conf").c_str()),
- QSettings::NativeFormat),
- currentProfile(Profile())
-{
- QHBoxLayout* layout = new QHBoxLayout(this);
-
- // Setup UI elements
- layersComboBox = new QComboBox(this);
- connect(layersComboBox, QOverload<int>::of(&QComboBox::activated),
- this, &ArvizProfileManagerWidget::onProfileSelected);
-
- addButton = new QPushButton("Add", this);
- editButton = new QPushButton("Edit", this);
- saveButton = new QPushButton("Save", this);
-
- layout->addWidget(new QLabel("ArViz Profile: "));
- layout->addWidget(layersComboBox);
- layout->addWidget(addButton);
- layout->addWidget(editButton);
- layout->addWidget(saveButton);
-
- setLayout(layout);
+ const QString ArvizProfileManagerWidget::SETTINGS_DEFAULT_PROFILE_KEY = "defaultProfileName";
- connect(addButton, &QPushButton::clicked, this, &ArvizProfileManagerWidget::addProfile);
- connect(editButton, &QPushButton::clicked, this, &ArvizProfileManagerWidget::editProfile);
- connect(saveButton, &QPushButton::clicked, this, &ArvizProfileManagerWidget::saveCurrentProfile);
+ ProfileDialog::ProfileDialog(QWidget* parent,
+ const QString& name,
+ bool additive,
+ bool addDialog,
+ bool isDefault) :
+ QDialog(parent)
+ {
+ setWindowTitle(addDialog ? "Add ArViz Profile" : "Edit ArViz Profile");
+
+ // Set up UI components
+ nameInput = new QLineEdit(this);
+ nameInput->setText(name);
+
+ additiveRadioButton = new QRadioButton("Save added layers", this);
+ additiveRadioButton->setChecked(additive);
+ substractiveRadioButton = new QRadioButton("Save removed layers", this);
+ substractiveRadioButton->setChecked(not additive);
+
+ defaultCheckBox = new QCheckBox("Mark default", this);
+ defaultCheckBox->setChecked(isDefault);
+
+ // Save/Cancel buttons
+ QDialogButtonBox* buttonBox =
+ new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel, this);
+ connect(buttonBox, &QDialogButtonBox::accepted, this, &ProfileDialog::accept);
+ connect(buttonBox, &QDialogButtonBox::rejected, this, &ProfileDialog::reject);
+ buttonBox->setCenterButtons(true);
+
+ // Delete button
+ deleteButton = new QPushButton("Delete", this);
+ connect(deleteButton, &QPushButton::clicked, this, &ProfileDialog::deleteProfile);
+
+ // Layouts
+ QFormLayout* formLayout = new QFormLayout;
+ formLayout->addRow("Profile Name:", nameInput);
+ formLayout->addRow(additiveRadioButton);
+ formLayout->addRow(substractiveRadioButton);
+ formLayout->addRow(defaultCheckBox);
+ formLayout->addRow(buttonBox);
+ formLayout->addRow(deleteButton);
+
+ setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum);
+ setLayout(formLayout);
+ adjustSize();
+ }
- loadLayerNames();
+ ProfileDialog::~ProfileDialog()
+ {
+ }
- // Load default profile
- const QString defaultProfileName
- = settings.value(SETTINGS_DEFAULT_PROFILE_KEY).toString();
- if (int i = layersComboBox->findText(defaultProfileName); i >= 0)
+ QString
+ ProfileDialog::getName() const
{
- layersComboBox->setCurrentText(defaultProfileName);
- onProfileSelected(i);
+ return nameInput->text();
}
- else
+
+ bool
+ ProfileDialog::isAdditive() const
{
- // Not found. Load empty profile
- layersComboBox->setCurrentIndex(-1);
+ return additiveRadioButton->isChecked();
}
-}
-void ArvizProfileManagerWidget::onProfileSelected(int index)
-{
- if (index >= 0 and index < layersComboBox->count())
+ bool
+ ProfileDialog::isDefaultProfile() const
{
- loadProfile(layersComboBox->itemText(index));
- emit publishUpdate();
+ return defaultCheckBox->isChecked();
}
-}
-void ArvizProfileManagerWidget::saveCurrentProfile()
-{
- const QString name = layersComboBox->currentText();
- if (name.isEmpty())
+ void
+ ProfileDialog::deleteProfile()
{
- QMessageBox::warning(this, "Warning", "Please enter a valid name.");
- return;
+ // Handle the profile deletion
+ emit deleteProfileSignal();
+ reject(); // Close the dialog after deletion
}
- if (saveProfile(name, currentProfile))
+ ArvizProfileManagerWidget::ArvizProfileManagerWidget(QWidget* parent) :
+ QWidget(parent),
+ settings(QString((armarx::ArmarXDataPath::GetDefaultUserConfigPath() +
+ "/ArvizProfileManager.conf")
+ .c_str()),
+ QSettings::NativeFormat),
+ currentProfile(Profile())
{
- if (layersComboBox->findText(name) == -1)
+ QHBoxLayout* layout = new QHBoxLayout(this);
+
+ // Setup UI elements
+ layersComboBox = new QComboBox(this);
+ connect(layersComboBox,
+ QOverload<int>::of(&QComboBox::activated),
+ this,
+ &ArvizProfileManagerWidget::onProfileSelected);
+
+ addButton = new QPushButton("Add", this);
+ editButton = new QPushButton("Edit", this);
+ saveButton = new QPushButton("Save", this);
+
+ layout->addWidget(new QLabel("ArViz Profile: "));
+ layout->addWidget(layersComboBox);
+ layout->addWidget(addButton);
+ layout->addWidget(editButton);
+ layout->addWidget(saveButton);
+
+ setLayout(layout);
+
+ connect(addButton, &QPushButton::clicked, this, &ArvizProfileManagerWidget::addProfile);
+ connect(editButton, &QPushButton::clicked, this, &ArvizProfileManagerWidget::editProfile);
+ connect(saveButton,
+ &QPushButton::clicked,
+ this,
+ &ArvizProfileManagerWidget::saveCurrentProfile);
+
+ loadLayerNames();
+
+ // Load default profile
+ const QString defaultProfileName = settings.value(SETTINGS_DEFAULT_PROFILE_KEY).toString();
+ if (int i = layersComboBox->findText(defaultProfileName); i >= 0)
{
- layersComboBox->addItem(name);
+ layersComboBox->setCurrentText(defaultProfileName);
+ onProfileSelected(i);
+ }
+ else
+ {
+ // Not found. Load empty profile
+ layersComboBox->setCurrentIndex(-1);
}
- layersComboBox->setCurrentText(name);
}
-}
-
-void ArvizProfileManagerWidget::deleteCurrentProfileSave()
-{
- const QString name = layersComboBox->currentText();
- settings.remove(name);
- const int index = layersComboBox->currentIndex();
- layersComboBox->removeItem(index);
- settings.remove(name);
-
- if (layersComboBox->count() == 0)
+ void
+ ArvizProfileManagerWidget::onProfileSelected(int index)
{
- editButton->setDisabled(true);
- saveButton->setDisabled(true);
+ if (index >= 0 and index < layersComboBox->count())
+ {
+ loadProfile(layersComboBox->itemText(index));
+ emit publishUpdate();
+ }
}
- emit publishUpdate();
-}
-
-void ArvizProfileManagerWidget::addProfile()
-{
- if (dialog == nullptr || !dialog->isVisible())
+ void
+ ArvizProfileManagerWidget::saveCurrentProfile()
{
- const QString defaultName = "Profile";
- const bool defaultAdditive = false;
- QString name = defaultName;
- unsigned int counter = 2;
- while (layersComboBox->count() > 0 and layersComboBox->findText(name) >= 0)
+ const QString name = layersComboBox->currentText();
+ if (name.isEmpty())
{
- name = defaultName + QString::fromStdString(std::to_string(counter++));
+ QMessageBox::warning(this, "Warning", "Please enter a valid name.");
+ return;
}
- dialog = new ProfileDialog(this, name, defaultAdditive, true,
- settings.value(SETTINGS_DEFAULT_PROFILE_KEY) == name);
- const int result = dialog->exec();
- if (result == QDialog::Accepted)
+ if (saveProfile(name, currentProfile))
{
- if (dialog->getName().isEmpty())
- {
- ARMARX_WARNING << "Empty name for profile set.";
- return;
- }
- if (layersComboBox->findText(dialog->getName()) >= 0)
+ if (layersComboBox->findText(name) == -1)
{
- ARMARX_WARNING << "Profile with name " << dialog->getName().toStdString() << " already exists!";
- return;
+ layersComboBox->addItem(name);
}
+ layersComboBox->setCurrentText(name);
+ }
+ }
- if (dialog->isDefaultProfile())
- {
- settings.setValue(SETTINGS_DEFAULT_PROFILE_KEY, dialog->getName());
- }
+ void
+ ArvizProfileManagerWidget::deleteCurrentProfileSave()
+ {
+ const QString name = layersComboBox->currentText();
+ settings.remove(name);
- currentProfile.layers = {};
- currentProfile.additive = dialog->isAdditive();
- emit fetchUpdate();
+ const int index = layersComboBox->currentIndex();
+ layersComboBox->removeItem(index);
+ settings.remove(name);
- saveProfile(dialog->getName(), currentProfile);
- loadLayerNames();
- layersComboBox->setCurrentText(dialog->getName());
- }
- else
+ if (layersComboBox->count() == 0)
{
- // do nothing
+ editButton->setDisabled(true);
+ saveButton->setDisabled(true);
}
- }
- else
- {
- QMessageBox::information(this, "Dialog Already Open", "The dialog is already open.");
+
+ emit publishUpdate();
}
-}
-void ArvizProfileManagerWidget::editProfile()
-{
- if (layersComboBox->count() > 0)
+ void
+ ArvizProfileManagerWidget::addProfile()
{
- if (dialog == nullptr || !dialog->isVisible())
+ if (dialog == nullptr || !dialog->isVisible())
{
- const auto name = layersComboBox->currentText();
- dialog = new ProfileDialog(this, name, currentProfile.additive,
- false, settings.value(SETTINGS_DEFAULT_PROFILE_KEY) == name);
- connect(dialog, &ProfileDialog::deleteProfileSignal, this, &ArvizProfileManagerWidget::deleteCurrentProfileSave);
+ const QString defaultName = "Profile";
+ const bool defaultAdditive = false;
+ QString name = defaultName;
+ unsigned int counter = 2;
+ while (layersComboBox->count() > 0 and layersComboBox->findText(name) >= 0)
+ {
+ name = defaultName + QString::fromStdString(std::to_string(counter++));
+ }
+ dialog = new ProfileDialog(this,
+ name,
+ defaultAdditive,
+ true,
+ settings.value(SETTINGS_DEFAULT_PROFILE_KEY) == name);
const int result = dialog->exec();
- if (result == QDialog::Accepted)
+ if (result == QDialog::Accepted)
{
- const auto newName = dialog->getName();
-
- if (newName.isEmpty())
+ if (dialog->getName().isEmpty())
{
ARMARX_WARNING << "Empty name for profile set.";
return;
}
-
- if (newName != name)
- {
- // Rename
- if (layersComboBox->findText(newName) >= 0)
- {
- ARMARX_WARNING << "Profile with name " << newName.toStdString() << " already exists!";
- return;
- }
- settings.remove(layersComboBox->currentText());
- }
-
- if (dialog->isAdditive() != currentProfile.additive)
+ if (layersComboBox->findText(dialog->getName()) >= 0)
{
- currentProfile.layers.clear();
- currentProfile.additive = dialog->isAdditive();
- emit fetchUpdate();
+ ARMARX_WARNING << "Profile with name " << dialog->getName().toStdString()
+ << " already exists!";
+ return;
}
if (dialog->isDefaultProfile())
{
- settings.setValue(SETTINGS_DEFAULT_PROFILE_KEY, newName);
- }
- else if (settings.value(SETTINGS_DEFAULT_PROFILE_KEY) == name)
- {
- settings.setValue(SETTINGS_DEFAULT_PROFILE_KEY, "");
+ settings.setValue(SETTINGS_DEFAULT_PROFILE_KEY, dialog->getName());
}
-
+
+ currentProfile.layers = {};
+ currentProfile.additive = dialog->isAdditive();
+ emit fetchUpdate();
+
saveProfile(dialog->getName(), currentProfile);
loadLayerNames();
- layersComboBox->setCurrentText(newName);
- }
- else
+ layersComboBox->setCurrentText(dialog->getName());
+ }
+ else
{
// do nothing
}
- }
- else
+ }
+ else
{
QMessageBox::information(this, "Dialog Already Open", "The dialog is already open.");
}
}
-}
-void ArvizProfileManagerWidget::loadLayerNames()
-{
- layersComboBox->clear();
- for (const QString& groups : settings.childGroups())
+ void
+ ArvizProfileManagerWidget::editProfile()
{
- layersComboBox->addItem(groups);
- }
- if (layersComboBox->count() == 0)
- {
- editButton->setDisabled(true);
- saveButton->setDisabled(true);
+ if (layersComboBox->count() > 0)
+ {
+ if (dialog == nullptr || !dialog->isVisible())
+ {
+ const auto name = layersComboBox->currentText();
+ dialog = new ProfileDialog(this,
+ name,
+ currentProfile.additive,
+ false,
+ settings.value(SETTINGS_DEFAULT_PROFILE_KEY) == name);
+ connect(dialog,
+ &ProfileDialog::deleteProfileSignal,
+ this,
+ &ArvizProfileManagerWidget::deleteCurrentProfileSave);
+ const int result = dialog->exec();
+
+ if (result == QDialog::Accepted)
+ {
+ const auto newName = dialog->getName();
+
+ if (newName.isEmpty())
+ {
+ ARMARX_WARNING << "Empty name for profile set.";
+ return;
+ }
+
+ if (newName != name)
+ {
+ // Rename
+ if (layersComboBox->findText(newName) >= 0)
+ {
+ ARMARX_WARNING << "Profile with name " << newName.toStdString()
+ << " already exists!";
+ return;
+ }
+ settings.remove(layersComboBox->currentText());
+ }
+
+ if (dialog->isAdditive() != currentProfile.additive)
+ {
+ currentProfile.layers.clear();
+ currentProfile.additive = dialog->isAdditive();
+ emit fetchUpdate();
+ }
+
+ if (dialog->isDefaultProfile())
+ {
+ settings.setValue(SETTINGS_DEFAULT_PROFILE_KEY, newName);
+ }
+ else if (settings.value(SETTINGS_DEFAULT_PROFILE_KEY) == name)
+ {
+ settings.setValue(SETTINGS_DEFAULT_PROFILE_KEY, "");
+ }
+
+ saveProfile(dialog->getName(), currentProfile);
+ loadLayerNames();
+ layersComboBox->setCurrentText(newName);
+ }
+ else
+ {
+ // do nothing
+ }
+ }
+ else
+ {
+ QMessageBox::information(
+ this, "Dialog Already Open", "The dialog is already open.");
+ }
+ }
}
- else
+
+ void
+ ArvizProfileManagerWidget::loadLayerNames()
{
- editButton->setEnabled(true);
- saveButton->setEnabled(true);
+ layersComboBox->clear();
+ for (const QString& groups : settings.childGroups())
+ {
+ layersComboBox->addItem(groups);
+ }
+ if (layersComboBox->count() == 0)
+ {
+ editButton->setDisabled(true);
+ saveButton->setDisabled(true);
+ }
+ else
+ {
+ editButton->setEnabled(true);
+ saveButton->setEnabled(true);
+ }
}
-}
-void ArvizProfileManagerWidget::loadProfile(const QString& name)
-{
- ARMARX_INFO << "Loading ArViz profile " << name.toStdString();
-
- std::scoped_lock lock(mtx);
- currentProfile.layers.clear();
-
- settings.beginGroup(name);
- currentProfile.additive = settings.value("additive").toBool();
- const int size = settings.beginReadArray("layers");
- for (int i = 0; i < size; ++i)
+ void
+ ArvizProfileManagerWidget::loadProfile(const QString& name)
{
- settings.setArrayIndex(i);
- QString first = settings.value("component").toString();
- QString second = settings.value("layer").toString();
- currentProfile.layers.insert({first.toStdString(), second.toStdString()});
+ ARMARX_INFO << "Loading ArViz profile " << name.toStdString();
+
+ std::scoped_lock lock(mtx);
+ currentProfile.layers.clear();
+
+ settings.beginGroup(name);
+ currentProfile.additive = settings.value("additive").toBool();
+ const int size = settings.beginReadArray("layers");
+ for (int i = 0; i < size; ++i)
+ {
+ settings.setArrayIndex(i);
+ QString first = settings.value("component").toString();
+ QString second = settings.value("layer").toString();
+ currentProfile.layers.insert({first.toStdString(), second.toStdString()});
+ }
+ settings.endArray();
+ settings.endGroup();
+ editButton->setEnabled(true);
}
- settings.endArray();
- settings.endGroup();
- editButton->setEnabled(true);
-}
-bool ArvizProfileManagerWidget::saveProfile(const QString& name, const Profile& profile)
-{
- settings.beginGroup(name);
- settings.setValue("additive", profile.additive);
- settings.remove("layers");
- settings.beginWriteArray("layers", profile.layers.size());
- int i = 0;
- for (const auto& pair : profile.layers)
+ bool
+ ArvizProfileManagerWidget::saveProfile(const QString& name, const Profile& profile)
{
- settings.setArrayIndex(i++);
- settings.setValue("component", QString::fromStdString(pair.first));
- settings.setValue("layer", QString::fromStdString(pair.second));
+ settings.beginGroup(name);
+ settings.setValue("additive", profile.additive);
+ settings.remove("layers");
+ settings.beginWriteArray("layers", profile.layers.size());
+ int i = 0;
+ for (const auto& pair : profile.layers)
+ {
+ settings.setArrayIndex(i++);
+ settings.setValue("component", QString::fromStdString(pair.first));
+ settings.setValue("layer", QString::fromStdString(pair.second));
+ }
+ settings.endArray();
+ settings.endGroup();
+ return true;
}
- settings.endArray();
- settings.endGroup();
- return true;
-}
-}
\ No newline at end of file
+} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.h b/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.h
index 1f5a97151f4043c723356e6db76cb491e0479a87..0bdd3834b5510c5cf4a38f01137226d107bfd4f8 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArvizProfileManagerWidget.h
@@ -21,30 +21,31 @@
*/
#pragma once
-#include <QWidget>
-#include <QDialog>
+#include <mutex>
+#include <string>
+#include <unordered_set>
+#include <utility>
+
+#include <QCheckBox>
#include <QComboBox>
-#include <QPushButton>
+#include <QDialog>
#include <QLineEdit>
+#include <QPushButton>
+#include <QRadioButton>
#include <QSettings>
#include <QString>
-#include <QCheckBox>
-#include <QRadioButton>
-
-#include <mutex>
+#include <QWidget>
#include <qcheckbox.h>
-#include <unordered_set>
-#include <string>
-#include <utility>
#include <RobotAPI/components/ArViz/Coin/Visualizer.h>
namespace armarx
{
- struct pair_hash
+ struct pair_hash
{
template <typename T1, typename T2>
- std::size_t operator ()(const std::pair<T1, T2>& p) const
+ std::size_t
+ operator()(const std::pair<T1, T2>& p) const
{
auto h1 = std::hash<T1>{}(p.first);
auto h2 = std::hash<T2>{}(p.second);
@@ -60,14 +61,16 @@ namespace armarx
bool additive = false;
};
-
class ProfileDialog : public QDialog
{
Q_OBJECT
public:
- explicit ProfileDialog(QWidget *parent = nullptr, const QString &name = "",
- bool additive = false, bool addDialog = true, bool isDefault = false);
+ explicit ProfileDialog(QWidget* parent = nullptr,
+ const QString& name = "",
+ bool additive = false,
+ bool addDialog = true,
+ bool isDefault = false);
~ProfileDialog();
QString getName() const;
@@ -81,22 +84,22 @@ namespace armarx
void deleteProfile();
private:
- QLineEdit *nameInput;
- QRadioButton *additiveRadioButton;
- QRadioButton *substractiveRadioButton;
- QPushButton *deleteButton;
- QCheckBox *defaultCheckBox;
+ QLineEdit* nameInput;
+ QRadioButton* additiveRadioButton;
+ QRadioButton* substractiveRadioButton;
+ QPushButton* deleteButton;
+ QCheckBox* defaultCheckBox;
};
-
- class ArvizProfileManagerWidget : public QWidget
+ class ArvizProfileManagerWidget : public QWidget
{
Q_OBJECT
public:
explicit ArvizProfileManagerWidget(QWidget* parent = nullptr);
- inline void update(const viz::CoinLayerID& layerID, bool visible)
+ inline void
+ update(const viz::CoinLayerID& layerID, bool visible)
{
std::scoped_lock lock(mtx);
@@ -109,35 +112,39 @@ namespace armarx
currentProfile.layers.erase(layerID);
currentProfile.layers.erase({layerID.first, "*"});
}
-
}
- inline void update(const std::string& componentName, bool visible)
+ inline void
+ update(const std::string& componentName, bool visible)
{
update({componentName, "*"}, visible);
}
- inline bool isHidden(const viz::CoinLayerID& layerID) const
+ inline bool
+ isHidden(const viz::CoinLayerID& layerID) const
{
std::scoped_lock lock(mtx);
- const bool result = currentProfile.layers.count(layerID) > 0 or currentProfile.layers.count({layerID.first, "*"}) > 0;
+ const bool result = currentProfile.layers.count(layerID) > 0 or
+ currentProfile.layers.count({layerID.first, "*"}) > 0;
if (currentProfile.additive)
{
return not result;
}
- else
+ else
{
return result;
}
}
- inline bool isHidden(const std::string& componentName) const
+ inline bool
+ isHidden(const std::string& componentName) const
{
std::scoped_lock lock(mtx);
- return currentProfile.additive == (currentProfile.layers.count({componentName, "*"}) == 0);
+ return currentProfile.additive ==
+ (currentProfile.layers.count({componentName, "*"}) == 0);
}
signals:
@@ -172,4 +179,4 @@ namespace armarx
static const QString SETTINGS_DEFAULT_PROFILE_KEY;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
index 2d20d7974a53aae2ebfa73d053b9d7a1a86b4439..0f19d1c75410aeacd0372c78c232a169ce9928bc 100644
--- a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.cpp
@@ -1,29 +1,29 @@
#include "LayerInfoTree.h"
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <SimoxUtility/algorithm/string.h>
-
#include <QCheckBox>
#include <QDoubleSpinBox>
#include <QLineEdit>
#include <QPushButton>
#include <QSpinBox>
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <RobotAPI/components/ArViz/Introspection/ElementJsonSerializers.h>
#include <RobotAPI/components/ArViz/Introspection/json_base.h>
#include <RobotAPI/components/ArViz/Introspection/json_elements.h>
#include <RobotAPI/components/ArViz/Introspection/json_layer.h>
-
namespace armarx
{
LayerInfoTree::LayerInfoTree()
{
}
- void LayerInfoTree::setWidget(QTreeWidget* widget)
+ void
+ LayerInfoTree::setWidget(QTreeWidget* widget)
{
if (!widgetConnections.empty())
{
@@ -38,48 +38,54 @@ namespace armarx
this->widget = widget;
// Update layer element properties when expanded.
- widgetConnections.append(connect(widget, &QTreeWidget::itemExpanded, [this](QTreeWidgetItem * item)
- {
- if (item->parent() || this->layers == nullptr)
- {
- return;
- }
-
- std::string id = item->text(0).toStdString();
- auto* layer = this->layers->findLayer(this->layerID);
- if (layer != nullptr)
- {
- viz::CoinLayerElement const* element = layer->findElement(id);
- if (element != nullptr)
- {
- this->updateLayerElementProperties(item, element->data);
- }
- }
- }));
+ widgetConnections.append(
+ connect(widget,
+ &QTreeWidget::itemExpanded,
+ [this](QTreeWidgetItem* item)
+ {
+ if (item->parent() || this->layers == nullptr)
+ {
+ return;
+ }
+
+ std::string id = item->text(0).toStdString();
+ auto* layer = this->layers->findLayer(this->layerID);
+ if (layer != nullptr)
+ {
+ viz::CoinLayerElement const* element = layer->findElement(id);
+ if (element != nullptr)
+ {
+ this->updateLayerElementProperties(item, element->data);
+ }
+ }
+ }));
widget->setColumnCount(2);
widget->setHeaderLabels({"Property", "Value"});
widget->setHeaderHidden(false);
}
- void LayerInfoTree::registerVisualizerCallbacks(viz::CoinVisualizer& visualizer)
+ void
+ LayerInfoTree::registerVisualizerCallbacks(viz::CoinVisualizer& visualizer)
{
visualizer.layerUpdatedCallbacks.emplace_back(
- [this](const viz::CoinLayerID & layerID, const viz::CoinLayer & layer)
- {
- if (layerID == this->layerID)
+ [this](const viz::CoinLayerID& layerID, const viz::CoinLayer& layer)
{
- this->update();
- }
- });
+ if (layerID == this->layerID)
+ {
+ this->update();
+ }
+ });
}
- void LayerInfoTree::setMaxElementCountDefault(int max)
+ void
+ LayerInfoTree::setMaxElementCountDefault(int max)
{
this->maxElementCountDefault = max;
}
- void LayerInfoTree::setEnabled(bool value)
+ void
+ LayerInfoTree::setEnabled(bool value)
{
if (this->enabled != value)
{
@@ -95,7 +101,8 @@ namespace armarx
}
}
- void LayerInfoTree::setSelectedLayer(viz::CoinLayerID id, viz::CoinLayerMap* layers)
+ void
+ LayerInfoTree::setSelectedLayer(viz::CoinLayerID id, viz::CoinLayerMap* layers)
{
this->layerID = id;
this->layers = layers;
@@ -108,8 +115,8 @@ namespace armarx
update();
}
-
- void LayerInfoTree::update()
+ void
+ LayerInfoTree::update()
{
if (enabled && layers)
{
@@ -121,8 +128,8 @@ namespace armarx
}
}
-
- void LayerInfoTree::clearLayerElements()
+ void
+ LayerInfoTree::clearLayerElements()
{
if (widget)
{
@@ -131,15 +138,16 @@ namespace armarx
}
}
-
- void LayerInfoTree::updateLayerElements(const std::vector<viz::CoinLayerElement>& elements)
+ void
+ LayerInfoTree::updateLayerElements(const std::vector<viz::CoinLayerElement>& elements)
{
if (showMoreItem)
{
// We always delete the item up-front. If we still need it, it will be added again.
// This way, we don't have to check for the showMoreItem while we update the entries.
int index = widget->indexOfTopLevelItem(showMoreItem);
- ARMARX_CHECK_NONNEGATIVE(index) << "Invariant: If showMoreItem != nullptr, then the widget contains it.";
+ ARMARX_CHECK_NONNEGATIVE(index)
+ << "Invariant: If showMoreItem != nullptr, then the widget contains it.";
delete widget->takeTopLevelItem(index);
showMoreItem = nullptr;
}
@@ -201,41 +209,46 @@ namespace armarx
}
}
-
- QTreeWidgetItem* LayerInfoTree::insertLayerElementItem(
- int index, const std::string& name, const viz::data::ElementPtr& element)
+ QTreeWidgetItem*
+ LayerInfoTree::insertLayerElementItem(int index,
+ const std::string& name,
+ const viz::data::ElementPtr& element)
{
- QTreeWidgetItem* item = new QTreeWidgetItem(QStringList { name.c_str(), getTypeName(element).c_str() });
+ QTreeWidgetItem* item =
+ new QTreeWidgetItem(QStringList{name.c_str(), getTypeName(element).c_str()});
// To be used when we can hide specific elements.
// item->setCheckState(0, Qt::CheckState::Unchecked);
widget->insertTopLevelItem(index, item);
return item;
}
- void LayerInfoTree::updateLayerElementItem(QTreeWidgetItem* item, const viz::data::ElementPtr& element)
+ void
+ LayerInfoTree::updateLayerElementItem(QTreeWidgetItem* item,
+ const viz::data::ElementPtr& element)
{
// Update type name.
item->setText(1, getTypeName(element).c_str());
}
-
- void LayerInfoTree::updateLayerElementProperties(QTreeWidgetItem* item, const viz::data::ElementPtr& element)
+ void
+ LayerInfoTree::updateLayerElementProperties(QTreeWidgetItem* item,
+ const viz::data::ElementPtr& element)
{
bool recursive = true;
updateJsonChildren(item, serializeElement(element), recursive);
}
-
- std::string LayerInfoTree::getTypeName(const viz::data::ElementPtr& element) const
+ std::string
+ LayerInfoTree::getTypeName(const viz::data::ElementPtr& element) const
{
const std::string typeName = simox::meta::get_type_name(*element);
return viz::json::ElementJsonSerializers::isTypeRegistered(typeName)
- ? typeName
- : "<unknown type '" + typeName + "'>";
+ ? typeName
+ : "<unknown type '" + typeName + "'>";
}
-
- nlohmann::json LayerInfoTree::serializeElement(const viz::data::ElementPtr& element) const
+ nlohmann::json
+ LayerInfoTree::serializeElement(const viz::data::ElementPtr& element) const
{
try
{
@@ -247,8 +260,10 @@ namespace armarx
}
}
-
- void LayerInfoTree::updateJsonChildren(QTreeWidgetItem* parent, const nlohmann::json& json, bool recursive)
+ void
+ LayerInfoTree::updateJsonChildren(QTreeWidgetItem* parent,
+ const nlohmann::json& json,
+ bool recursive)
{
if (json.is_array())
{
@@ -279,8 +294,8 @@ namespace armarx
else if (json.is_object())
{
namespace meta = viz::json::meta;
- const nlohmann::json& jmeta = json.count(meta::KEY) > 0 ? json.at(meta::KEY)
- : nlohmann::json::object();
+ const nlohmann::json& jmeta =
+ json.count(meta::KEY) > 0 ? json.at(meta::KEY) : nlohmann::json::object();
int iItem = 0;
for (auto it : json.items())
@@ -334,10 +349,10 @@ namespace armarx
}
}
-
- QTreeWidgetItem* LayerInfoTree::addJsonChild(QTreeWidgetItem* parent, const std::string& key)
+ QTreeWidgetItem*
+ LayerInfoTree::addJsonChild(QTreeWidgetItem* parent, const std::string& key)
{
- QTreeWidgetItem* child = new QTreeWidgetItem(QStringList { key.c_str(), "" });
+ QTreeWidgetItem* child = new QTreeWidgetItem(QStringList{key.c_str(), ""});
// To be used when we can actually change the values (enabled = use value from GUI).
// child->setCheckState(0, Qt::CheckState::Unchecked);
parent->addChild(child);
@@ -345,11 +360,12 @@ namespace armarx
return child;
}
-
- void LayerInfoTree::updateJsonItem(
- QTreeWidgetItem* item, const std::string& key, const nlohmann::json& value,
- const nlohmann::json& parentMeta,
- bool recursive)
+ void
+ LayerInfoTree::updateJsonItem(QTreeWidgetItem* item,
+ const std::string& key,
+ const nlohmann::json& value,
+ const nlohmann::json& parentMeta,
+ bool recursive)
{
const int columnKey = 0;
@@ -365,10 +381,11 @@ namespace armarx
}
}
-
- void LayerInfoTree::updateJsonItemValue(
- QTreeWidgetItem* item, const std::string& key, const nlohmann::json& value,
- const nlohmann::json& parentMeta)
+ void
+ LayerInfoTree::updateJsonItemValue(QTreeWidgetItem* item,
+ const std::string& key,
+ const nlohmann::json& value,
+ const nlohmann::json& parentMeta)
{
const int columnValue = 1;
@@ -404,8 +421,8 @@ namespace armarx
}
}
-
- QVariant LayerInfoTree::getValuePreview(const nlohmann::json& json) const
+ QVariant
+ LayerInfoTree::getValuePreview(const nlohmann::json& json) const
{
std::stringstream ss;
@@ -448,13 +465,10 @@ namespace armarx
}
}
-
- QWidget* LayerInfoTree::getValueWidget(QTreeWidgetItem* item, const nlohmann::json& json) const
+ QWidget*
+ LayerInfoTree::getValueWidget(QTreeWidgetItem* item, const nlohmann::json& json) const
{
- auto setChecked = [item]()
- {
- item->setCheckState(0, Qt::CheckState::Checked);
- };
+ auto setChecked = [item]() { item->setCheckState(0, Qt::CheckState::Checked); };
switch (json.type())
{
@@ -501,8 +515,8 @@ namespace armarx
}
}
-
- void LayerInfoTree::updateValueWidget(const nlohmann::json& json, QWidget* widget) const
+ void
+ LayerInfoTree::updateValueWidget(const nlohmann::json& json, QWidget* widget) const
{
switch (json.type())
{
@@ -543,41 +557,46 @@ namespace armarx
}
}
-
- bool LayerInfoTree::isMetaKey(const std::string& key)
+ bool
+ LayerInfoTree::isMetaKey(const std::string& key)
{
return key.substr(0, 2) == "__" && key.substr(key.size() - 2) == "__";
}
- void LayerInfoTree::addShowMoreItem()
+ void
+ LayerInfoTree::addShowMoreItem()
{
- ARMARX_CHECK_NULL(showMoreItem) << "There should not be a showMoreItem when this function is called.";
+ ARMARX_CHECK_NULL(showMoreItem)
+ << "There should not be a showMoreItem when this function is called.";
auto* layer = this->layers->findLayer(this->layerID);
if (layer == nullptr)
{
- ARMARX_WARNING << "No layer with ID '" << this->layerID.first
- << "/" << this->layerID.second << "' found";
+ ARMARX_WARNING << "No layer with ID '" << this->layerID.first << "/"
+ << this->layerID.second << "' found";
return;
}
std::stringstream ss;
- ss << "Showing " << widget->topLevelItemCount() << " of " << layer->elements.size() << " elements.";
+ ss << "Showing " << widget->topLevelItemCount() << " of " << layer->elements.size()
+ << " elements.";
// Add a new item.
- QTreeWidgetItem* item = new QTreeWidgetItem(QStringList { "", ss.str().c_str() });
+ QTreeWidgetItem* item = new QTreeWidgetItem(QStringList{"", ss.str().c_str()});
widget->addTopLevelItem(item);
QPushButton* button = new QPushButton("Show more");
widget->setItemWidget(item, 0, button);
- connect(button, &QPushButton::pressed, [this, layer]()
- {
- maxElementCount += maxElementCountDefault;
- this->updateLayerElements(layer->elements);
- });
+ connect(button,
+ &QPushButton::pressed,
+ [this, layer]()
+ {
+ maxElementCount += maxElementCountDefault;
+ this->updateLayerElements(layer->elements);
+ });
this->showMoreItem = item;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
index bccbff58de0ad18bfbd289f0d08ac21d69419bfb..99e0946552a30464e06fe7f51a54368443daddb0 100644
--- a/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
+++ b/source/RobotAPI/gui-plugins/ArViz/LayerInfoTree.h
@@ -6,7 +6,6 @@
#include <RobotAPI/components/ArViz/Coin/Visualizer.h>
-
namespace armarx
{
@@ -18,7 +17,6 @@ namespace armarx
Q_OBJECT
public:
-
LayerInfoTree();
virtual ~LayerInfoTree() = default;
@@ -49,7 +47,6 @@ namespace armarx
private:
-
void clearLayerElements();
/**
@@ -60,12 +57,15 @@ namespace armarx
void updateLayerElements(const std::vector<viz::CoinLayerElement>& elements);
/// Insert a (top-level) layer element at `ìndex`.
- QTreeWidgetItem* insertLayerElementItem(int index, const std::string& name, const viz::data::ElementPtr& element);
+ QTreeWidgetItem* insertLayerElementItem(int index,
+ const std::string& name,
+ const viz::data::ElementPtr& element);
/// Update the layer element's item data. Does not update its children.
void updateLayerElementItem(QTreeWidgetItem* item, const viz::data::ElementPtr& element);
/// Update the layer element's properties (children).
- void updateLayerElementProperties(QTreeWidgetItem* item, const viz::data::ElementPtr& element);
+ void updateLayerElementProperties(QTreeWidgetItem* item,
+ const viz::data::ElementPtr& element);
/// Get `element`'s type name.
@@ -82,7 +82,9 @@ namespace armarx
*
* @param recursive If true,
*/
- void updateJsonChildren(QTreeWidgetItem* parent, const nlohmann::json& json, bool recursive = false);
+ void updateJsonChildren(QTreeWidgetItem* parent,
+ const nlohmann::json& json,
+ bool recursive = false);
/// Adds a child item with `key` to `parent`.
QTreeWidgetItem* addJsonChild(QTreeWidgetItem* parent, const std::string& key);
@@ -91,11 +93,15 @@ namespace armarx
* @brief Updates `item`'s key and value.
* @param recursive If true, also update its children.
*/
- void updateJsonItem(QTreeWidgetItem* item, const std::string& key, const nlohmann::json& value,
+ void updateJsonItem(QTreeWidgetItem* item,
+ const std::string& key,
+ const nlohmann::json& value,
const nlohmann::json& parentMeta,
bool recursive = false);
- void updateJsonItemValue(QTreeWidgetItem* item, const std::string& key, const nlohmann::json& value,
+ void updateJsonItemValue(QTreeWidgetItem* item,
+ const std::string& key,
+ const nlohmann::json& value,
const nlohmann::json& parentMeta);
@@ -110,7 +116,6 @@ namespace armarx
private:
-
QTreeWidget* widget = nullptr;
/// Connections to the current widget.
QVector<QMetaObject::Connection> widgetConnections;
@@ -123,8 +128,5 @@ namespace armarx
viz::CoinLayerID layerID = {"", ""};
const viz::CoinLayerMap* layers = nullptr;
-
-
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.cpp b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.cpp
index de721d9a03ffbaacffa0afd6173d7b8fb1a29048..cc8a973b6be65a5d83fd2e8559cbb013a194866a 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.cpp
@@ -28,6 +28,6 @@ namespace armarx
{
ArVizDrawerGuiGuiPlugin::ArVizDrawerGuiGuiPlugin()
{
- addWidget < ArVizDrawerGuiWidgetController > ();
+ addWidget<ArVizDrawerGuiWidgetController>();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.h b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.h
index 91dda180873e5afaa28036839cc53cc163aec25a..d6215d689a71225f5d6357efc704a7c745c527a5 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiGuiPlugin.h
@@ -22,8 +22,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -34,8 +35,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT ArVizDrawerGuiGuiPlugin:
- public armarx::ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT ArVizDrawerGuiGuiPlugin : public armarx::ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -47,4 +47,4 @@ namespace armarx
*/
ArVizDrawerGuiGuiPlugin();
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp
index 090a2c7e47eeb7647b955dc69b166d9aa97b2c05..e57d9517d57500a06eb43f2a158fc614c7051502 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.cpp
@@ -20,24 +20,27 @@
* GNU General Public License
*/
+#include "ArVizDrawerGuiWidgetController.h"
+
#include <string>
-#include "ArVizDrawerGuiWidgetController.h"
#include "Elements/ElementWidgetBase.h"
-#include "Elements/RobotWidget.h"
#include "Elements/PoseWidget.h"
+#include "Elements/RobotWidget.h"
namespace armarx
{
ArVizDrawerGuiWidgetController::ArVizDrawerGuiWidgetController()
{
_ui.setupUi(getWidget());
- connect(_ui.pushButtonElementAdd, &QPushButton::clicked, this,
+ connect(_ui.pushButtonElementAdd,
+ &QPushButton::clicked,
+ this,
&ArVizDrawerGuiWidgetController::on_pushButtonElementAdd_clicked);
//setup factories
{
_factory["Robot"] = [] { return new RobotWidget; };
- _factory["Pose" ] = [] { return new PoseWidget ; };
+ _factory["Pose"] = [] { return new PoseWidget; };
for (const auto& [key, _] : _factory)
{
@@ -47,16 +50,20 @@ namespace armarx
startTimer(20);
}
- void ArVizDrawerGuiWidgetController::onConnectComponent()
+ void
+ ArVizDrawerGuiWidgetController::onConnectComponent()
{
_connected = true;
}
- void ArVizDrawerGuiWidgetController::onDisconnectComponent()
+
+ void
+ ArVizDrawerGuiWidgetController::onDisconnectComponent()
{
_connected = false;
}
- void ArVizDrawerGuiWidgetController::on_pushButtonElementAdd_clicked()
+ void
+ ArVizDrawerGuiWidgetController::on_pushButtonElementAdd_clicked()
{
const auto name = _ui.comboBoxElement->currentText().toStdString();
auto* ptr = _factory.at(name)();
@@ -64,7 +71,8 @@ namespace armarx
_elements.emplace(ptr);
}
- void ArVizDrawerGuiWidgetController::timerEvent(QTimerEvent*)
+ void
+ ArVizDrawerGuiWidgetController::timerEvent(QTimerEvent*)
{
if (!_connected)
{
@@ -86,5 +94,4 @@ namespace armarx
}
getArvizClient().commit({layer});
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.h b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.h
index 20382221d2c2810ad7c62cbe5ff3921d688f6f50..ff12aea828f3dd8065162ce975648cace94b1d81 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/ArVizDrawerGuiWidgetController.h
@@ -25,11 +25,11 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/gui-plugins/ArVizDrawerGui/ui_ArVizDrawerGuiWidget.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
namespace armarx
{
@@ -53,9 +53,8 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- ArVizDrawerGuiWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < ArVizDrawerGuiWidgetController >,
+ class ARMARXCOMPONENT_IMPORT_EXPORT ArVizDrawerGuiWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<ArVizDrawerGuiWidgetController>,
public virtual ArVizComponentPluginUser
{
Q_OBJECT
@@ -63,15 +62,27 @@ namespace armarx
explicit ArVizDrawerGuiWidgetController();
virtual ~ArVizDrawerGuiWidgetController() = default;
- void loadSettings(QSettings* settings) override {}
- void saveSettings(QSettings* settings) override {}
+ void
+ loadSettings(QSettings* settings) override
+ {
+ }
+
+ void
+ saveSettings(QSettings* settings) override
+ {
+ }
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Visualization.ArVizDrawer";
}
- void onInitComponent() override {}
+ void
+ onInitComponent() override
+ {
+ }
+
void onConnectComponent() override;
void onDisconnectComponent() override;
@@ -82,11 +93,9 @@ namespace armarx
void on_pushButtonElementAdd_clicked();
private:
- Ui::ArVizDrawerGuiWidget _ui;
- std::atomic_bool _connected{false};
- std::set<ElementWidgetBase*> _elements;
+ Ui::ArVizDrawerGuiWidget _ui;
+ std::atomic_bool _connected{false};
+ std::set<ElementWidgetBase*> _elements;
std::map<std::string, std::function<ElementWidgetBase*()>> _factory;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/ElementWidgetBase.h b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/ElementWidgetBase.h
index 0d111c37d8af5e7cf528b36aa8fa3d83275601b4..bad53b3eb7a2e6bc1a99916d25407125d91fb163 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/ElementWidgetBase.h
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/ElementWidgetBase.h
@@ -2,8 +2,8 @@
#include <QWidget>
-#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/components/ArViz/Client/Layer.h>
namespace armarx
{
@@ -16,7 +16,7 @@ namespace armarx
virtual bool toDelete() const = 0;
};
- template<class UiT>
+ template <class UiT>
class ElementWidgetBaseTemplate : public ElementWidgetBase
{
public:
@@ -26,10 +26,13 @@ namespace armarx
_ui.pushButtonDelete->setCheckable(true);
_ui.pushButtonDelete->setChecked(false);
}
- bool toDelete() const override
+
+ bool
+ toDelete() const override
{
return _ui.pushButtonDelete->isChecked();
}
+
UiT _ui;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/PoseWidget.cpp b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/PoseWidget.cpp
index 61e5af2ff9534ecd9a538981101445cb4810f92f..2ba595692fc38e4052b8ab0f852741ae9e43f4f0 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/PoseWidget.cpp
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/PoseWidget.cpp
@@ -2,16 +2,17 @@
namespace armarx
{
- void PoseWidget::addTo(viz::Layer& layer) const
+ void
+ PoseWidget::addTo(viz::Layer& layer) const
{
const auto adr = reinterpret_cast<std::intptr_t>(this);
layer.add(viz::Pose("Pose_" + std::to_string(adr))
- .position(_ui.doubleSpinBoxTX->value(),
- _ui.doubleSpinBoxTY->value(),
- _ui.doubleSpinBoxTZ->value())
- .orientation(_ui.doubleSpinBoxRX->value(),
- _ui.doubleSpinBoxRY->value(),
- _ui.doubleSpinBoxRZ->value())
- .scale(_ui.doubleSpinBoxScale->value()));
+ .position(_ui.doubleSpinBoxTX->value(),
+ _ui.doubleSpinBoxTY->value(),
+ _ui.doubleSpinBoxTZ->value())
+ .orientation(_ui.doubleSpinBoxRX->value(),
+ _ui.doubleSpinBoxRY->value(),
+ _ui.doubleSpinBoxRZ->value())
+ .scale(_ui.doubleSpinBoxScale->value()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/PoseWidget.h b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/PoseWidget.h
index e54984d7a48cc7ff78123dddd16e97c8d9fef0af..e401feb5caf56fab8e9974ffebee17b3d0332cef 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/PoseWidget.h
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/PoseWidget.h
@@ -1,9 +1,9 @@
#pragma once
-#include "ElementWidgetBase.h"
-
#include <RobotAPI/gui-plugins/ArVizDrawerGui/ui_PoseWidget.h>
+#include "ElementWidgetBase.h"
+
namespace armarx
{
class PoseWidget : public ElementWidgetBaseTemplate<Ui::PoseWidget>
@@ -11,4 +11,4 @@ namespace armarx
public:
void addTo(viz::Layer& layer) const override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.cpp b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.cpp
index 1e1866223b5b817d999610f59ac0e006353fe9d1..35dbb0c6ae477a26ead85fed16517322a8d162db 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.cpp
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.cpp
@@ -2,18 +2,19 @@
namespace armarx
{
- void RobotWidget::addTo(viz::Layer& layer) const
+ void
+ RobotWidget::addTo(viz::Layer& layer) const
{
const auto adr = reinterpret_cast<std::intptr_t>(this);
auto r = viz::Robot("Robot_" + std::to_string(adr))
- .file(_ui.lineEditProject->text().toStdString(),
- _ui.comboBoxFile->currentText().toStdString())
- .position(_ui.doubleSpinBoxTX->value(),
- _ui.doubleSpinBoxTY->value(),
- _ui.doubleSpinBoxTZ->value())
- .orientation(_ui.doubleSpinBoxRX->value(),
- _ui.doubleSpinBoxRY->value(),
- _ui.doubleSpinBoxRZ->value());
+ .file(_ui.lineEditProject->text().toStdString(),
+ _ui.comboBoxFile->currentText().toStdString())
+ .position(_ui.doubleSpinBoxTX->value(),
+ _ui.doubleSpinBoxTY->value(),
+ _ui.doubleSpinBoxTZ->value())
+ .orientation(_ui.doubleSpinBoxRX->value(),
+ _ui.doubleSpinBoxRY->value(),
+ _ui.doubleSpinBoxRZ->value());
if (_ui.checkBoxColor->isChecked())
{
r.overrideColor(viz::Color{static_cast<float>(_ui.doubleSpinBoxR->value()),
@@ -23,4 +24,4 @@ namespace armarx
}
layer.add(r);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.h b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.h
index d2b24d8f4e644171361dbf603d2228f24a55055a..f11682ad9e7df6ef4b8252d7614d5f65b6415479 100644
--- a/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.h
+++ b/source/RobotAPI/gui-plugins/ArVizDrawerGui/Elements/RobotWidget.h
@@ -1,9 +1,9 @@
#pragma once
-#include "ElementWidgetBase.h"
-
#include <RobotAPI/gui-plugins/ArVizDrawerGui/ui_RobotWidget.h>
+#include "ElementWidgetBase.h"
+
namespace armarx
{
class RobotWidget : public ElementWidgetBaseTemplate<Ui::RobotWidget>
@@ -11,4 +11,4 @@ namespace armarx
public:
void addTo(viz::Layer& layer) const override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.cpp b/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.cpp
index 1e8b97c14c4d086336373c19dfcd59aff13e4ca9..cde276c52e1e2e70cd5e4cbcb55f3bed41cf7025 100644
--- a/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.cpp
@@ -20,53 +20,60 @@
* GNU General Public License
*/
+#include "BoxToGraspCandidatesWidgetController.h"
+
#include <string>
-#include <SimoxUtility/shapes/OrientedBox.h>
-#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <SimoxUtility/math/convert/pos_mat3f_to_mat4f.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <SimoxUtility/math/distance/angle_between.h>
-#include <SimoxUtility/math/distance/angle_between.h>
#include <SimoxUtility/math/project_to_plane.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
#include <RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h>
-#include <RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h>
#include <RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.h>
-
-#include "BoxToGraspCandidatesWidgetController.h"
+#include <RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h>
// configure
namespace armarx
{
- void BoxToGraspCandidatesWidgetController::loadSettings(QSettings* settings)
+ void
+ BoxToGraspCandidatesWidgetController::loadSettings(QSettings* settings)
{
- getRobotStateComponentPlugin().setRobotStateComponentName(settings->value("rsc", "Armar6StateComponent").toString().toStdString());
+ getRobotStateComponentPlugin().setRobotStateComponentName(
+ settings->value("rsc", "Armar6StateComponent").toString().toStdString());
_gc_topic_name = settings->value("rsc", "GraspCandidatesTopic").toString().toStdString();
}
- void BoxToGraspCandidatesWidgetController::saveSettings(QSettings* settings)
+ void
+ BoxToGraspCandidatesWidgetController::saveSettings(QSettings* settings)
{
- settings->setValue("rsc", QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
+ settings->setValue(
+ "rsc",
+ QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
settings->setValue("gct", QString::fromStdString(_gc_topic_name));
}
- QPointer<QDialog> BoxToGraspCandidatesWidgetController::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ BoxToGraspCandidatesWidgetController::getConfigDialog(QWidget* parent)
{
if (!_dialog)
{
_dialog = new SimpleConfigDialog(parent);
- _dialog->addProxyFinder<RobotStateComponentInterfacePrx>("rsc", "Robot State Component", "*Component");
+ _dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
+ "rsc", "Robot State Component", "*Component");
_dialog->addLineEdit("gct", "Grasp Candidates Topic", "GraspCandidatesTopic");
}
return qobject_cast<SimpleConfigDialog*>(_dialog);
}
- void BoxToGraspCandidatesWidgetController::configured()
+
+ void
+ BoxToGraspCandidatesWidgetController::configured()
{
getRobotStateComponentPlugin().setRobotStateComponentName(_dialog->getProxyName("rsc"));
_gc_topic_name = _dialog->getLineEditText("gct");
}
-}
+} // namespace armarx
namespace armarx
{
@@ -75,16 +82,10 @@ namespace armarx
_ui.setupUi(getWidget());
_box_pose.setXYZWidgets(
- _ui.doubleSpinBoxBoxTX,
- _ui.doubleSpinBoxBoxTY,
- _ui.doubleSpinBoxBoxTZ
- );
+ _ui.doubleSpinBoxBoxTX, _ui.doubleSpinBoxBoxTY, _ui.doubleSpinBoxBoxTZ);
_box_pose.setRPYWidgets(
- _ui.doubleSpinBoxBoxRX,
- _ui.doubleSpinBoxBoxRY,
- _ui.doubleSpinBoxBoxRZ
- );
+ _ui.doubleSpinBoxBoxRX, _ui.doubleSpinBoxBoxRY, _ui.doubleSpinBoxBoxRZ);
_box_extend.addWidget(_ui.doubleSpinBoxBoxSizeX);
_box_extend.addWidget(_ui.doubleSpinBoxBoxSizeY);
@@ -114,70 +115,97 @@ namespace armarx
_tcp_shift_r.addWidget(_ui.doubleSpinBoxTCPShiftRY);
_tcp_shift_r.addWidget(_ui.doubleSpinBoxTCPShiftRZ);
- const std::vector spinboxes
- {
+ const std::vector spinboxes{
_ui.doubleSpinBoxBoundTransvMax, _ui.doubleSpinBoxBoundTransvMin,
- _ui.doubleSpinBoxBoundWidthMin, _ui.doubleSpinBoxBoundWidthMax,
- _ui.doubleSpinBoxBoundDepthMin, _ui.doubleSpinBoxBoundDepthMax,
-
- _ui.doubleSpinBoxBoxTX, _ui.doubleSpinBoxBoxTY, _ui.doubleSpinBoxBoxTZ,
- _ui.doubleSpinBoxBoxRX, _ui.doubleSpinBoxBoxRY, _ui.doubleSpinBoxBoxRZ,
- _ui.doubleSpinBoxBoxSizeX, _ui.doubleSpinBoxBoxSizeY, _ui.doubleSpinBoxBoxSizeZ,
-
- _ui.doubleSpinBoxHandLTransvX, _ui.doubleSpinBoxHandLTransvY, _ui.doubleSpinBoxHandLTransvZ,
- _ui.doubleSpinBoxHandRTransvX, _ui.doubleSpinBoxHandRTransvY, _ui.doubleSpinBoxHandRTransvZ,
-
- _ui.doubleSpinBoxHandLUpX, _ui.doubleSpinBoxHandLUpY, _ui.doubleSpinBoxHandLUpZ,
- _ui.doubleSpinBoxHandRUpX, _ui.doubleSpinBoxHandRUpY, _ui.doubleSpinBoxHandRUpZ,
-
- _ui.doubleSpinBoxTCPShiftLX, _ui.doubleSpinBoxTCPShiftLY, _ui.doubleSpinBoxTCPShiftLZ,
- _ui.doubleSpinBoxTCPShiftRX, _ui.doubleSpinBoxTCPShiftRY, _ui.doubleSpinBoxTCPShiftRZ
- };
+ _ui.doubleSpinBoxBoundWidthMin, _ui.doubleSpinBoxBoundWidthMax,
+ _ui.doubleSpinBoxBoundDepthMin, _ui.doubleSpinBoxBoundDepthMax,
+
+ _ui.doubleSpinBoxBoxTX, _ui.doubleSpinBoxBoxTY,
+ _ui.doubleSpinBoxBoxTZ, _ui.doubleSpinBoxBoxRX,
+ _ui.doubleSpinBoxBoxRY, _ui.doubleSpinBoxBoxRZ,
+ _ui.doubleSpinBoxBoxSizeX, _ui.doubleSpinBoxBoxSizeY,
+ _ui.doubleSpinBoxBoxSizeZ,
+
+ _ui.doubleSpinBoxHandLTransvX, _ui.doubleSpinBoxHandLTransvY,
+ _ui.doubleSpinBoxHandLTransvZ, _ui.doubleSpinBoxHandRTransvX,
+ _ui.doubleSpinBoxHandRTransvY, _ui.doubleSpinBoxHandRTransvZ,
+
+ _ui.doubleSpinBoxHandLUpX, _ui.doubleSpinBoxHandLUpY,
+ _ui.doubleSpinBoxHandLUpZ, _ui.doubleSpinBoxHandRUpX,
+ _ui.doubleSpinBoxHandRUpY, _ui.doubleSpinBoxHandRUpZ,
+
+ _ui.doubleSpinBoxTCPShiftLX, _ui.doubleSpinBoxTCPShiftLY,
+ _ui.doubleSpinBoxTCPShiftLZ, _ui.doubleSpinBoxTCPShiftRX,
+ _ui.doubleSpinBoxTCPShiftRY, _ui.doubleSpinBoxTCPShiftRZ};
for (QDoubleSpinBox* ptr : spinboxes)
{
- connect(ptr, qOverload<double>(&QDoubleSpinBox::valueChanged), this, &BoxToGraspCandidatesWidgetController::update);
+ connect(ptr,
+ qOverload<double>(&QDoubleSpinBox::valueChanged),
+ this,
+ &BoxToGraspCandidatesWidgetController::update);
}
- const std::vector checkboxes
- {
- //Left
- _ui.checkBoxGraspLPosXPosY, _ui.checkBoxGraspLPosXPosZ,
- _ui.checkBoxGraspLPosXNegY, _ui.checkBoxGraspLPosXNegZ,
-
- _ui.checkBoxGraspLNegXPosY, _ui.checkBoxGraspLNegXPosZ,
- _ui.checkBoxGraspLNegXNegY, _ui.checkBoxGraspLNegXNegZ,
-
- _ui.checkBoxGraspLPosYPosX, _ui.checkBoxGraspLPosYPosZ,
- _ui.checkBoxGraspLPosYNegX, _ui.checkBoxGraspLPosYNegZ,
-
- _ui.checkBoxGraspLNegYPosX, _ui.checkBoxGraspLNegYPosZ,
- _ui.checkBoxGraspLNegYNegX, _ui.checkBoxGraspLNegYNegZ,
-
- _ui.checkBoxGraspLPosZPosX, _ui.checkBoxGraspLPosZPosY,
- _ui.checkBoxGraspLPosZNegX, _ui.checkBoxGraspLPosZNegY,
-
- _ui.checkBoxGraspLNegZPosX, _ui.checkBoxGraspLNegZPosY,
- _ui.checkBoxGraspLNegZNegX, _ui.checkBoxGraspLNegZNegY,
-
- //Right
- _ui.checkBoxGraspRPosXPosY, _ui.checkBoxGraspRPosXPosZ,
- _ui.checkBoxGraspRPosXNegY, _ui.checkBoxGraspRPosXNegZ,
-
- _ui.checkBoxGraspRNegXPosY, _ui.checkBoxGraspRNegXPosZ,
- _ui.checkBoxGraspRNegXNegY, _ui.checkBoxGraspRNegXNegZ,
-
- _ui.checkBoxGraspRPosYPosX, _ui.checkBoxGraspRPosYPosZ,
- _ui.checkBoxGraspRPosYNegX, _ui.checkBoxGraspRPosYNegZ,
-
- _ui.checkBoxGraspRNegYPosX, _ui.checkBoxGraspRNegYPosZ,
- _ui.checkBoxGraspRNegYNegX, _ui.checkBoxGraspRNegYNegZ,
-
- _ui.checkBoxGraspRPosZPosX, _ui.checkBoxGraspRPosZPosY,
- _ui.checkBoxGraspRPosZNegX, _ui.checkBoxGraspRPosZNegY,
-
- _ui.checkBoxGraspRNegZPosX, _ui.checkBoxGraspRNegZPosY,
- _ui.checkBoxGraspRNegZNegX, _ui.checkBoxGraspRNegZNegY
- };
+ const std::vector checkboxes{//Left
+ _ui.checkBoxGraspLPosXPosY,
+ _ui.checkBoxGraspLPosXPosZ,
+ _ui.checkBoxGraspLPosXNegY,
+ _ui.checkBoxGraspLPosXNegZ,
+
+ _ui.checkBoxGraspLNegXPosY,
+ _ui.checkBoxGraspLNegXPosZ,
+ _ui.checkBoxGraspLNegXNegY,
+ _ui.checkBoxGraspLNegXNegZ,
+
+ _ui.checkBoxGraspLPosYPosX,
+ _ui.checkBoxGraspLPosYPosZ,
+ _ui.checkBoxGraspLPosYNegX,
+ _ui.checkBoxGraspLPosYNegZ,
+
+ _ui.checkBoxGraspLNegYPosX,
+ _ui.checkBoxGraspLNegYPosZ,
+ _ui.checkBoxGraspLNegYNegX,
+ _ui.checkBoxGraspLNegYNegZ,
+
+ _ui.checkBoxGraspLPosZPosX,
+ _ui.checkBoxGraspLPosZPosY,
+ _ui.checkBoxGraspLPosZNegX,
+ _ui.checkBoxGraspLPosZNegY,
+
+ _ui.checkBoxGraspLNegZPosX,
+ _ui.checkBoxGraspLNegZPosY,
+ _ui.checkBoxGraspLNegZNegX,
+ _ui.checkBoxGraspLNegZNegY,
+
+ //Right
+ _ui.checkBoxGraspRPosXPosY,
+ _ui.checkBoxGraspRPosXPosZ,
+ _ui.checkBoxGraspRPosXNegY,
+ _ui.checkBoxGraspRPosXNegZ,
+
+ _ui.checkBoxGraspRNegXPosY,
+ _ui.checkBoxGraspRNegXPosZ,
+ _ui.checkBoxGraspRNegXNegY,
+ _ui.checkBoxGraspRNegXNegZ,
+
+ _ui.checkBoxGraspRPosYPosX,
+ _ui.checkBoxGraspRPosYPosZ,
+ _ui.checkBoxGraspRPosYNegX,
+ _ui.checkBoxGraspRPosYNegZ,
+
+ _ui.checkBoxGraspRNegYPosX,
+ _ui.checkBoxGraspRNegYPosZ,
+ _ui.checkBoxGraspRNegYNegX,
+ _ui.checkBoxGraspRNegYNegZ,
+
+ _ui.checkBoxGraspRPosZPosX,
+ _ui.checkBoxGraspRPosZPosY,
+ _ui.checkBoxGraspRPosZNegX,
+ _ui.checkBoxGraspRPosZNegY,
+
+ _ui.checkBoxGraspRNegZPosX,
+ _ui.checkBoxGraspRNegZPosY,
+ _ui.checkBoxGraspRNegZNegX,
+ _ui.checkBoxGraspRNegZNegY};
for (QCheckBox* ptr : checkboxes)
{
ptr->setChecked(true);
@@ -185,7 +213,8 @@ namespace armarx
}
}
- void BoxToGraspCandidatesWidgetController::onConnectComponent()
+ void
+ BoxToGraspCandidatesWidgetController::onConnectComponent()
{
{
std::lock_guard guard{_mutex};
@@ -195,7 +224,8 @@ namespace armarx
QMetaObject::invokeMethod(this, "update", Qt::QueuedConnection);
}
- void BoxToGraspCandidatesWidgetController::update()
+ void
+ BoxToGraspCandidatesWidgetController::update()
{
ARMARX_TRACE;
std::lock_guard guard{_mutex};
@@ -204,7 +234,8 @@ namespace armarx
return;
}
synchronizeLocalClone(_robot);
- const simox::OrientedBox<float> box {_box_pose.getMat4(), _box_extend.get<Eigen::Vector3f>()};
+ const simox::OrientedBox<float> box{_box_pose.getMat4(),
+ _box_extend.get<Eigen::Vector3f>()};
grasp_candidate_drawer gc_draw{getRobotNameHelper(), _robot};
box_to_grasp_candidates b2gc{getRobotNameHelper(), _robot};
@@ -215,19 +246,19 @@ namespace armarx
l.hand_transverse = _transversal_l.get<Eigen::Vector3f>().normalized();
r.hand_transverse = _transversal_r.get<Eigen::Vector3f>().normalized();
- l.hand_up = _up_l.get<Eigen::Vector3f>().normalized();
- r.hand_up = _up_r.get<Eigen::Vector3f>().normalized();
- l.tcp_shift = _tcp_shift_l.get<Eigen::Vector3f>().normalized();
- r.tcp_shift = _tcp_shift_r.get<Eigen::Vector3f>().normalized();
+ l.hand_up = _up_l.get<Eigen::Vector3f>().normalized();
+ r.hand_up = _up_r.get<Eigen::Vector3f>().normalized();
+ l.tcp_shift = _tcp_shift_l.get<Eigen::Vector3f>().normalized();
+ r.tcp_shift = _tcp_shift_r.get<Eigen::Vector3f>().normalized();
}
box_to_grasp_candidates::length_limit lim;
{
lim.transverse_length_min = _ui.doubleSpinBoxBoundTransvMin->value();
lim.transverse_length_max = _ui.doubleSpinBoxBoundTransvMax->value();
- lim.upward_length_min = _ui.doubleSpinBoxBoundDepthMin->value();
- lim.upward_length_max = _ui.doubleSpinBoxBoundDepthMax->value();
- lim.forward_length_min = _ui.doubleSpinBoxBoundWidthMin->value();
- lim.forward_length_max = _ui.doubleSpinBoxBoundWidthMax->value();
+ lim.upward_length_min = _ui.doubleSpinBoxBoundDepthMin->value();
+ lim.upward_length_max = _ui.doubleSpinBoxBoundDepthMax->value();
+ lim.forward_length_min = _ui.doubleSpinBoxBoundWidthMin->value();
+ lim.forward_length_max = _ui.doubleSpinBoxBoundWidthMax->value();
}
box_to_grasp_candidates::side_enabled_map side_enabled_l;
{
@@ -296,34 +327,31 @@ namespace armarx
side_enabled_r[ba::neg_z_neg_y] = _ui.checkBoxGraspRNegZNegY->isChecked();
}
- viz::Layer layer_l = arviz.layer("grasps_l");
- viz::Layer layer_r = arviz.layer("grasps_r");
+ viz::Layer layer_l = arviz.layer("grasps_l");
+ viz::Layer layer_r = arviz.layer("grasps_r");
viz::Layer layer_hand_vec = arviz.layer("hand_vec");
- viz::Layer layer_box = arviz.layer("box");
- layer_box.add(viz::Box{"box"}
- .set(box)
- .transformPose(_robot->getGlobalPose())
- );
+ viz::Layer layer_box = arviz.layer("box");
+ layer_box.add(viz::Box{"box"}.set(box).transformPose(_robot->getGlobalPose()));
- gc_draw.draw(b2gc.side("Left"), layer_hand_vec);
+ gc_draw.draw(b2gc.side("Left"), layer_hand_vec);
gc_draw.draw(b2gc.side("Right"), layer_hand_vec);
grasping::GraspCandidateSeq graps;
- const auto consume_l = [&](const auto & g)
+ const auto consume_l = [&](const auto& g)
{
graps.emplace_back(g.make_candidate(getName()));
gc_draw.draw(graps.back(), layer_l);
};
- const auto consume_r = [&](const auto & g)
+ const auto consume_r = [&](const auto& g)
{
graps.emplace_back(g.make_candidate(getName()));
gc_draw.draw(graps.back(), layer_r);
};
- b2gc.center_grasps(box, "Left", lim, consume_l, side_enabled_l);
+ b2gc.center_grasps(box, "Left", lim, consume_l, side_enabled_l);
b2gc.center_grasps(box, "Right", lim, consume_r, side_enabled_r);
arviz.commit({layer_l, layer_r, layer_hand_vec, layer_box});
_gc_topic->reportGraspCandidates(getName(), graps);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.h b/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.h
index 4a74933b2dec8300cc88109b760effaee7ded3c6..5dac46723db68a8c725551281e9fcdc090cd05be 100644
--- a/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.h
+++ b/source/RobotAPI/gui-plugins/BoxToGraspCandidates/BoxToGraspCandidatesWidgetController.h
@@ -23,17 +23,16 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToPose.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/SpinBoxToVector.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+#include <RobotAPI/gui-plugins/BoxToGraspCandidates/ui_BoxToGraspCandidatesWidget.h>
+#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
-
-#include <RobotAPI/gui-plugins/BoxToGraspCandidates/ui_BoxToGraspCandidatesWidget.h>
namespace armarx
{
@@ -55,9 +54,9 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- BoxToGraspCandidatesWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < BoxToGraspCandidatesWidgetController >,
+ class ARMARXCOMPONENT_IMPORT_EXPORT BoxToGraspCandidatesWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<
+ BoxToGraspCandidatesWidgetController>,
public virtual ArVizComponentPluginUser,
public virtual RobotStateComponentPluginUser
{
@@ -70,12 +69,17 @@ namespace armarx
void loadSettings(QSettings* settings) override;
void saveSettings(QSettings* settings) override;
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Grasping.BoxToGraspCandidates";
}
- void onInitComponent() override {}
+ void
+ onInitComponent() override
+ {
+ }
+
void onConnectComponent() override;
QPointer<QDialog> getConfigDialog(QWidget* parent);
@@ -84,23 +88,21 @@ namespace armarx
void update();
private:
- std::mutex _mutex;
- QPointer<SimpleConfigDialog> _dialog;
- Ui::BoxToGraspCandidatesWidget _ui;
-
- SpinBoxToPose<QDoubleSpinBox> _box_pose;
- SpinBoxToVector<QDoubleSpinBox, 3> _box_extend;
- SpinBoxToVector<QDoubleSpinBox, 3> _tcp_shift_l;
- SpinBoxToVector<QDoubleSpinBox, 3> _tcp_shift_r;
- SpinBoxToVector<QDoubleSpinBox, 3> _transversal_l;
- SpinBoxToVector<QDoubleSpinBox, 3> _transversal_r;
- SpinBoxToVector<QDoubleSpinBox, 3> _up_l;
- SpinBoxToVector<QDoubleSpinBox, 3> _up_r;
+ std::mutex _mutex;
+ QPointer<SimpleConfigDialog> _dialog;
+ Ui::BoxToGraspCandidatesWidget _ui;
+
+ SpinBoxToPose<QDoubleSpinBox> _box_pose;
+ SpinBoxToVector<QDoubleSpinBox, 3> _box_extend;
+ SpinBoxToVector<QDoubleSpinBox, 3> _tcp_shift_l;
+ SpinBoxToVector<QDoubleSpinBox, 3> _tcp_shift_r;
+ SpinBoxToVector<QDoubleSpinBox, 3> _transversal_l;
+ SpinBoxToVector<QDoubleSpinBox, 3> _transversal_r;
+ SpinBoxToVector<QDoubleSpinBox, 3> _up_l;
+ SpinBoxToVector<QDoubleSpinBox, 3> _up_r;
VirtualRobot::RobotPtr _robot;
- std::string _gc_topic_name;
+ std::string _gc_topic_name;
armarx::grasping::GraspCandidatesTopicInterfacePrx _gc_topic;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginGuiPlugin.cpp b/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginGuiPlugin.cpp
index cb25e8c75ea94d59c26c88d2e4e5bcf16b264163..188ce6ecf8a11d15b099acfc0badf7c7703a5914 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginGuiPlugin.cpp
@@ -28,6 +28,6 @@ namespace armarx
{
DebugDrawerGuiPluginGuiPlugin::DebugDrawerGuiPluginGuiPlugin()
{
- addWidget < DebugDrawerGuiPluginWidgetController > ();
+ addWidget<DebugDrawerGuiPluginWidgetController>();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginGuiPlugin.h b/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginGuiPlugin.h
index 762c61fc88303d0d627de2b37164ef9b9bd1e6b1..3469ed34b008d536db4ac301685eb7123f0fd612 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginGuiPlugin.h
@@ -22,8 +22,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -34,7 +35,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT DebugDrawerGuiPluginGuiPlugin:
+ class ARMARXCOMPONENT_IMPORT_EXPORT DebugDrawerGuiPluginGuiPlugin :
public armarx::ArmarXGuiPlugin
{
Q_OBJECT
@@ -47,4 +48,4 @@ namespace armarx
*/
DebugDrawerGuiPluginGuiPlugin();
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginWidgetController.cpp
index 5aac5e0f7192f3d87fb9b9fd15fc9eadef546b9c..35465ceaa49f4d0ec2bc733fe2553077ce394d46 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginWidgetController.cpp
@@ -20,6 +20,8 @@
* GNU General Public License
*/
+#include "DebugDrawerGuiPluginWidgetController.h"
+
#include <string>
#include <Ice/UUID.h>
@@ -28,19 +30,25 @@
#include <ArmarXCore/util/CPPUtility/trace.h>
-#include "DebugDrawerGuiPluginWidgetController.h"
-
namespace armarx
{
- DebugDrawerGuiPluginWidgetController::DebugDrawerGuiPluginWidgetController():
- _layerName{Ice::generateUUID()},
- _debugDrawer{_layerName}
+ DebugDrawerGuiPluginWidgetController::DebugDrawerGuiPluginWidgetController() :
+ _layerName{Ice::generateUUID()}, _debugDrawer{_layerName}
{
ARMARX_TRACE;
_ui.setupUi(getWidget());
- connect(_ui.pushButtonLayerClear, SIGNAL(clicked()), this, SLOT(on_pushButtonLayerClear_clicked()));
- connect(_ui.pushButtonPoseDelete, SIGNAL(clicked()), this, SLOT(on_pushButtonPoseDelete_clicked()));
- connect(_ui.pushButtonArrowDelete, SIGNAL(clicked()), this, SLOT(on_pushButtonArrowDelete_clicked()));
+ connect(_ui.pushButtonLayerClear,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_pushButtonLayerClear_clicked()));
+ connect(_ui.pushButtonPoseDelete,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_pushButtonPoseDelete_clicked()));
+ connect(_ui.pushButtonArrowDelete,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_pushButtonArrowDelete_clicked()));
connect(_ui.doubleSpinBoxPoseTX, SIGNAL(valueChanged(double)), this, SLOT(updatePose()));
connect(_ui.doubleSpinBoxPoseTY, SIGNAL(valueChanged(double)), this, SLOT(updatePose()));
@@ -56,67 +64,76 @@ namespace armarx
connect(_ui.doubleSpinBoxArrowTX, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
connect(_ui.doubleSpinBoxArrowTY, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
connect(_ui.doubleSpinBoxArrowTZ, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
- connect(_ui.doubleSpinBoxArrowClrR, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
- connect(_ui.doubleSpinBoxArrowClrG, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
- connect(_ui.doubleSpinBoxArrowClrB, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
- connect(_ui.doubleSpinBoxArrowClrA, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
- connect(_ui.doubleSpinBoxArrowWidth, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
+ connect(
+ _ui.doubleSpinBoxArrowClrR, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
+ connect(
+ _ui.doubleSpinBoxArrowClrG, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
+ connect(
+ _ui.doubleSpinBoxArrowClrB, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
+ connect(
+ _ui.doubleSpinBoxArrowClrA, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
+ connect(
+ _ui.doubleSpinBoxArrowWidth, SIGNAL(valueChanged(double)), this, SLOT(updateArrow()));
}
-
DebugDrawerGuiPluginWidgetController::~DebugDrawerGuiPluginWidgetController()
{
-
}
-
- void DebugDrawerGuiPluginWidgetController::loadSettings(QSettings* settings)
+ void
+ DebugDrawerGuiPluginWidgetController::loadSettings(QSettings* settings)
{
}
- void DebugDrawerGuiPluginWidgetController::saveSettings(QSettings* settings)
+ void
+ DebugDrawerGuiPluginWidgetController::saveSettings(QSettings* settings)
{
}
-
- void DebugDrawerGuiPluginWidgetController::onInitComponent()
+ void
+ DebugDrawerGuiPluginWidgetController::onInitComponent()
{
ARMARX_TRACE;
offeringTopic(_debugDrawerTopicName);
}
-
- void DebugDrawerGuiPluginWidgetController::onConnectComponent()
+ void
+ DebugDrawerGuiPluginWidgetController::onConnectComponent()
{
ARMARX_TRACE;
_debugDrawer.setDebugDrawer(getTopic<DebugDrawerInterfacePrx>(_debugDrawerTopicName));
}
- void DebugDrawerGuiPluginWidgetController::onDisconnectComponent()
+ void
+ DebugDrawerGuiPluginWidgetController::onDisconnectComponent()
{
_debugDrawer.clearLayer();
}
- void DebugDrawerGuiPluginWidgetController::on_pushButtonArrowDelete_clicked()
+ void
+ DebugDrawerGuiPluginWidgetController::on_pushButtonArrowDelete_clicked()
{
const auto name = _ui.lineEditArrowName->text().toStdString();
ARMARX_INFO << "Delete arrow " << name;
_debugDrawer.getDebugDrawer()->removePoseVisu(_layerName, name);
}
- void DebugDrawerGuiPluginWidgetController::on_pushButtonPoseDelete_clicked()
+ void
+ DebugDrawerGuiPluginWidgetController::on_pushButtonPoseDelete_clicked()
{
const auto name = _ui.lineEditPoseName->text().toStdString();
ARMARX_INFO << "Delete pose " << name;
_debugDrawer.getDebugDrawer()->removePoseVisu(_layerName, name);
}
- void DebugDrawerGuiPluginWidgetController::on_pushButtonLayerClear_clicked()
+ void
+ DebugDrawerGuiPluginWidgetController::on_pushButtonLayerClear_clicked()
{
_debugDrawer.clearLayer();
}
- void DebugDrawerGuiPluginWidgetController::updatePose()
+ void
+ DebugDrawerGuiPluginWidgetController::updatePose()
{
const auto name = _ui.lineEditPoseName->text().toStdString();
ARMARX_INFO << "Updated pose " << name;
@@ -125,54 +142,41 @@ namespace armarx
_debugDrawer.drawPose(
name,
- VirtualRobot::MathTools::posrpy2eigen4f(
- _ui.doubleSpinBoxPoseTX->value(),
- _ui.doubleSpinBoxPoseTY->value(),
- _ui.doubleSpinBoxPoseTZ->value(),
- _ui.doubleSpinBoxPoseRX->value() * deg2rad,
- _ui.doubleSpinBoxPoseRY->value() * deg2rad,
- _ui.doubleSpinBoxPoseRZ->value() * deg2rad
- ),
+ VirtualRobot::MathTools::posrpy2eigen4f(_ui.doubleSpinBoxPoseTX->value(),
+ _ui.doubleSpinBoxPoseTY->value(),
+ _ui.doubleSpinBoxPoseTZ->value(),
+ _ui.doubleSpinBoxPoseRX->value() * deg2rad,
+ _ui.doubleSpinBoxPoseRY->value() * deg2rad,
+ _ui.doubleSpinBoxPoseRZ->value() * deg2rad),
_ui.doubleSpinBoxPoseScale->value());
}
- void DebugDrawerGuiPluginWidgetController::updateArrow()
+ void
+ DebugDrawerGuiPluginWidgetController::updateArrow()
{
- const Eigen::Vector3f from
- {
- static_cast<float>(_ui.doubleSpinBoxArrowFX->value()),
- static_cast<float>(_ui.doubleSpinBoxArrowFY->value()),
- static_cast<float>(_ui.doubleSpinBoxArrowFZ->value())
- };
-
- const Eigen::Vector3f to
- {
- static_cast<float>(_ui.doubleSpinBoxArrowTX->value()),
- static_cast<float>(_ui.doubleSpinBoxArrowTY->value()),
- static_cast<float>(_ui.doubleSpinBoxArrowTZ->value())
- };
+ const Eigen::Vector3f from{static_cast<float>(_ui.doubleSpinBoxArrowFX->value()),
+ static_cast<float>(_ui.doubleSpinBoxArrowFY->value()),
+ static_cast<float>(_ui.doubleSpinBoxArrowFZ->value())};
+
+ const Eigen::Vector3f to{static_cast<float>(_ui.doubleSpinBoxArrowTX->value()),
+ static_cast<float>(_ui.doubleSpinBoxArrowTY->value()),
+ static_cast<float>(_ui.doubleSpinBoxArrowTZ->value())};
const Eigen::Vector3f dir = to - from;
- const float len = dir.norm();
+ const float len = dir.norm();
const auto name = _ui.lineEditArrowName->text().toStdString();
ARMARX_INFO << "Updated arrow " << name;
- _debugDrawer.drawArrow(
- name,
- from,
- dir.normalized(),
- {
- static_cast<float>(_ui.doubleSpinBoxArrowClrR->value()),
- static_cast<float>(_ui.doubleSpinBoxArrowClrG->value()),
- static_cast<float>(_ui.doubleSpinBoxArrowClrB->value()),
- static_cast<float>(_ui.doubleSpinBoxArrowClrA->value())
- },
- len,
- static_cast<float>(_ui.doubleSpinBoxArrowWidth->value()));
+ _debugDrawer.drawArrow(name,
+ from,
+ dir.normalized(),
+ {static_cast<float>(_ui.doubleSpinBoxArrowClrR->value()),
+ static_cast<float>(_ui.doubleSpinBoxArrowClrG->value()),
+ static_cast<float>(_ui.doubleSpinBoxArrowClrB->value()),
+ static_cast<float>(_ui.doubleSpinBoxArrowClrA->value())},
+ len,
+ static_cast<float>(_ui.doubleSpinBoxArrowWidth->value()));
}
-}
-
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginWidgetController.h b/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginWidgetController.h
index 0e94de05075e4ec5074068cb46733a59d3df99b2..cd7ea98f9f51d575ba14b85e4163b82a5524d7b0 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginWidgetController.h
+++ b/source/RobotAPI/gui-plugins/DebugDrawerGuiPlugin/DebugDrawerGuiPluginWidgetController.h
@@ -21,15 +21,14 @@
*/
#pragma once
-#include <RobotAPI/gui-plugins/DebugDrawerGuiPlugin/ui_DebugDrawerGuiPluginWidget.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
#include <RobotAPI/components/DebugDrawer/DebugDrawerHelper.h>
+#include <RobotAPI/gui-plugins/DebugDrawerGuiPlugin/ui_DebugDrawerGuiPluginWidget.h>
namespace armarx
{
@@ -51,9 +50,8 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- DebugDrawerGuiPluginWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < DebugDrawerGuiPluginWidgetController >
+ class ARMARXCOMPONENT_IMPORT_EXPORT DebugDrawerGuiPluginWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<DebugDrawerGuiPluginWidgetController>
{
Q_OBJECT
@@ -68,7 +66,8 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Visualization.DebugDrawerGuiPlugin";
}
@@ -93,14 +92,11 @@ namespace armarx
*/
Ui::DebugDrawerGuiPluginWidget _ui;
- std::string _layerName;
-
- std::string _debugDrawerTopicName{"DebugDrawerUpdates"};
- DebugDrawerHelper _debugDrawer;
+ std::string _layerName;
- QPointer<SimpleConfigDialog> _dialog;
+ std::string _debugDrawerTopicName{"DebugDrawerUpdates"};
+ DebugDrawerHelper _debugDrawer;
+ QPointer<SimpleConfigDialog> _dialog;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp
index 7180e3d9d23968e74fd485da13545b35b943ca7b..b734a66d9c46a29617fe57ba6bb6b14237dbbf2d 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.cpp
@@ -28,6 +28,6 @@ namespace armarx
{
DebugDrawerViewerGuiPlugin::DebugDrawerViewerGuiPlugin()
{
- addWidget < DebugDrawerViewerWidgetController > ();
+ addWidget<DebugDrawerViewerWidgetController>();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.h
index ffb710f7c20103bf02a3152d88f4da9dee97feb9..bfcdb6d1fb8e7abcab40e24ddfbddc303b30a06a 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerGuiPlugin.h
@@ -23,8 +23,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -35,8 +36,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT DebugDrawerViewerGuiPlugin:
- public armarx::ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT DebugDrawerViewerGuiPlugin : public armarx::ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -48,5 +48,4 @@ namespace armarx
*/
DebugDrawerViewerGuiPlugin();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
index 9c437484cb636ac16705c5a16d5303501a828361..aecc49b612f59bf11a98c25928f9253d19ea382e 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.cpp
@@ -21,14 +21,13 @@
*/
#include "DebugDrawerViewerWidgetController.h"
-#include <ArmarXCore/core/ArmarXManager.h>
-
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <string>
#include <QTimer>
-#include <string>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <ArmarXCore/core/ArmarXManager.h>
namespace armarx
{
@@ -42,36 +41,46 @@ namespace armarx
timer->start(100);
}
-
- void DebugDrawerViewerWidgetController::loadSettings(QSettings* settings)
+ void
+ DebugDrawerViewerWidgetController::loadSettings(QSettings* settings)
{
- (void) settings; // unused
+ (void)settings; // unused
}
- void DebugDrawerViewerWidgetController::saveSettings(QSettings* settings)
+ void
+ DebugDrawerViewerWidgetController::saveSettings(QSettings* settings)
{
- (void) settings; // unused
+ (void)settings; // unused
}
-
- void DebugDrawerViewerWidgetController::onInitComponent()
+ void
+ DebugDrawerViewerWidgetController::onInitComponent()
{
rootVisu = new SoSeparator;
rootVisu->ref();
enableMainWidgetAsync(false);
- connect(widget.btnClearAll, SIGNAL(clicked()), this, SLOT(on_btnClearAll_clicked()), Qt::UniqueConnection);
- connect(widget.btnClearLayer, SIGNAL(clicked()), this, SLOT(on_btnClearLayer_clicked()), Qt::UniqueConnection);
+ connect(widget.btnClearAll,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_btnClearAll_clicked()),
+ Qt::UniqueConnection);
+ connect(widget.btnClearLayer,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_btnClearLayer_clicked()),
+ Qt::UniqueConnection);
}
-
- void DebugDrawerViewerWidgetController::onConnectComponent()
+ void
+ DebugDrawerViewerWidgetController::onConnectComponent()
{
// create the debugdrawer component
std::string debugDrawerComponentName = "GUIDebugDrawer_" + getName();
ARMARX_INFO << "Creating component " << debugDrawerComponentName;
- debugDrawer = Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
+ debugDrawer =
+ Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
if (mutex3D)
{
@@ -93,7 +102,8 @@ namespace armarx
enableMainWidgetAsync(true);
}
- void armarx::DebugDrawerViewerWidgetController::onExitComponent()
+ void
+ armarx::DebugDrawerViewerWidgetController::onExitComponent()
{
if (rootVisu)
{
@@ -103,13 +113,14 @@ namespace armarx
}
}
-
- SoNode* DebugDrawerViewerWidgetController::getScene()
+ SoNode*
+ DebugDrawerViewerWidgetController::getScene()
{
return rootVisu;
}
- void armarx::DebugDrawerViewerWidgetController::on_btnClearAll_clicked()
+ void
+ armarx::DebugDrawerViewerWidgetController::on_btnClearAll_clicked()
{
if (debugDrawer)
{
@@ -118,12 +129,14 @@ namespace armarx
}
}
- void DebugDrawerViewerWidgetController::on_btnClearLayer_clicked()
+ void
+ DebugDrawerViewerWidgetController::on_btnClearLayer_clicked()
{
if (debugDrawer)
{
int index = widget.comboClearLayer->currentIndex();
- std::string layerName = widget.comboClearLayer->itemData(index).toString().toStdString();
+ std::string layerName =
+ widget.comboClearLayer->itemData(index).toString().toStdString();
if (!layerName.empty())
{
@@ -133,8 +146,8 @@ namespace armarx
}
}
-
- void DebugDrawerViewerWidgetController::updateComboClearLayer()
+ void
+ DebugDrawerViewerWidgetController::updateComboClearLayer()
{
QComboBox* combo = widget.comboClearLayer;
@@ -145,7 +158,7 @@ namespace armarx
combo->setFont(font);
};
- auto disableButton = [combo, this, &setItalic](const std::string & hint)
+ auto disableButton = [combo, this, &setItalic](const std::string& hint)
{
QString itemText(hint.c_str());
QString itemData("");
@@ -186,25 +199,28 @@ namespace armarx
}
// sort the layers by name
- std::sort(layers.begin(), layers.end(), [](const LayerInformation & lhs, const LayerInformation & rhs)
- {
- // compare case insensitively
- for (std::size_t i = 0; i < lhs.layerName.size() && i < lhs.layerName.size(); ++i)
- {
- auto lhsLow = std::tolower(lhs.layerName[i]);
- auto rhsLow = std::tolower(rhs.layerName[i]);
- if (lhsLow < rhsLow)
- {
- return true;
- }
- else if (lhsLow > rhsLow)
- {
- return false;
- }
- }
- // if one is a prefix of the other, the shorter one "smaller"
- return lhs.layerName.size() < rhs.layerName.size();
- });
+ std::sort(layers.begin(),
+ layers.end(),
+ [](const LayerInformation& lhs, const LayerInformation& rhs)
+ {
+ // compare case insensitively
+ for (std::size_t i = 0; i < lhs.layerName.size() && i < lhs.layerName.size();
+ ++i)
+ {
+ auto lhsLow = std::tolower(lhs.layerName[i]);
+ auto rhsLow = std::tolower(rhs.layerName[i]);
+ if (lhsLow < rhsLow)
+ {
+ return true;
+ }
+ else if (lhsLow > rhsLow)
+ {
+ return false;
+ }
+ }
+ // if one is a prefix of the other, the shorter one "smaller"
+ return lhs.layerName.size() < rhs.layerName.size();
+ });
const int numLayers = static_cast<int>(layers.size());
@@ -215,7 +231,7 @@ namespace armarx
QString layerName(layer.layerName.c_str());
- if (i < combo->count()) // in range
+ if (i < combo->count()) // in range
{
QString itemData = combo->itemData(i).toString();
@@ -240,7 +256,7 @@ namespace armarx
combo->insertItem(i, makeLayerItemText(layer), layerName);
}
}
- else // out of range
+ else // out of range
{
combo->insertItem(i, makeLayerItemText(layer), layerName);
}
@@ -256,9 +272,8 @@ namespace armarx
}
}
-
-
- QString DebugDrawerViewerWidgetController::makeLayerItemText(const LayerInformation& layer)
+ QString
+ DebugDrawerViewerWidgetController::makeLayerItemText(const LayerInformation& layer)
{
std::vector<std::string> annotations;
if (layer.elementCount == 0)
@@ -276,16 +291,13 @@ namespace armarx
if (annotations.empty())
{
- return { layer.layerName.c_str() };
+ return {layer.layerName.c_str()};
}
else
{
std::stringstream itemText;
- itemText << layer.layerName
- << " (" << simox::alg::join(annotations, ", ") << ")";
- return { itemText.str().c_str() };
+ itemText << layer.layerName << " (" << simox::alg::join(annotations, ", ") << ")";
+ return {itemText.str().c_str()};
}
}
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h
index ac525eccf6f521bce1b4a5d0554b0fe13661d828..fad78b1b64252978a8083694f0c318ddd59e5e5d 100644
--- a/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/DebugDrawerViewer/DebugDrawerViewerWidgetController.h
@@ -22,15 +22,13 @@
#pragma once
-#include <RobotAPI/gui-plugins/DebugDrawerViewer/ui_DebugDrawerViewerWidget.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
-
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
+#include <RobotAPI/gui-plugins/DebugDrawerViewer/ui_DebugDrawerViewerWidget.h>
namespace armarx
{
@@ -51,14 +49,12 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- DebugDrawerViewerWidgetController :
+ class ARMARXCOMPONENT_IMPORT_EXPORT DebugDrawerViewerWidgetController :
public ArmarXComponentWidgetControllerTemplate<DebugDrawerViewerWidgetController>
{
Q_OBJECT
public:
-
/// Controller Constructor
explicit DebugDrawerViewerWidgetController();
@@ -75,7 +71,8 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Visualization.DebugDrawerViewer";
}
@@ -109,13 +106,11 @@ namespace armarx
private:
-
/// Make an item text for a layer entry in the clear layer combo box.
static QString makeLayerItemText(const LayerInformation& layer);
private:
-
/// Widget Form
Ui::DebugDrawerViewerWidget widget;
@@ -130,10 +125,10 @@ namespace armarx
// ArmarXWidgetController interface
public:
- static QIcon GetWidgetIcon()
+ static QIcon
+ GetWidgetIcon()
{
return QIcon(":icons/Outline-3D-DebugDrawer.png");
}
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp
index c895e131e75cc63287f43dc323c600e825734550..83ebe119a58f601fb4af44c3a791c9c0a2e39c3b 100644
--- a/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.cpp
@@ -27,18 +27,22 @@
namespace armarx
{
- void DebugRobotUnitDataStreamingWidgetController::loadSettings(QSettings* settings)
+ void
+ DebugRobotUnitDataStreamingWidgetController::loadSettings(QSettings* settings)
{
getRobotUnitComponentPlugin().setRobotUnitName(
settings->value("ru", "Armar6Unit").toString().toStdString());
}
- void DebugRobotUnitDataStreamingWidgetController::saveSettings(QSettings* settings)
+ void
+ DebugRobotUnitDataStreamingWidgetController::saveSettings(QSettings* settings)
{
- settings->setValue("ru", QString::fromStdString(getRobotUnitComponentPlugin().getRobotUnitName()));
+ settings->setValue(
+ "ru", QString::fromStdString(getRobotUnitComponentPlugin().getRobotUnitName()));
}
- QPointer<QDialog> DebugRobotUnitDataStreamingWidgetController::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ DebugRobotUnitDataStreamingWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
@@ -48,7 +52,8 @@ namespace armarx
return qobject_cast<SimpleConfigDialog*>(dialog);
}
- void DebugRobotUnitDataStreamingWidgetController::configured()
+ void
+ DebugRobotUnitDataStreamingWidgetController::configured()
{
getRobotUnitComponentPlugin().setRobotUnitName(dialog->get("ru"));
}
@@ -59,13 +64,15 @@ namespace armarx
startTimer(10);
}
- void DebugRobotUnitDataStreamingWidgetController::onDisconnectComponent()
+ void
+ DebugRobotUnitDataStreamingWidgetController::onDisconnectComponent()
{
std::lock_guard f{mutex};
rec.clear();
}
- void DebugRobotUnitDataStreamingWidgetController::timerEvent(QTimerEvent* event)
+ void
+ DebugRobotUnitDataStreamingWidgetController::timerEvent(QTimerEvent* event)
{
const std::size_t n = widget.spinBoxNumberOfStreams->value();
if (!getRobotUnit())
@@ -88,8 +95,7 @@ namespace armarx
}
while (rec.size() < n)
{
- rec.emplace_back(getRobotUnitComponentPlugin()
- .startDataStreaming(cfg));
+ rec.emplace_back(getRobotUnitComponentPlugin().startDataStreaming(cfg));
ARMARX_INFO << rec.back()->getDataDescriptionString();
}
for (auto& r : rec)
@@ -97,4 +103,4 @@ namespace armarx
r->getDataBuffer().clear();
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.h b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.h
index 430af82b3b31ed6c955cd3ab851152fafcf14890..e40548271d71eded7149199d2ee0bb1c7dfd9e15 100644
--- a/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.h
+++ b/source/RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/DebugRobotUnitDataStreamingWidgetController.h
@@ -23,9 +23,9 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
#include <RobotAPI/gui-plugins/DebugRobotUnitDataStreaming/ui_DebugRobotUnitDataStreamingWidget.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
@@ -59,9 +59,9 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- DebugRobotUnitDataStreamingWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < DebugRobotUnitDataStreamingWidgetController >,
+ class ARMARXCOMPONENT_IMPORT_EXPORT DebugRobotUnitDataStreamingWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<
+ DebugRobotUnitDataStreamingWidgetController>,
public virtual RobotUnitComponentPluginUser
{
Q_OBJECT
@@ -77,25 +77,36 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Debugging.DebugRobotUnitDataStreaming";
}
- void onInitComponent() override {}
- void onConnectComponent() override {}
+ void
+ onInitComponent() override
+ {
+ }
+
+ void
+ onConnectComponent() override
+ {
+ }
+
void onDisconnectComponent() override;
- void onExitComponent() override {}
+
+ void
+ onExitComponent() override
+ {
+ }
protected:
void timerEvent(QTimerEvent* event) override;
private:
- std::mutex mutex;
- Ui::DebugRobotUnitDataStreamingWidget widget;
+ std::mutex mutex;
+ Ui::DebugRobotUnitDataStreamingWidget widget;
std::vector<RobotUnitDataStreamingReceiverPtr> rec;
- QPointer<SimpleConfigDialog> dialog;
+ QPointer<SimpleConfigDialog> dialog;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp
index 6cc8d730eec3f7c083e46ff1f5c6b77300cce16f..61199918f5c874936d4a3e00cdc0a028c42d6c78 100644
--- a/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.cpp
@@ -19,38 +19,50 @@
* \copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+#include "FTSensorCalibrationGuiWidgetController.h"
+
#include <string>
#include <QRegExp>
+
#include <VirtualRobot/RobotNodeSet.h>
-#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
-
-#include "FTSensorCalibrationGuiWidgetController.h"
+#include <ArmarXCore/util/CPPUtility/trace.h>
//boilerplate config
namespace armarx
{
- void FTSensorCalibrationGuiWidgetController::loadSettings(QSettings* settings)
+ void
+ FTSensorCalibrationGuiWidgetController::loadSettings(QSettings* settings)
{
ARMARX_TRACE;
std::lock_guard g{_all_mutex};
- getRobotStateComponentPlugin().setRobotStateComponentName(settings->value("rsc", "Armar6StateComponent").toString().toStdString());
- getRobotUnitComponentPlugin().setRobotUnitName(settings->value("ru", "Armar6Unit").toString().toStdString());
- getDebugObserverComponentPlugin().setDebugObserverTopic(settings->value("dbgo", "DebugObserver").toString().toStdString());
+ getRobotStateComponentPlugin().setRobotStateComponentName(
+ settings->value("rsc", "Armar6StateComponent").toString().toStdString());
+ getRobotUnitComponentPlugin().setRobotUnitName(
+ settings->value("ru", "Armar6Unit").toString().toStdString());
+ getDebugObserverComponentPlugin().setDebugObserverTopic(
+ settings->value("dbgo", "DebugObserver").toString().toStdString());
}
- void FTSensorCalibrationGuiWidgetController::saveSettings(QSettings* settings)
+ void
+ FTSensorCalibrationGuiWidgetController::saveSettings(QSettings* settings)
{
ARMARX_TRACE;
std::lock_guard g{_all_mutex};
- settings->setValue("rsc", QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
- settings->setValue("ru", QString::fromStdString(getRobotUnitComponentPlugin().getRobotUnitName()));
- settings->setValue("dbgo", QString::fromStdString(getDebugObserverComponentPlugin().getDebugObserverTopic()));
+ settings->setValue(
+ "rsc",
+ QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
+ settings->setValue(
+ "ru", QString::fromStdString(getRobotUnitComponentPlugin().getRobotUnitName()));
+ settings->setValue(
+ "dbgo",
+ QString::fromStdString(getDebugObserverComponentPlugin().getDebugObserverTopic()));
}
- QPointer<QDialog> FTSensorCalibrationGuiWidgetController::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ FTSensorCalibrationGuiWidgetController::getConfigDialog(QWidget* parent)
{
ARMARX_TRACE;
std::lock_guard g{_all_mutex};
@@ -58,13 +70,15 @@ namespace armarx
{
_dialog = new SimpleConfigDialog(parent);
_dialog->addProxyFinder<RobotUnitInterfacePrx>("ru", "Robot Unit", "*Unit");
- _dialog->addProxyFinder<RobotStateComponentInterfacePrx>("rsc", "Robot State Component", "*Component");
+ _dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
+ "rsc", "Robot State Component", "*Component");
_dialog->addLineEdit("dbgo", "DebugObserver Topic", "DebugObserver");
}
return qobject_cast<SimpleConfigDialog*>(_dialog);
}
- void FTSensorCalibrationGuiWidgetController::configured()
+ void
+ FTSensorCalibrationGuiWidgetController::configured()
{
ARMARX_TRACE;
std::lock_guard g{_all_mutex};
@@ -73,7 +87,8 @@ namespace armarx
getDebugObserverComponentPlugin().setDebugObserverTopic(_dialog->get("dbgo"));
}
-}
+} // namespace armarx
+
namespace armarx
{
FTSensorCalibrationGuiWidgetController::FTSensorCalibrationGuiWidgetController()
@@ -81,47 +96,71 @@ namespace armarx
_widget.setupUi(getWidget());
_widget.pushButtonLoadCalibFromFile->setVisible(false);
- connect(_widget.pushButtonLoadCalibFromFile, &QPushButton::clicked,
- this, &FTSensorCalibrationGuiWidgetController::loadCalibFromFile);
- connect(_widget.pushButtonStart, &QPushButton::clicked,
- this, &FTSensorCalibrationGuiWidgetController::startRecording);
- connect(_widget.pushButtonStop, &QPushButton::clicked,
- this, &FTSensorCalibrationGuiWidgetController::stopRecording);
- connect(_widget.pushButtonLoadDefaultArmar6FTL, &QPushButton::clicked,
- this, &FTSensorCalibrationGuiWidgetController::loadDefaultArmar6FTL);
- connect(_widget.pushButtonLoadDefaultArmar6FTR, &QPushButton::clicked,
- this, &FTSensorCalibrationGuiWidgetController::loadDefaultArmar6FTR);
-
- connect(_widget.tableWidgetOffset, &QTableWidget::itemChanged,
- this, &FTSensorCalibrationGuiWidgetController::updateCalibration);
- connect(_widget.tableWidgetOffset, &QTableWidget::itemChanged,
- this, &FTSensorCalibrationGuiWidgetController::updateCalibration);
- connect(_widget.tableWidgetChannelOrder, &QTableWidget::itemChanged,
- this, &FTSensorCalibrationGuiWidgetController::updateCalibration);
- connect(_widget.doubleSpinBoxTickToVolt, qOverload<double>(&QDoubleSpinBox::valueChanged),
- this, &FTSensorCalibrationGuiWidgetController::updateCalibration);
- connect(_widget.doubleSpinBoxTickToVoltE, qOverload<int>(&QSpinBox::valueChanged),
- this, &FTSensorCalibrationGuiWidgetController::updateCalibration);
-
- connect(_widget.tableWidgetCompensate, &QTableWidget::itemChanged,
- this, &FTSensorCalibrationGuiWidgetController::updateCompensation);
+ connect(_widget.pushButtonLoadCalibFromFile,
+ &QPushButton::clicked,
+ this,
+ &FTSensorCalibrationGuiWidgetController::loadCalibFromFile);
+ connect(_widget.pushButtonStart,
+ &QPushButton::clicked,
+ this,
+ &FTSensorCalibrationGuiWidgetController::startRecording);
+ connect(_widget.pushButtonStop,
+ &QPushButton::clicked,
+ this,
+ &FTSensorCalibrationGuiWidgetController::stopRecording);
+ connect(_widget.pushButtonLoadDefaultArmar6FTL,
+ &QPushButton::clicked,
+ this,
+ &FTSensorCalibrationGuiWidgetController::loadDefaultArmar6FTL);
+ connect(_widget.pushButtonLoadDefaultArmar6FTR,
+ &QPushButton::clicked,
+ this,
+ &FTSensorCalibrationGuiWidgetController::loadDefaultArmar6FTR);
+
+ connect(_widget.tableWidgetOffset,
+ &QTableWidget::itemChanged,
+ this,
+ &FTSensorCalibrationGuiWidgetController::updateCalibration);
+ connect(_widget.tableWidgetOffset,
+ &QTableWidget::itemChanged,
+ this,
+ &FTSensorCalibrationGuiWidgetController::updateCalibration);
+ connect(_widget.tableWidgetChannelOrder,
+ &QTableWidget::itemChanged,
+ this,
+ &FTSensorCalibrationGuiWidgetController::updateCalibration);
+ connect(_widget.doubleSpinBoxTickToVolt,
+ qOverload<double>(&QDoubleSpinBox::valueChanged),
+ this,
+ &FTSensorCalibrationGuiWidgetController::updateCalibration);
+ connect(_widget.doubleSpinBoxTickToVoltE,
+ qOverload<int>(&QSpinBox::valueChanged),
+ this,
+ &FTSensorCalibrationGuiWidgetController::updateCalibration);
+
+ connect(_widget.tableWidgetCompensate,
+ &QTableWidget::itemChanged,
+ this,
+ &FTSensorCalibrationGuiWidgetController::updateCompensation);
loadDefaultArmar6FTR();
startTimer(10);
}
-
FTSensorCalibrationGuiWidgetController::~FTSensorCalibrationGuiWidgetController()
- {}
+ {
+ }
- void FTSensorCalibrationGuiWidgetController::onConnectComponent()
+ void
+ FTSensorCalibrationGuiWidgetController::onConnectComponent()
{
ARMARX_TRACE;
std::lock_guard g{_all_mutex};
_robot = addOrGetRobot("r", VirtualRobot::RobotIO::RobotDescription::eStructure);
}
- void FTSensorCalibrationGuiWidgetController::onDisconnectComponent()
+ void
+ FTSensorCalibrationGuiWidgetController::onDisconnectComponent()
{
ARMARX_TRACE;
std::lock_guard g{_all_mutex};
@@ -130,7 +169,8 @@ namespace armarx
_robot = nullptr;
}
- void FTSensorCalibrationGuiWidgetController::timerEvent(QTimerEvent* event)
+ void
+ FTSensorCalibrationGuiWidgetController::timerEvent(QTimerEvent* event)
{
ARMARX_TRACE;
std::lock_guard g{_all_mutex};
@@ -139,7 +179,7 @@ namespace armarx
_widget.widgetFields->setEnabled(false);
return;
}
- const auto to_list = [](const auto & container)
+ const auto to_list = [](const auto& container)
{
QStringList qnames;
for (const auto& name : container)
@@ -155,7 +195,7 @@ namespace armarx
_all_logging_names = std::set<std::string>(vec.begin(), vec.end());
}
const auto qnames = to_list(_all_logging_names);
- const auto setup = [&](auto box, const auto & lst, auto regex)
+ const auto setup = [&](auto box, const auto& lst, auto regex)
{
box->clear();
box->addItems(lst);
@@ -179,9 +219,8 @@ namespace armarx
setup(_widget.comboBoxADC5Temp, qnames, R"(^sens\.FT R\.adc_4_to_6_temperature$)");
setup(_widget.comboBoxADC6Temp, qnames, R"(^sens\.FT R\.adc_4_to_6_temperature$)");
- setup(_widget.comboBoxKinematicChain,
- to_list(_robot->getRobotNodeSetNames()),
- "^Robot$");
+ setup(
+ _widget.comboBoxKinematicChain, to_list(_robot->getRobotNodeSetNames()), "^Robot$");
_widget.pushButtonStart->setEnabled(true);
_widget.widgetFields->setEnabled(true);
@@ -196,12 +235,12 @@ namespace armarx
_widget.comboBoxADC5Value->setEnabled(!_recording);
_widget.comboBoxADC6Value->setEnabled(!_recording);
- _widget.comboBoxADC1Temp ->setEnabled(!_recording);
- _widget.comboBoxADC2Temp ->setEnabled(!_recording);
- _widget.comboBoxADC3Temp ->setEnabled(!_recording);
- _widget.comboBoxADC4Temp ->setEnabled(!_recording);
- _widget.comboBoxADC5Temp ->setEnabled(!_recording);
- _widget.comboBoxADC6Temp ->setEnabled(!_recording);
+ _widget.comboBoxADC1Temp->setEnabled(!_recording);
+ _widget.comboBoxADC2Temp->setEnabled(!_recording);
+ _widget.comboBoxADC3Temp->setEnabled(!_recording);
+ _widget.comboBoxADC4Temp->setEnabled(!_recording);
+ _widget.comboBoxADC5Temp->setEnabled(!_recording);
+ _widget.comboBoxADC6Temp->setEnabled(!_recording);
if (!_recording)
{
@@ -215,10 +254,7 @@ namespace armarx
long time_compensation_us = 0;
if (!data.empty())
{
- const auto log = [&](auto n)
- {
- _logfile << ';' << n;
- };
+ const auto log = [&](auto n) { _logfile << ';' << n; };
for (const auto& s : data)
{
_logfile << s.iterationId << ';' << s.timestampUSec;
@@ -231,10 +267,7 @@ namespace armarx
std::vector<float> vals;
{
const auto& last = data.back();
- const auto pback = [&](auto n)
- {
- vals.emplace_back(n);
- };
+ const auto pback = [&](auto n) { vals.emplace_back(n); };
RobotUnitDataStreamingReceiver::VisitEntries(pback, last, _adc);
RobotUnitDataStreamingReceiver::VisitEntries(pback, last, _adc_temp);
}
@@ -263,12 +296,12 @@ namespace armarx
_widget.labelADC5Value->setText(QString::number(raw_ft_unordered(4)));
_widget.labelADC6Value->setText(QString::number(raw_ft_unordered(5)));
- _widget.labelADC1Temp ->setText(QString::number(temp_unordered(0)));
- _widget.labelADC2Temp ->setText(QString::number(temp_unordered(1)));
- _widget.labelADC3Temp ->setText(QString::number(temp_unordered(2)));
- _widget.labelADC4Temp ->setText(QString::number(temp_unordered(3)));
- _widget.labelADC5Temp ->setText(QString::number(temp_unordered(4)));
- _widget.labelADC6Temp ->setText(QString::number(temp_unordered(5)));
+ _widget.labelADC1Temp->setText(QString::number(temp_unordered(0)));
+ _widget.labelADC2Temp->setText(QString::number(temp_unordered(1)));
+ _widget.labelADC3Temp->setText(QString::number(temp_unordered(2)));
+ _widget.labelADC4Temp->setText(QString::number(temp_unordered(3)));
+ _widget.labelADC5Temp->setText(QString::number(temp_unordered(4)));
+ _widget.labelADC6Temp->setText(QString::number(temp_unordered(5)));
const auto do_compensation = [&]
{
@@ -276,10 +309,11 @@ namespace armarx
for (int i = 0; i < 6; ++i)
{
raw_ft_unordered(i) =
- _comp_table.compensate(temp_unordered(i), raw_ft_unordered(i));
+ _comp_table.compensate(temp_unordered(i), raw_ft_unordered(i));
}
const auto dt = std::chrono::high_resolution_clock::now() - st;
- time_compensation_us = std::chrono::duration_cast<std::chrono::microseconds>(dt).count();
+ time_compensation_us =
+ std::chrono::duration_cast<std::chrono::microseconds>(dt).count();
};
if (_widget.radioButtonCompBeforeOffset->isChecked())
@@ -317,32 +351,27 @@ namespace armarx
sendDebugObserverBatch();
}
- void FTSensorCalibrationGuiWidgetController::startRecording()
+ void
+ FTSensorCalibrationGuiWidgetController::startRecording()
{
ARMARX_TRACE;
std::lock_guard g{_all_mutex};
_widget.pushButtonStart->setEnabled(false);
_widget.pushButtonStop->setEnabled(true);
- const std::array field_adc_value =
- {
- _widget.comboBoxADC1Value->currentText().toStdString(),
- _widget.comboBoxADC2Value->currentText().toStdString(),
- _widget.comboBoxADC3Value->currentText().toStdString(),
- _widget.comboBoxADC4Value->currentText().toStdString(),
- _widget.comboBoxADC5Value->currentText().toStdString(),
- _widget.comboBoxADC6Value->currentText().toStdString()
- };
+ const std::array field_adc_value = {_widget.comboBoxADC1Value->currentText().toStdString(),
+ _widget.comboBoxADC2Value->currentText().toStdString(),
+ _widget.comboBoxADC3Value->currentText().toStdString(),
+ _widget.comboBoxADC4Value->currentText().toStdString(),
+ _widget.comboBoxADC5Value->currentText().toStdString(),
+ _widget.comboBoxADC6Value->currentText().toStdString()};
- const std::array field_adc_temp =
- {
- _widget.comboBoxADC1Temp ->currentText().toStdString(),
- _widget.comboBoxADC2Temp ->currentText().toStdString(),
- _widget.comboBoxADC3Temp ->currentText().toStdString(),
- _widget.comboBoxADC4Temp ->currentText().toStdString(),
- _widget.comboBoxADC5Temp ->currentText().toStdString(),
- _widget.comboBoxADC6Temp ->currentText().toStdString()
- };
+ const std::array field_adc_temp = {_widget.comboBoxADC1Temp->currentText().toStdString(),
+ _widget.comboBoxADC2Temp->currentText().toStdString(),
+ _widget.comboBoxADC3Temp->currentText().toStdString(),
+ _widget.comboBoxADC4Temp->currentText().toStdString(),
+ _widget.comboBoxADC5Temp->currentText().toStdString(),
+ _widget.comboBoxADC6Temp->currentText().toStdString()};
//setup streaming
@@ -368,7 +397,7 @@ namespace armarx
///TODO
//filter by rns
- const auto rns_name = _widget.comboBoxKinematicChain ->currentText().toStdString();
+ const auto rns_name = _widget.comboBoxKinematicChain->currentText().toStdString();
ARMARX_CHECK_EXPRESSION(_robot->hasRobotNodeSet(rns_name)) << VAROUT(rns_name);
for (const auto& name : _robot->getRobotNodeSet(rns_name)->getNodeNames())
{
@@ -387,9 +416,11 @@ namespace armarx
auto entries = _streaming_handler->getDataDescription().entries;
for (std::size_t i = 0; i < 6; ++i)
{
- ARMARX_CHECK_EXPRESSION(entries.count(field_adc_value.at(i))) << VAROUT(field_adc_value.at(i));
- ARMARX_CHECK_EXPRESSION(entries.count(field_adc_temp.at(i))) << VAROUT(field_adc_temp.at(i));
- _adc.at(i) = entries.at(field_adc_value.at(i));
+ ARMARX_CHECK_EXPRESSION(entries.count(field_adc_value.at(i)))
+ << VAROUT(field_adc_value.at(i));
+ ARMARX_CHECK_EXPRESSION(entries.count(field_adc_temp.at(i)))
+ << VAROUT(field_adc_temp.at(i));
+ _adc.at(i) = entries.at(field_adc_value.at(i));
_adc_temp.at(i) = entries.at(field_adc_temp.at(i));
}
for (std::size_t i = 0; i < 6; ++i)
@@ -398,14 +429,17 @@ namespace armarx
entries.erase(field_adc_temp.at(i));
}
- const std::filesystem::path path = FileSystemPathBuilder::ApplyFormattingAndResolveEnvAndCMakeVars(_widget.lineEditLogFile->text().toStdString());
+ const std::filesystem::path path =
+ FileSystemPathBuilder::ApplyFormattingAndResolveEnvAndCMakeVars(
+ _widget.lineEditLogFile->text().toStdString());
if (path.has_parent_path())
{
std::filesystem::create_directories(path.parent_path());
}
ARMARX_INFO << "Logging to " << path;
_logfile.open(path.string());
- _logfile << "iteration;timestamp;adc1;adc2;adc3;adc4;adc5;acd1_temp;acd2_temp;acd3_temp;acd4_temp;acd5_temp";
+ _logfile << "iteration;timestamp;adc1;adc2;adc3;adc4;adc5;acd1_temp;acd2_temp;acd3_"
+ "temp;acd4_temp;acd5_temp";
for (const auto& [name, entry] : entries)
{
@@ -417,7 +451,9 @@ namespace armarx
_recording = true;
}
- void FTSensorCalibrationGuiWidgetController::stopRecording()
+
+ void
+ FTSensorCalibrationGuiWidgetController::stopRecording()
{
ARMARX_TRACE;
std::lock_guard g{_all_mutex};
@@ -428,39 +464,43 @@ namespace armarx
_logfile = std::ofstream{};
}
- template<class T>
- T str_to_val(const auto& str)
+ template <class T>
+ T
+ str_to_val(const auto& str)
{
-#define option(type, fn) if constexpr(std::is_same_v<T, type>) { return str.fn() ; } else
- option(double, toDouble)
- option(float, toFloat)
- option(int, toInt)
- option(long, toLong)
- option(qlonglong, toLongLong)
- option(short, toShort)
- option(uint, toUInt)
- option(ulong, toULong)
- option(qulonglong, toULongLong)
- option(ushort, toUShort)
+#define option(type, fn) \
+ if constexpr (std::is_same_v<T, type>) \
+ { \
+ return str.fn(); \
+ } \
+ else
+ option(double, toDouble) option(float, toFloat) option(int, toInt) option(long, toLong)
+ option(qlonglong, toLongLong) option(short, toShort) option(uint, toUInt)
+ option(ulong, toULong) option(qulonglong, toULongLong) option(ushort, toUShort)
{
- static_assert(!std::is_same_v<float_t, float_t>, "this function can't handle the given type");
+ static_assert(!std::is_same_v<float_t, float_t>,
+ "this function can't handle the given type");
}
#undef option
}
- template<class T>
- void str_to_val(const auto& str, T& val)
+ template <class T>
+ void
+ str_to_val(const auto& str, T& val)
{
val = str_to_val<T>(str);
}
- template<class T>
- T str_to_val_with_opt_on_empty(const auto& str, T opt)
+ template <class T>
+ T
+ str_to_val_with_opt_on_empty(const auto& str, T opt)
{
- return str.isEmpty() ? opt : str_to_val<T>(str);;
+ return str.isEmpty() ? opt : str_to_val<T>(str);
+ ;
}
- void read(auto& eigen, auto* table)
+ void
+ read(auto& eigen, auto* table)
{
for (int c = 0; c < eigen.cols(); ++c)
{
@@ -478,7 +518,8 @@ namespace armarx
}
}
- void FTSensorCalibrationGuiWidgetController::updateCalibration()
+ void
+ FTSensorCalibrationGuiWidgetController::updateCalibration()
{
ARMARX_TRACE;
//read all tables:
@@ -489,7 +530,8 @@ namespace armarx
std::pow(10.0, 1.0 * _widget.doubleSpinBoxTickToVoltE->value());
}
- void FTSensorCalibrationGuiWidgetController::updateCompensation()
+ void
+ FTSensorCalibrationGuiWidgetController::updateCompensation()
{
ARMARX_TRACE;
_comp_table.table.clear();
@@ -509,23 +551,22 @@ namespace armarx
continue;
}
- _comp_table.table.emplace_back(std::array
- {
- str_to_val<float>(c0),
- str_to_val_with_opt_on_empty<float>(read_cell(1), 0),
- str_to_val_with_opt_on_empty<float>(read_cell(2), 0)
- });
+ _comp_table.table.emplace_back(
+ std::array{str_to_val<float>(c0),
+ str_to_val_with_opt_on_empty<float>(read_cell(1), 0),
+ str_to_val_with_opt_on_empty<float>(read_cell(2), 0)});
std::sort(_comp_table.table.begin(), _comp_table.table.end());
}
}
- void FTSensorCalibrationGuiWidgetController::loadCalibFromFile()
+ void
+ FTSensorCalibrationGuiWidgetController::loadCalibFromFile()
{
ARMARX_TRACE;
-
}
- void FTSensorCalibrationGuiWidgetController::loadDefaultArmar6FTR()
+ void
+ FTSensorCalibrationGuiWidgetController::loadDefaultArmar6FTR()
{
ARMARX_TRACE;
const auto set = [&](auto tab, auto row, auto col, auto str)
@@ -595,7 +636,8 @@ namespace armarx
_widget.doubleSpinBoxTickToVoltE->setValue(-12);
}
- void FTSensorCalibrationGuiWidgetController::loadDefaultArmar6FTL()
+ void
+ FTSensorCalibrationGuiWidgetController::loadDefaultArmar6FTL()
{
ARMARX_TRACE;
const auto set = [&](auto tab, auto row, auto col, auto str)
@@ -664,4 +706,4 @@ namespace armarx
_widget.doubleSpinBoxTickToVolt->setValue(-7.275957614183426);
_widget.doubleSpinBoxTickToVoltE->setValue(-12);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.h b/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.h
index 8c69592b4d3b18b4dca7877fe4a8de4264ba855e..2c433b6b89d32806b832f1fb915235f9e10d1799 100644
--- a/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/FTSensorCalibrationGui/FTSensorCalibrationGuiWidgetController.h
@@ -23,23 +23,22 @@
#include <fstream>
-#include <RobotAPI/gui-plugins/FTSensorCalibrationGui/ui_FTSensorCalibrationGuiWidget.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
-#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+#include <RobotAPI/gui-plugins/FTSensorCalibrationGui/ui_FTSensorCalibrationGuiWidget.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
//compensation_table
namespace armarx
{
- template<class T>
+ template <class T>
struct compensation_table
{
using value_t = T;
@@ -47,16 +46,18 @@ namespace armarx
std::vector<entry_t> table;
value_t compensate(value_t decision, value_t value) const;
+
private:
value_t apply(const entry_t& f, value_t value) const;
- value_t interpolate(value_t decision, const entry_t& lo, const entry_t& hi, value_t value) const;
+ value_t
+ interpolate(value_t decision, const entry_t& lo, const entry_t& hi, value_t value) const;
mutable std::size_t last_idx = 0;
- mutable entry_t search_dummy;
+ mutable entry_t search_dummy;
};
- template<class T> inline
- typename compensation_table<T>::value_t
+ template <class T>
+ inline typename compensation_table<T>::value_t
compensation_table<T>::compensate(value_t decision, value_t value) const
{
if (table.empty())
@@ -65,12 +66,11 @@ namespace armarx
}
search_dummy.at(0) = decision;
- const auto lower = std::lower_bound(
- table.begin(), table.end(), decision,
- [](const entry_t& e, value_t v)
- {
- return e.at(0) < v;
- });
+ const auto lower =
+ std::lower_bound(table.begin(),
+ table.end(),
+ decision,
+ [](const entry_t& e, value_t v) { return e.at(0) < v; });
if (lower == table.end())
{
return apply(table.back(), value);
@@ -82,23 +82,27 @@ namespace armarx
//interpolate
return interpolate(decision, *(lower - 1), *lower, value);
}
- template<class T> inline
- typename compensation_table<T>::value_t
+
+ template <class T>
+ inline typename compensation_table<T>::value_t
compensation_table<T>::apply(const entry_t& f, value_t value) const
{
return f.at(1) * value + f.at(2);
}
- template<class T> inline
- typename compensation_table<T>::value_t
- compensation_table<T>::interpolate(value_t decision, const entry_t& lo,
- const entry_t& hi, value_t value) const
+
+ template <class T>
+ inline typename compensation_table<T>::value_t
+ compensation_table<T>::interpolate(value_t decision,
+ const entry_t& lo,
+ const entry_t& hi,
+ value_t value) const
{
const value_t t = (decision - lo.at(0)) / (hi.at(0) - lo.at(0));
const value_t a = lo.at(1) * (1 - t) + hi.at(1) * t;
const value_t b = lo.at(2) * (1 - t) + hi.at(2) * t;
return a * value + b;
}
-}
+} // namespace armarx
namespace armarx
{
@@ -120,9 +124,9 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- FTSensorCalibrationGuiWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < FTSensorCalibrationGuiWidgetController >,
+ class ARMARXCOMPONENT_IMPORT_EXPORT FTSensorCalibrationGuiWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<
+ FTSensorCalibrationGuiWidgetController>,
public virtual RobotUnitComponentPluginUser,
public virtual RobotStateComponentPluginUser,
public virtual DebugObserverComponentPluginUser
@@ -142,15 +146,24 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Debugging.FTSensorCalibrationGui";
}
- void onInitComponent() override {}
- void onConnectComponent() override;
+ void
+ onInitComponent() override
+ {
+ }
+
+ void onConnectComponent() override;
void onDisconnectComponent() override;
- void onExitComponent() override {}
+
+ void
+ onExitComponent() override
+ {
+ }
protected:
void timerEvent(QTimerEvent* event) override;
@@ -165,27 +178,25 @@ namespace armarx
void loadDefaultArmar6FTR();
private:
- QPointer<SimpleConfigDialog> _dialog;
- mutable std::recursive_mutex _all_mutex;
- VirtualRobot::RobotPtr _robot;
- Ui::FTSensorCalibrationGuiWidget _widget;
- bool _recording = false;
- bool _comboboxes_are_set_up = false;
- std::set<std::string> _all_logging_names;
-
- Eigen::Matrix6f _conversion_matrix;
- Eigen::Vector6f _offset;
- Eigen::Vector6i _channel_order;
- float _ticks_to_volt_factor;
+ QPointer<SimpleConfigDialog> _dialog;
+ mutable std::recursive_mutex _all_mutex;
+ VirtualRobot::RobotPtr _robot;
+ Ui::FTSensorCalibrationGuiWidget _widget;
+ bool _recording = false;
+ bool _comboboxes_are_set_up = false;
+ std::set<std::string> _all_logging_names;
+
+ Eigen::Matrix6f _conversion_matrix;
+ Eigen::Vector6f _offset;
+ Eigen::Vector6i _channel_order;
+ float _ticks_to_volt_factor;
RobotUnitDataStreamingReceiverPtr _streaming_handler;
std::array<RobotUnitDataStreaming::DataEntry, 6> _adc;
std::array<RobotUnitDataStreaming::DataEntry, 6> _adc_temp;
- std::vector<RobotUnitDataStreaming::DataEntry> _joints;
- std::ofstream _logfile;
+ std::vector<RobotUnitDataStreaming::DataEntry> _joints;
+ std::ofstream _logfile;
compensation_table<float> _comp_table;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.cpp
index 0cddd8405f669bf3b65ddd1e3c56d40317a4ff5d..a17a9b453f02ea6c447debb26038a1f5b75baa00 100644
--- a/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.cpp
@@ -20,52 +20,69 @@
* GNU General Public License
*/
+#include "GraspCandidateViewerWidgetController.h"
+
#include <string>
#include <SimoxUtility/math/convert/quat_to_rpy.h>
#include <RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h>
-#include "GraspCandidateViewerWidgetController.h"
//setting management
namespace armarx
{
- void GraspCandidateViewerWidgetController::loadSettings(QSettings* settings)
+ void
+ GraspCandidateViewerWidgetController::loadSettings(QSettings* settings)
{
ARMARX_TRACE;
std::lock_guard g{_mutex};
- getRobotStateComponentPlugin().setRobotStateComponentName(settings->value("rsc", "Armar6StateComponent").toString().toStdString());
- getGraspCandidateObserverComponentPlugin().setGraspCandidateObserverName(settings->value("gco", "GraspCandidateObserver").toString().toStdString());
-
+ getRobotStateComponentPlugin().setRobotStateComponentName(
+ settings->value("rsc", "Armar6StateComponent").toString().toStdString());
+ getGraspCandidateObserverComponentPlugin().setGraspCandidateObserverName(
+ settings->value("gco", "GraspCandidateObserver").toString().toStdString());
}
- void GraspCandidateViewerWidgetController::saveSettings(QSettings* settings)
+
+ void
+ GraspCandidateViewerWidgetController::saveSettings(QSettings* settings)
{
ARMARX_TRACE;
std::lock_guard g{_mutex};
- settings->setValue("rsc", QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
- settings->setValue("rsc", QString::fromStdString(getGraspCandidateObserverComponentPlugin().getGraspCandidateObserverName()));
-
+ settings->setValue(
+ "rsc",
+ QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
+ settings->setValue(
+ "rsc",
+ QString::fromStdString(
+ getGraspCandidateObserverComponentPlugin().getGraspCandidateObserverName()));
}
- QPointer<QDialog> GraspCandidateViewerWidgetController::getConfigDialog(QWidget* parent)
+
+ QPointer<QDialog>
+ GraspCandidateViewerWidgetController::getConfigDialog(QWidget* parent)
{
ARMARX_TRACE;
std::lock_guard g{_mutex};
if (!_dialog)
{
_dialog = new SimpleConfigDialog(parent);
- _dialog->addProxyFinder<RobotStateComponentInterfacePrx>("rsc", "Robot State Component", "*Component");
- _dialog->addProxyFinder<grasping::GraspCandidateObserverInterfacePrx>("gco", "Grasp Candidate Observer", "*Observer");
+ _dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
+ "rsc", "Robot State Component", "*Component");
+ _dialog->addProxyFinder<grasping::GraspCandidateObserverInterfacePrx>(
+ "gco", "Grasp Candidate Observer", "*Observer");
}
return qobject_cast<SimpleConfigDialog*>(_dialog);
}
- void GraspCandidateViewerWidgetController::configured()
+
+ void
+ GraspCandidateViewerWidgetController::configured()
{
ARMARX_TRACE;
std::lock_guard g{_mutex};
getRobotStateComponentPlugin().setRobotStateComponentName(_dialog->getProxyName("rsc"));
- getGraspCandidateObserverComponentPlugin().setGraspCandidateObserverName(_dialog->getProxyName("gco"));
+ getGraspCandidateObserverComponentPlugin().setGraspCandidateObserverName(
+ _dialog->getProxyName("gco"));
}
-}
+} // namespace armarx
+
//ctor
namespace armarx
{
@@ -75,19 +92,27 @@ namespace armarx
std::lock_guard g{_mutex};
_ui.setupUi(getWidget());
- connect(_ui.pushButtonUpdateGC, &QPushButton::clicked,
- this, &GraspCandidateViewerWidgetController::update_gc);
- connect(_ui.treeWidgetGC, &QTreeWidget::itemSelectionChanged,
- this, &GraspCandidateViewerWidgetController::selected_item_changed);
+ connect(_ui.pushButtonUpdateGC,
+ &QPushButton::clicked,
+ this,
+ &GraspCandidateViewerWidgetController::update_gc);
+ connect(_ui.treeWidgetGC,
+ &QTreeWidget::itemSelectionChanged,
+ this,
+ &GraspCandidateViewerWidgetController::selected_item_changed);
- connect(_ui.treeWidgetGC, SIGNAL(itemChanged(QTreeWidgetItem*, int)),
- this, SLOT(check_state_changed()));
+ connect(_ui.treeWidgetGC,
+ SIGNAL(itemChanged(QTreeWidgetItem*, int)),
+ this,
+ SLOT(check_state_changed()));
}
GraspCandidateViewerWidgetController::~GraspCandidateViewerWidgetController()
- {}
+ {
+ }
- void GraspCandidateViewerWidgetController::update_gc()
+ void
+ GraspCandidateViewerWidgetController::update_gc()
{
ARMARX_TRACE;
std::lock_guard g{_mutex};
@@ -117,7 +142,7 @@ namespace armarx
{
pr.item = new QTreeWidgetItem;
pr.item->setCheckState(0, Qt::Unchecked);
- _ui.treeWidgetGC-> addTopLevelItem(pr.item);
+ _ui.treeWidgetGC->addTopLevelItem(pr.item);
pr.item->setText(0, QString::fromStdString(pname));
}
@@ -134,12 +159,13 @@ namespace armarx
}
}
- void GraspCandidateViewerWidgetController::check_state_changed()
+ void
+ GraspCandidateViewerWidgetController::check_state_changed()
{
ARMARX_TRACE;
std::lock_guard g{_mutex};
synchronizeLocalClone(_robot);
- armarx::grasp_candidate_drawer gc_drawer{getRobotNameHelper(), _robot};
+ armarx::grasp_candidate_drawer gc_drawer{getRobotNameHelper(), _robot};
std::vector<viz::Layer> layers;
for (const auto& [pname, pr] : _providers)
@@ -156,7 +182,9 @@ namespace armarx
}
getArvizClient().commit(layers);
}
- void GraspCandidateViewerWidgetController::selected_item_changed()
+
+ void
+ GraspCandidateViewerWidgetController::selected_item_changed()
{
ARMARX_TRACE;
std::lock_guard g{_mutex};
@@ -174,37 +202,38 @@ namespace armarx
//provider item selected -> do nothing
}
- void GraspCandidateViewerWidgetController::show_entry(entry_gc* e)
+ void
+ GraspCandidateViewerWidgetController::show_entry(entry_gc* e)
{
if (!e)
{
- _ui.labelGCIdx ->setText("-");
- _ui.labelNumGC ->setText("-");
+ _ui.labelGCIdx->setText("-");
+ _ui.labelNumGC->setText("-");
- _ui.labelGPoseTX ->setText("-");
- _ui.labelGPoseTY ->setText("-");
- _ui.labelGPoseTZ ->setText("-");
- _ui.labelGPoseRX ->setText("-");
- _ui.labelGPoseRY ->setText("-");
- _ui.labelGPoseRZ ->setText("-");
+ _ui.labelGPoseTX->setText("-");
+ _ui.labelGPoseTY->setText("-");
+ _ui.labelGPoseTZ->setText("-");
+ _ui.labelGPoseRX->setText("-");
+ _ui.labelGPoseRY->setText("-");
+ _ui.labelGPoseRZ->setText("-");
- _ui.labelRPoseTX ->setText("-");
- _ui.labelRPoseTY ->setText("-");
- _ui.labelRPoseTZ ->setText("-");
- _ui.labelRPoseRX ->setText("-");
- _ui.labelRPoseRY ->setText("-");
- _ui.labelRPoseRZ ->setText("-");
+ _ui.labelRPoseTX->setText("-");
+ _ui.labelRPoseTY->setText("-");
+ _ui.labelRPoseTZ->setText("-");
+ _ui.labelRPoseRX->setText("-");
+ _ui.labelRPoseRY->setText("-");
+ _ui.labelRPoseRZ->setText("-");
- _ui.labelGApprTX ->setText("-");
- _ui.labelGApprTY ->setText("-");
- _ui.labelGApprTZ ->setText("-");
+ _ui.labelGApprTX->setText("-");
+ _ui.labelGApprTY->setText("-");
+ _ui.labelGApprTZ->setText("-");
- _ui.labelSrcFrame ->setText("-");
- _ui.labelTrgFrame ->setText("-");
- _ui.labelSide ->setText("-");
- _ui.labelGraspProb ->setText("-");
- _ui.labelGraspgroupNr ->setText("-");
- _ui.labelGraspObjType ->setText("-");
+ _ui.labelSrcFrame->setText("-");
+ _ui.labelTrgFrame->setText("-");
+ _ui.labelSide->setText("-");
+ _ui.labelGraspProb->setText("-");
+ _ui.labelGraspgroupNr->setText("-");
+ _ui.labelGraspObjType->setText("-");
_ui.labelGraspProviderName->setText("-");
return;
}
@@ -214,63 +243,57 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(e->gc->graspPose->orientation);
ARMARX_CHECK_NOT_NULL(e->gc->robotPose->orientation);
ARMARX_CHECK_NOT_NULL(e->gc->approachVector);
- _ui.labelGCIdx ->setText(QString::number(e->idx + 1));
- _ui.labelNumGC ->setText(QString::number(e->provider->candidates.size()));
+ _ui.labelGCIdx->setText(QString::number(e->idx + 1));
+ _ui.labelNumGC->setText(QString::number(e->provider->candidates.size()));
- _ui.labelGPoseTX ->setText(QString::number(e->gc->graspPose->position->x));
- _ui.labelGPoseTY ->setText(QString::number(e->gc->graspPose->position->y));
- _ui.labelGPoseTZ ->setText(QString::number(e->gc->graspPose->position->z));
- const Eigen::Vector3f vec_g = simox::math::quat_to_rpy(
- {
- e->gc->graspPose->orientation->qw,
- e->gc->graspPose->orientation->qx,
- e->gc->graspPose->orientation->qy,
- e->gc->graspPose->orientation->qz
+ _ui.labelGPoseTX->setText(QString::number(e->gc->graspPose->position->x));
+ _ui.labelGPoseTY->setText(QString::number(e->gc->graspPose->position->y));
+ _ui.labelGPoseTZ->setText(QString::number(e->gc->graspPose->position->z));
+ const Eigen::Vector3f vec_g = simox::math::quat_to_rpy({e->gc->graspPose->orientation->qw,
+ e->gc->graspPose->orientation->qx,
+ e->gc->graspPose->orientation->qy,
+ e->gc->graspPose->orientation->qz
- }
- );
- _ui.labelGPoseRX ->setText(QString::number(vec_g(0)));
- _ui.labelGPoseRY ->setText(QString::number(vec_g(1)));
- _ui.labelGPoseRZ ->setText(QString::number(vec_g(2)));
+ });
+ _ui.labelGPoseRX->setText(QString::number(vec_g(0)));
+ _ui.labelGPoseRY->setText(QString::number(vec_g(1)));
+ _ui.labelGPoseRZ->setText(QString::number(vec_g(2)));
- _ui.labelRPoseTX ->setText(QString::number(e->gc->robotPose->position->x));
- _ui.labelRPoseTY ->setText(QString::number(e->gc->robotPose->position->y));
- _ui.labelRPoseTZ ->setText(QString::number(e->gc->robotPose->position->z));
- const Eigen::Vector3f vec_r = simox::math::quat_to_rpy(
- {
- e->gc->robotPose->orientation->qw,
- e->gc->robotPose->orientation->qx,
- e->gc->robotPose->orientation->qy,
- e->gc->robotPose->orientation->qz
+ _ui.labelRPoseTX->setText(QString::number(e->gc->robotPose->position->x));
+ _ui.labelRPoseTY->setText(QString::number(e->gc->robotPose->position->y));
+ _ui.labelRPoseTZ->setText(QString::number(e->gc->robotPose->position->z));
+ const Eigen::Vector3f vec_r = simox::math::quat_to_rpy({e->gc->robotPose->orientation->qw,
+ e->gc->robotPose->orientation->qx,
+ e->gc->robotPose->orientation->qy,
+ e->gc->robotPose->orientation->qz
- }
- );
- _ui.labelRPoseRX ->setText(QString::number(vec_r(0)));
- _ui.labelRPoseRY ->setText(QString::number(vec_r(1)));
- _ui.labelRPoseRZ ->setText(QString::number(vec_r(2)));
+ });
+ _ui.labelRPoseRX->setText(QString::number(vec_r(0)));
+ _ui.labelRPoseRY->setText(QString::number(vec_r(1)));
+ _ui.labelRPoseRZ->setText(QString::number(vec_r(2)));
- _ui.labelGApprTX ->setText(QString::number(e->gc->approachVector->x));
- _ui.labelGApprTY ->setText(QString::number(e->gc->approachVector->y));
- _ui.labelGApprTZ ->setText(QString::number(e->gc->approachVector->z));
+ _ui.labelGApprTX->setText(QString::number(e->gc->approachVector->x));
+ _ui.labelGApprTY->setText(QString::number(e->gc->approachVector->y));
+ _ui.labelGApprTZ->setText(QString::number(e->gc->approachVector->z));
- _ui.labelSrcFrame ->setText(QString::fromStdString(e->gc->sourceFrame));
- _ui.labelTrgFrame ->setText(QString::fromStdString(e->gc->targetFrame));
- _ui.labelSide ->setText(QString::fromStdString(e->gc->side));
- _ui.labelGraspProb ->setText(QString::number(e->gc->graspSuccessProbability));
- _ui.labelGraspgroupNr ->setText(QString::number(e->gc->groupNr));
+ _ui.labelSrcFrame->setText(QString::fromStdString(e->gc->sourceFrame));
+ _ui.labelTrgFrame->setText(QString::fromStdString(e->gc->targetFrame));
+ _ui.labelSide->setText(QString::fromStdString(e->gc->side));
+ _ui.labelGraspProb->setText(QString::number(e->gc->graspSuccessProbability));
+ _ui.labelGraspgroupNr->setText(QString::number(e->gc->groupNr));
switch (e->gc->objectType)
{
- case objpose::ObjectType::AnyObject :
+ case objpose::ObjectType::AnyObject:
_ui.labelGraspObjType->setText("AnyObject");
break;
- case objpose::ObjectType::KnownObject :
+ case objpose::ObjectType::KnownObject:
_ui.labelGraspObjType->setText("KnownObject");
break;
- case objpose::ObjectType::UnknownObject :
+ case objpose::ObjectType::UnknownObject:
_ui.labelGraspObjType->setText("UnknownObject");
break;
}
_ui.labelGraspProviderName->setText(QString::fromStdString(e->gc->providerName));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.h b/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.h
index 72cd448b1d63b8047fde3bd84873b129223e2f38..58f2c0e36eb0eb2bb55433ebaa598b2fd888b98e 100644
--- a/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/GraspCandidateViewer/GraspCandidateViewerWidgetController.h
@@ -25,17 +25,16 @@
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/components/ArViz/Client/elements/Robot.h>
-
#include <RobotAPI/gui-plugins/GraspCandidateViewer/ui_GraspCandidateViewerWidget.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
namespace armarx
{
@@ -66,9 +65,9 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- GraspCandidateViewerWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < GraspCandidateViewerWidgetController >,
+ class ARMARXCOMPONENT_IMPORT_EXPORT GraspCandidateViewerWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<
+ GraspCandidateViewerWidgetController>,
virtual public RobotStateComponentPluginUser,
virtual public ArVizComponentPluginUser,
virtual public GraspCandidateObserverComponentPluginUser
@@ -77,22 +76,38 @@ namespace armarx
struct entry_gc;
struct entry_prov;
+
public:
explicit GraspCandidateViewerWidgetController();
virtual ~GraspCandidateViewerWidgetController();
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Grasping.GraspCandidateViewer";
}
- void onInitComponent() override {}
- void onConnectComponent() override
+ void
+ onInitComponent() override
+ {
+ }
+
+ void
+ onConnectComponent() override
{
_robot = addRobot("state robot", VirtualRobot::RobotIO::eStructure);
}
- void onDisconnectComponent() override {}
- void onExitComponent() override {}
+
+ void
+ onDisconnectComponent() override
+ {
+ }
+
+ void
+ onExitComponent() override
+ {
+ }
+
public:
void loadSettings(QSettings* settings) override;
void saveSettings(QSettings* settings) override;
@@ -105,36 +120,35 @@ namespace armarx
private:
void show_entry(entry_gc* e);
- private:
+ private:
struct entry_prov
{
- QTreeWidgetItem* item = nullptr;
- std::string provider_name;
- std::deque<entry_gc> candidates;
+ QTreeWidgetItem* item = nullptr;
+ std::string provider_name;
+ std::deque<entry_gc> candidates;
};
struct entry_gc
{
- QTreeWidgetItem* item = nullptr;
- grasping::GraspCandidatePtr gc = nullptr;
- entry_prov* provider = nullptr;
- int idx = -1;
- bool show = false;
-
- std::string name() const
+ QTreeWidgetItem* item = nullptr;
+ grasping::GraspCandidatePtr gc = nullptr;
+ entry_prov* provider = nullptr;
+ int idx = -1;
+ bool show = false;
+
+ std::string
+ name() const
{
return provider->provider_name + "_" + std::to_string(idx + 1);
}
};
- mutable std::recursive_mutex _mutex;
- QPointer<SimpleConfigDialog> _dialog;
- Ui::GraspCandidateViewerWidget _ui;
- VirtualRobot::RobotPtr _robot;
- std::map<std::string, entry_prov> _providers;
+ mutable std::recursive_mutex _mutex;
+ QPointer<SimpleConfigDialog> _dialog;
+ Ui::GraspCandidateViewerWidget _ui;
+ VirtualRobot::RobotPtr _robot;
+ std::map<std::string, entry_prov> _providers;
std::map<QTreeWidgetItem*, entry_gc*> _tree_item_to_gc;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.cpp
index cec6eaaffc0d521704a7dc7dfcf449bf4e61f69a..87e222369dfa2e19b062fb81dad68cfa6dfe316d 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.cpp
@@ -28,6 +28,6 @@ namespace armarx
{
GuiHealthClientGuiPlugin::GuiHealthClientGuiPlugin()
{
- addWidget < GuiHealthClientWidgetController > ();
+ addWidget<GuiHealthClientWidgetController>();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.h b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.h
index fcd2506d6c1ed1c373d87511e3c77ffb18cba45a..a887b68ff57174d5ba9f75180173bebc1a617370 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientGuiPlugin.h
@@ -22,8 +22,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -34,8 +35,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT GuiHealthClientGuiPlugin:
- public armarx::ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT GuiHealthClientGuiPlugin : public armarx::ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -47,4 +47,4 @@ namespace armarx
*/
GuiHealthClientGuiPlugin();
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
index 4868ebda8519f9d571c0a62a17ccbecd66e5a18d..ed54966fd8d19b812b4dfe7b332bc991a2cac1ce 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
@@ -22,16 +22,17 @@
#include "GuiHealthClientWidgetController.h"
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <iomanip>
+#include <string>
+
+#include <QLabel>
+#include <QPushButton>
#include <qcolor.h>
#include <qnamespace.h>
#include <qrgb.h>
#include <qtablewidget.h>
-#include <string>
-#include <iomanip>
-#include <QLabel>
-#include <QPushButton>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
@@ -39,15 +40,16 @@
namespace armarx
{
- std::string serializeList(const std::vector<std::string>& l)
+ std::string
+ serializeList(const std::vector<std::string>& l)
{
std::string s;
- for(std::size_t i = 0; i < l.size(); i++)
+ for (std::size_t i = 0; i < l.size(); i++)
{
const auto& tag = l.at(i);
s += tag;
- if(i != l.size() - 1)
+ if (i != l.size() - 1)
{
s += ", ";
}
@@ -71,10 +73,7 @@ namespace armarx
SLOT(onDisconnectComponentQt()),
Qt::QueuedConnection);
- connect(widget.pushButtonUnrequireAll,
- SIGNAL(clicked()),
- this,
- SLOT(unrequireAll()));
+ connect(widget.pushButtonUnrequireAll, SIGNAL(clicked()), this, SLOT(unrequireAll()));
updateSummaryTimer = new QTimer(this);
updateSummaryTimer->setInterval(100);
@@ -111,24 +110,22 @@ namespace armarx
robotHealthTopicPrx->heartbeat(getName(), now);
}
-
- std::string to_string_rounded(float value, int decimals = 100)
+ std::string
+ to_string_rounded(float value, int decimals = 100)
{
std::stringstream ss;
ss << std::fixed << std::setprecision(3) << value;
return ss.str();
}
-
- QTableWidgetItem* make_item(const std::string& text,
- Qt::AlignmentFlag horAlignment = Qt::AlignLeft)
+ QTableWidgetItem*
+ make_item(const std::string& text, Qt::AlignmentFlag horAlignment = Qt::AlignLeft)
{
auto* item = new QTableWidgetItem(QString::fromStdString(text));
item->setTextAlignment(horAlignment | Qt::AlignVCenter);
return item;
}
-
void
GuiHealthClientWidgetController::updateSummaryTimerClb()
{
@@ -148,9 +145,16 @@ namespace armarx
const auto summaryVals = simox::alg::get_values(summary.entries);
- tableWidget->setHorizontalHeaderLabels({
- "identifier", "required", "keeps frequency", "tags", "time since\nlast arrival [ms]", "time to\nreference [ms]", "time sync \n+ Ice [ms]", "warning\nthreshold [ms]", "error\nthreshold [ms]", "hostname"
- });
+ tableWidget->setHorizontalHeaderLabels({"identifier",
+ "required",
+ "keeps frequency",
+ "tags",
+ "time since\nlast arrival [ms]",
+ "time to\nreference [ms]",
+ "time sync \n+ Ice [ms]",
+ "warning\nthreshold [ms]",
+ "error\nthreshold [ms]",
+ "hostname"});
tableWidget->setColumnWidth(0, 250);
tableWidget->setColumnWidth(1, 80);
@@ -160,14 +164,14 @@ namespace armarx
tableWidget->setColumnWidth(5, 120);
tableWidget->setColumnWidth(6, 120);
tableWidget->setColumnWidth(7, 120);
-
- for(std::size_t i = 0; i < summary.entries.size(); i++)
+
+ for (std::size_t i = 0; i < summary.entries.size(); i++)
{
const auto& entry = summaryVals.at(i);
std::string stateRepr;
QColor stateColor;
- switch(entry.state)
+ switch (entry.state)
{
case HealthOK:
stateRepr = "yes";
@@ -185,27 +189,36 @@ namespace armarx
const std::string hostname = entry.lastReferenceTimestamp.hostname;
- const std::string timeSinceLastArrivalRepr = to_string_rounded(entry.timeSinceLastArrival.microSeconds / 1000.0);
- const std::string timeToLastReferenceRepr = to_string_rounded(entry.timeSinceLastUpdateReference.microSeconds / 1000.0);
+ const std::string timeSinceLastArrivalRepr =
+ to_string_rounded(entry.timeSinceLastArrival.microSeconds / 1000.0);
+ const std::string timeToLastReferenceRepr =
+ to_string_rounded(entry.timeSinceLastUpdateReference.microSeconds / 1000.0);
const std::string tagsRepr = serializeList(entry.tags);
- const float syncErrorMilliSeconds = std::abs(entry.timeSinceLastArrival.microSeconds - entry.timeSinceLastUpdateReference.microSeconds) / 1000.0;
+ const float syncErrorMilliSeconds =
+ std::abs(entry.timeSinceLastArrival.microSeconds -
+ entry.timeSinceLastUpdateReference.microSeconds) /
+ 1000.0;
- tableWidget->setItem(i, 0, new QTableWidgetItem(QString::fromStdString(entry.identifier)));
- tableWidget->setItem(i, 1, make_item(entry.required ? "yes" : "no", Qt::AlignHCenter));
+ tableWidget->setItem(
+ i, 0, new QTableWidgetItem(QString::fromStdString(entry.identifier)));
+ tableWidget->setItem(
+ i, 1, make_item(entry.required ? "yes" : "no", Qt::AlignHCenter));
{
auto* item = make_item(stateRepr, Qt::AlignHCenter);
item->setBackgroundColor(stateColor);
tableWidget->setItem(i, 2, item);
}
-
- tableWidget->setItem(i, 3, new QTableWidgetItem(QString::fromStdString(tagsRepr)));
+
+ tableWidget->setItem(
+ i, 3, new QTableWidgetItem(QString::fromStdString(tagsRepr)));
tableWidget->setItem(i, 4, make_item(timeSinceLastArrivalRepr, Qt::AlignRight));
tableWidget->setItem(i, 5, make_item(timeToLastReferenceRepr, Qt::AlignRight));
{
- auto* item = make_item(to_string_rounded(syncErrorMilliSeconds), Qt::AlignRight);
+ auto* item =
+ make_item(to_string_rounded(syncErrorMilliSeconds), Qt::AlignRight);
QColor timeSyncColor;
if (syncErrorMilliSeconds > 20.)
@@ -220,10 +233,21 @@ namespace armarx
tableWidget->setItem(i, 6, item);
}
-
- tableWidget->setItem(i, 7, make_item(to_string_rounded(entry.maximumCycleTimeWarning.microSeconds / 1000.), Qt::AlignRight));
- tableWidget->setItem(i, 8, make_item(to_string_rounded(entry.maximumCycleTimeError.microSeconds / 1000.), Qt::AlignRight));
- tableWidget->setItem(i, 9, new QTableWidgetItem(QString::fromStdString(hostname)));
+
+ tableWidget->setItem(
+ i,
+ 7,
+ make_item(
+ to_string_rounded(entry.maximumCycleTimeWarning.microSeconds / 1000.),
+ Qt::AlignRight));
+ tableWidget->setItem(
+ i,
+ 8,
+ make_item(
+ to_string_rounded(entry.maximumCycleTimeError.microSeconds / 1000.),
+ Qt::AlignRight));
+ tableWidget->setItem(
+ i, 9, new QTableWidgetItem(QString::fromStdString(hostname)));
}
std::string tagsText = "Active tags: [";
@@ -252,8 +276,10 @@ namespace armarx
try
{
robotHealthComponentPrx->resetRequiredTags();
- }
- catch(...){}
+ }
+ catch (...)
+ {
+ }
}
}
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
index f5bc6ff28a593ba1d5e58bf1a02aabf42cd29bae..17e70e4c7b4f64afd6568193c4fef2f8d2ba7780 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
@@ -21,16 +21,16 @@
*/
#pragma once
-#include <RobotAPI/gui-plugins/GuiHealthClient/ui_GuiHealthClientWidget.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <QTimer>
#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
+#include <RobotAPI/gui-plugins/GuiHealthClient/ui_GuiHealthClientWidget.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <QTimer>
namespace armarx
{
@@ -52,9 +52,8 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- GuiHealthClientWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < GuiHealthClientWidgetController >
+ class ARMARXCOMPONENT_IMPORT_EXPORT GuiHealthClientWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<GuiHealthClientWidgetController>
{
Q_OBJECT
@@ -83,11 +82,14 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "GuiHealthClient";
}
- static QIcon GetWidgetIcon()
+
+ static QIcon
+ GetWidgetIcon()
{
return QIcon("://icons/heart.svg");
}
@@ -127,4 +129,4 @@ namespace armarx
RobotHealthComponentInterfacePrx robotHealthComponentPrx;
QTimer* updateSummaryTimer;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
index 55776817eee69b3f783cda883871b88ec9679439..ca9ca1f35d984a3b7913cb52efb9d16dd32abe7d 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp
@@ -21,18 +21,18 @@
*/
#include "HandUnitConfigDialog.h"
-#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitConfigDialog.h>
-#include <RobotAPI/components/units/HandUnit.h>
#include <IceUtil/UUID.h>
+#include <RobotAPI/components/units/HandUnit.h>
+#include <RobotAPI/gui-plugins/HandUnitPlugin/ui_HandUnitConfigDialog.h>
+
armarx::HandUnitConfigDialog::HandUnitConfigDialog(QWidget* parent) :
- QDialog(parent),
- ui(new Ui::HandUnitConfigDialog),
- uuid(IceUtil::generateUUID())
+ QDialog(parent), ui(new Ui::HandUnitConfigDialog), uuid(IceUtil::generateUUID())
{
ui->setupUi(this);
- setName(getDefaultName()); // @@@ This is necessary for more than 1 widget or even reopening the widget.
+ setName(
+ getDefaultName()); // @@@ This is necessary for more than 1 widget or even reopening the widget.
proxyFinderLeftHand = new IceProxyFinder<HandUnitInterfacePrx>(this);
@@ -49,14 +49,14 @@ armarx::HandUnitConfigDialog::~HandUnitConfigDialog()
delete ui;
}
-
-
-void armarx::HandUnitConfigDialog::onInitComponent()
+void
+armarx::HandUnitConfigDialog::onInitComponent()
{
proxyFinderLeftHand->setIceManager(getIceManager());
proxyFinderRightHand->setIceManager(getIceManager());
}
-void armarx::HandUnitConfigDialog::onConnectComponent()
+void
+armarx::HandUnitConfigDialog::onConnectComponent()
{
}
diff --git a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h
index 149d8ce6101c2484d2a40e45d903d0faf45ed491..75eee92b1d513df548f4963e2b2a51c0615892d6 100644
--- a/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.h
@@ -36,9 +36,7 @@ namespace Ui
namespace armarx
{
- class HandUnitConfigDialog :
- public QDialog,
- virtual public ManagedIceObject
+ class HandUnitConfigDialog : public QDialog, virtual public ManagedIceObject
{
Q_OBJECT
@@ -48,10 +46,12 @@ namespace armarx
protected:
// ManagedIceObject interface
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "HandUnitConfigDialog" + uuid;
}
+
void onInitComponent() override;
void onConnectComponent() override;
@@ -63,7 +63,5 @@ namespace armarx
std::string uuid;
friend class HandUnitWidget;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp
index 5b7f350afdc769c246cc6c8021953389859c0784..973a97dc1ac68eee77fd1406ef5bebeef0e4cdcd 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.cpp
@@ -21,11 +21,11 @@
*/
#include "HapticUnitConfigDialog.h"
+
#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_HapticUnitConfigDialog.h>
armarx::HapticUnitConfigDialog::HapticUnitConfigDialog(QWidget* parent) :
- QDialog(parent),
- ui(new Ui::HapticUnitConfigDialog)
+ QDialog(parent), ui(new Ui::HapticUnitConfigDialog)
{
ui->setupUi(this);
}
@@ -34,4 +34,3 @@ armarx::HapticUnitConfigDialog::~HapticUnitConfigDialog()
{
delete ui;
}
-
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.h b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.h
index ba318632ec9c8b70e4662f36e128152cc372eefd..07bd8d40fadf60c86a68759ea81bc85958e4f37e 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitConfigDialog.h
@@ -31,8 +31,7 @@ namespace Ui
namespace armarx
{
- class HapticUnitConfigDialog :
- public QDialog
+ class HapticUnitConfigDialog : public QDialog
{
Q_OBJECT
@@ -45,5 +44,4 @@ namespace armarx
//friend class HapticUnitWidget;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
index 3d4272f26d708bcdccd58e334f31e33fab5ffac9..a4c6690ddb4aa3db0bfc62aaf2627411ddaa9634 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
@@ -22,23 +22,26 @@
* GNU General Public License
*/
#include "HapticUnitGuiPlugin.h"
-#include "HapticUnitConfigDialog.h"
-#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_HapticUnitConfigDialog.h>
+
+#include <IceUtil/Time.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
-#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-#include <IceUtil/Time.h>
+#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+
+#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_HapticUnitConfigDialog.h>
+
+#include "HapticUnitConfigDialog.h"
// Qt headers
+#include <QComboBox>
#include <QInputDialog>
+#include <QMenu>
+#include <QPushButton>
#include <Qt>
#include <QtGlobal>
-#include <QPushButton>
-#include <QComboBox>
-#include <QMenu>
namespace armarx
{
@@ -47,7 +50,6 @@ namespace armarx
addWidget<HapticUnitWidget>();
}
-
HapticUnitWidget::HapticUnitWidget()
{
// init gui
@@ -57,22 +59,17 @@ namespace armarx
hapticUnitProxyName = "WeissHapticUnit";
updateTimer = new QTimer(this);
-
-
-
-
}
-
- void HapticUnitWidget::onInitComponent()
+ void
+ HapticUnitWidget::onInitComponent()
{
usingProxy(hapticObserverProxyName);
usingProxy(hapticUnitProxyName);
-
}
-
- void HapticUnitWidget::onConnectComponent()
+ void
+ HapticUnitWidget::onConnectComponent()
{
hapticObserverProxy = getProxy<ObserverInterfacePrx>(hapticObserverProxyName);
weissHapticUnit = getProxy<WeissHapticUnitInterfacePrx>(hapticUnitProxyName);
@@ -80,22 +77,22 @@ namespace armarx
connectSlots();
createMatrixWidgets();
updateTimer->start(25); // 50 Hz
-
}
-
- void HapticUnitWidget::onExitComponent()
+ void
+ HapticUnitWidget::onExitComponent()
{
updateTimer->stop();
}
- void HapticUnitWidget::onDisconnectComponent()
+ void
+ HapticUnitWidget::onDisconnectComponent()
{
updateTimer->stop();
}
-
- QPointer<QDialog> HapticUnitWidget::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ HapticUnitWidget::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
@@ -105,33 +102,45 @@ namespace armarx
return qobject_cast<HapticUnitConfigDialog*>(dialog);
}
-
- void HapticUnitWidget::configured()
+ void
+ HapticUnitWidget::configured()
{
-
}
- void HapticUnitWidget::updateData()
+ void
+ HapticUnitWidget::updateData()
{
for (std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
{
//MatrixFloatPtr matrix = VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "matrix")))->get<MatrixFloat>();
- std::string name = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "name"))->getString();
- std::string deviceName = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "device"))->getString();
+ std::string name = hapticObserverProxy
+ ->getDataField(new DataFieldIdentifier(
+ hapticObserverProxyName, pair.first, "name"))
+ ->getString();
+ std::string deviceName = hapticObserverProxy
+ ->getDataField(new DataFieldIdentifier(
+ hapticObserverProxyName, pair.first, "device"))
+ ->getString();
//float rate = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "rate"))->getFloat();
- TimestampVariantPtr timestamp = VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, pair.first, "timestamp")))->get<TimestampVariant>();
+ TimestampVariantPtr timestamp =
+ VariantPtr::dynamicCast(hapticObserverProxy->getDataField(new DataFieldIdentifier(
+ hapticObserverProxyName, pair.first, "timestamp")))
+ ->get<TimestampVariant>();
MatrixDatafieldDisplayWidget* matrixDisplay = pair.second;
- matrixDisplay->setInfoOverlay(QString::fromStdString(deviceName) + ": " + QString::fromStdString(name) + "\n" + QString::fromStdString(timestamp->toTime().toDateTime()));
+ matrixDisplay->setInfoOverlay(QString::fromStdString(deviceName) + ": " +
+ QString::fromStdString(name) + "\n" +
+ QString::fromStdString(timestamp->toTime().toDateTime()));
matrixDisplay->invokeUpdate();
}
-
}
- void HapticUnitWidget::onContextMenu(QPoint point)
+ void
+ HapticUnitWidget::onContextMenu(QPoint point)
{
QMenu* contextMenu = new QMenu(getWidget());
- MatrixDatafieldDisplayWidget* matrixDisplay = qobject_cast<MatrixDatafieldDisplayWidget*>(sender());
+ MatrixDatafieldDisplayWidget* matrixDisplay =
+ qobject_cast<MatrixDatafieldDisplayWidget*>(sender());
if (!matrixDisplay)
{
@@ -147,16 +156,24 @@ namespace armarx
if (action == setDeviceTag)
{
- std::string tag = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, channelName, "name"))->getString();
+ std::string tag = hapticObserverProxy
+ ->getDataField(new DataFieldIdentifier(
+ hapticObserverProxyName, channelName, "name"))
+ ->getString();
bool ok;
- QString newTag = QInputDialog::getText(getWidget(), tr("Set Device Tag"),
- QString::fromStdString("New Tag for Device '" + deviceName + "'"), QLineEdit::Normal,
- QString::fromStdString(tag), &ok);
+ QString newTag = QInputDialog::getText(
+ getWidget(),
+ tr("Set Device Tag"),
+ QString::fromStdString("New Tag for Device '" + deviceName + "'"),
+ QLineEdit::Normal,
+ QString::fromStdString(tag),
+ &ok);
if (ok && !newTag.isEmpty())
{
- ARMARX_IMPORTANT_S << "requesting to set new device tag for " << deviceName << ": " << newTag.toStdString();
+ ARMARX_IMPORTANT_S << "requesting to set new device tag for " << deviceName << ": "
+ << newTag.toStdString();
weissHapticUnit->setDeviceTag(deviceName, newTag.toStdString());
}
}
@@ -164,31 +181,39 @@ namespace armarx
delete contextMenu;
}
-
- void HapticUnitWidget::loadSettings(QSettings* settings)
+ void
+ HapticUnitWidget::loadSettings(QSettings* settings)
{
}
-
- void HapticUnitWidget::saveSettings(QSettings* settings)
+ void
+ HapticUnitWidget::saveSettings(QSettings* settings)
{
}
-
- void HapticUnitWidget::connectSlots()
+ void
+ HapticUnitWidget::connectSlots()
{
- connect(this, SIGNAL(doUpdateDisplayWidgets()), SLOT(updateDisplayWidgets()), Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(doUpdateDisplayWidgets()),
+ SLOT(updateDisplayWidgets()),
+ Qt::QueuedConnection);
connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateData()));
- connect(ui.checkBoxOffsetFilter, SIGNAL(stateChanged(int)), this, SLOT(onCheckBoxOffsetFilterStateChanged(int)));
+ connect(ui.checkBoxOffsetFilter,
+ SIGNAL(stateChanged(int)),
+ this,
+ SLOT(onCheckBoxOffsetFilterStateChanged(int)));
}
- void HapticUnitWidget::createMatrixWidgets()
+ void
+ HapticUnitWidget::createMatrixWidgets()
{
//ARMARX_LOG << "HapticUnitWidget::createMatrixWidgets()";
emit doUpdateDisplayWidgets();
}
- void HapticUnitWidget::updateDisplayWidgets()
+ void
+ HapticUnitWidget::updateDisplayWidgets()
{
QLayoutItem* child;
@@ -203,12 +228,21 @@ namespace armarx
for (std::pair<std::string, ChannelRegistryEntry> pair : channels)
{
std::string channelName = pair.first;
- MatrixDatafieldDisplayWidget* matrixDisplay = new MatrixDatafieldDisplayWidget(new DatafieldRef(hapticObserverProxy, channelName, "matrix"), hapticObserverProxy, getWidget());
+ MatrixDatafieldDisplayWidget* matrixDisplay = new MatrixDatafieldDisplayWidget(
+ new DatafieldRef(hapticObserverProxy, channelName, "matrix"),
+ hapticObserverProxy,
+ getWidget());
matrixDisplay->setRange(0, 4095);
matrixDisplay->setContextMenuPolicy(Qt::CustomContextMenu);
- connect(matrixDisplay, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(onContextMenu(QPoint)));
+ connect(matrixDisplay,
+ SIGNAL(customContextMenuRequested(QPoint)),
+ this,
+ SLOT(onContextMenu(QPoint)));
matrixDisplays.insert(std::make_pair(pair.first, matrixDisplay));
- std::string deviceName = hapticObserverProxy->getDataField(new DataFieldIdentifier(hapticObserverProxyName, channelName, "device"))->getString();
+ std::string deviceName = hapticObserverProxy
+ ->getDataField(new DataFieldIdentifier(
+ hapticObserverProxyName, channelName, "device"))
+ ->getString();
channelNameReverseMap.insert(std::make_pair(matrixDisplay, channelName));
deviceNameReverseMap.insert(std::make_pair(matrixDisplay, deviceName));
ui.gridLayoutDisplay->addWidget(matrixDisplay, 0, i);
@@ -216,7 +250,8 @@ namespace armarx
}
}
- void HapticUnitWidget::onCheckBoxOffsetFilterStateChanged(int state)
+ void
+ HapticUnitWidget::onCheckBoxOffsetFilterStateChanged(int state)
{
//ARMARX_IMPORTANT << "onCheckBoxOffsetFilterToggled: " << state;
for (std::pair<std::string, MatrixDatafieldDisplayWidget*> pair : matrixDisplays)
@@ -224,4 +259,4 @@ namespace armarx
pair.second->enableOffsetFilter(ui.checkBoxOffsetFilter->isChecked());
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
index 69f055d03ba40bb349c25792bd8e781b61fb38ce..f6e473cbe025c5c45533dd0e6a9f6d270eb4636e 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h
@@ -24,35 +24,39 @@
#pragma once
/* ArmarX headers */
-#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_HapticUnitGuiPlugin.h>
#include <ArmarXCore/core/Component.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXCore/observers/Observer.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
+#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_HapticUnitGuiPlugin.h>
+
/* Qt headers */
+#include <string>
+
+#include <QLayout>
#include <QMainWindow>
#include <QtCore/QTimer>
+
#include <RobotAPI/interface/units/WeissHapticUnit.h>
-#include <string>
-#include <QLayout>
#include "MatrixDatafieldDisplayWidget.h"
-
namespace armarx
{
class HapticUnitConfigDialog;
- class HapticUnitGuiPlugin :
- public ArmarXGuiPlugin
+ class HapticUnitGuiPlugin : public ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
public:
HapticUnitGuiPlugin();
- QString getPluginName() override
+
+ QString
+ getPluginName() override
{
return "HapticUnitGuiPlugin";
}
@@ -62,14 +66,15 @@ namespace armarx
\page RobotAPI-GuiPlugins-HapticUnitPlugin HapticUnitPlugin
\brief With this widget the HapticUnit can be controlled.
*/
- class HapticUnitWidget :
- public ArmarXComponentWidgetControllerTemplate<HapticUnitWidget>
+ class HapticUnitWidget : public ArmarXComponentWidgetControllerTemplate<HapticUnitWidget>
{
Q_OBJECT
public:
HapticUnitWidget();
+
~HapticUnitWidget() override
- {}
+ {
+ }
// inherited from Component
void onInitComponent() override;
@@ -78,11 +83,14 @@ namespace armarx
void onDisconnectComponent() override;
// inherited of ArmarXWidget
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "RobotControl.HapticUnitGUI";
}
- static QIcon GetWidgetIcon()
+
+ static QIcon
+ GetWidgetIcon()
{
return QIcon("://icons/haptic-hand.png");
}
@@ -109,11 +117,9 @@ namespace armarx
Ui::HapticUnitGuiPlugin ui;
private:
-
void createMatrixWidgets();
private:
-
std::string hapticObserverProxyName;
std::string hapticUnitProxyName;
ObserverInterfacePrx hapticObserverProxy;
@@ -127,8 +133,7 @@ namespace armarx
std::map<std::string, MatrixDatafieldDisplayWidget*> matrixDisplays;
std::map<MatrixDatafieldDisplayWidget*, std::string> channelNameReverseMap;
std::map<MatrixDatafieldDisplayWidget*, std::string> deviceNameReverseMap;
-
};
- //using HapticUnitGuiPluginPtr = std::shared_ptr<HapticUnitWidget>;
-}
+ //using HapticUnitGuiPluginPtr = std::shared_ptr<HapticUnitWidget>;
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
index 321e727cd020b44c0a7a2a34624f1bbdfb38f8da..fe755d20802b53521461bda9851d5bf8092bc208 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
@@ -23,16 +23,19 @@
*/
#include "MatrixDatafieldDisplayWidget.h"
-#include <QPainter>
-#include <pthread.h>
#include <iostream>
+#include <pthread.h>
+
+#include <QPainter>
+
#include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
namespace armarx
{
- void MatrixDatafieldDisplayWidget::updateRequested()
+ void
+ MatrixDatafieldDisplayWidget::updateRequested()
{
mtx.lock();
this->data = matrixDatafieldOffsetFiltered->getDataField()->get<MatrixFloat>()->toEigen();
@@ -41,7 +44,9 @@ namespace armarx
update();
}
- MatrixDatafieldDisplayWidget::MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield, ObserverInterfacePrx observer, QWidget* parent) :
+ MatrixDatafieldDisplayWidget::MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield,
+ ObserverInterfacePrx observer,
+ QWidget* parent) :
QWidget(parent)
{
this->matrixDatafield = DatafieldRefPtr::dynamicCast(matrixDatafield);
@@ -51,9 +56,13 @@ namespace armarx
this->data = MatrixXf(1, 1);
this->data(0, 0) = 0;
enableOffsetFilter(false);
- QColor c[] = {QColor::fromHsv(0, 0, 0), QColor::fromHsv(240, 255, 255), QColor::fromHsv(270, 255, 255), QColor::fromHsv(300, 255, 255),
- QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)
- };
+ QColor c[] = {QColor::fromHsv(0, 0, 0),
+ QColor::fromHsv(240, 255, 255),
+ QColor::fromHsv(270, 255, 255),
+ QColor::fromHsv(300, 255, 255),
+ QColor::fromHsv(0, 255, 255),
+ QColor::fromHsv(30, 255, 255),
+ QColor::fromHsv(60, 255, 255)};
this->colors = std::valarray<QColor>(c, sizeof c / sizeof c[0]);
//connect(this, SIGNAL(updateData(MatrixXf)), SLOT(setData(MatrixXf)), Qt::QueuedConnection);
@@ -65,7 +74,8 @@ namespace armarx
//delete ui;
}
- void MatrixDatafieldDisplayWidget::paintEvent(QPaintEvent*)
+ void
+ MatrixDatafieldDisplayWidget::paintEvent(QPaintEvent*)
{
mtx.lock();
@@ -84,21 +94,27 @@ namespace armarx
mtx.unlock();
}
- void MatrixDatafieldDisplayWidget::enableOffsetFilter(bool enabled)
+ void
+ MatrixDatafieldDisplayWidget::enableOffsetFilter(bool enabled)
{
if (enabled)
{
- this->matrixDatafieldOffsetFiltered = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(DatafieldFilterBasePtr(new filters::OffsetFilter()), matrixDatafield));
+ this->matrixDatafieldOffsetFiltered =
+ DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(
+ DatafieldFilterBasePtr(new filters::OffsetFilter()), matrixDatafield));
}
else
{
this->matrixDatafieldOffsetFiltered = this->matrixDatafield;
}
- this->percentilesDatafield = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(DatafieldFilterBasePtr(new filters::MatrixPercentilesFilter(50)), matrixDatafieldOffsetFiltered));
+ this->percentilesDatafield = DatafieldRefPtr::dynamicCast(observer->createFilteredDatafield(
+ DatafieldFilterBasePtr(new filters::MatrixPercentilesFilter(50)),
+ matrixDatafieldOffsetFiltered));
}
- QColor MatrixDatafieldDisplayWidget::getColor(float value, float min, float max)
+ QColor
+ MatrixDatafieldDisplayWidget::getColor(float value, float min, float max)
{
value = (value - min) / (max - min) * (colors.size() - 1);
@@ -117,10 +133,13 @@ namespace armarx
float f1 = 1 - f2;
QColor c1 = colors[i];
QColor c2 = colors[i + 1];
- return QColor((int)(c1.red() * f1 + c2.red() * f2), (int)(c1.green() * f1 + c2.green() * f2), (int)(c1.blue() * f1 + c2.blue() * f2));
+ return QColor((int)(c1.red() * f1 + c2.red() * f2),
+ (int)(c1.green() * f1 + c2.green() * f2),
+ (int)(c1.blue() * f1 + c2.blue() * f2));
}
- void MatrixDatafieldDisplayWidget::drawMatrix(const QRect& target, QPainter& painter)
+ void
+ MatrixDatafieldDisplayWidget::drawMatrix(const QRect& target, QPainter& painter)
{
int pixelSize = std::min(target.width() / data.cols(), target.height() / data.rows());
int dx = (target.width() - pixelSize * data.cols()) / 2;
@@ -139,7 +158,8 @@ namespace armarx
}
}
- void MatrixDatafieldDisplayWidget::drawPercentiles(const QRect& target, QPainter& painter)
+ void
+ MatrixDatafieldDisplayWidget::drawPercentiles(const QRect& target, QPainter& painter)
{
painter.setPen(QColor(Qt::GlobalColor::gray));
painter.drawRect(target);
@@ -151,7 +171,10 @@ namespace armarx
int x2 = (i + 1) * target.width() / (percentiles.size() - 1);
float y1 = (percentiles.at(i) - min) / (max - min) * target.height();
float y2 = (percentiles.at(i + 1) - min) / (max - min) * target.height();
- painter.drawLine(target.left() + x1, target.bottom() - (int)y1, target.left() + x2, target.bottom() - (int)y2);
+ painter.drawLine(target.left() + x1,
+ target.bottom() - (int)y1,
+ target.left() + x2,
+ target.bottom() - (int)y2);
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h
index 13f5d87da54ebf0ae578a008a3fdebd8f14a2a50..66847829bcc8174a8b59f9296cbebb4e69e4e3a4 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.h
@@ -23,10 +23,13 @@
*/
#pragma once
-#include <QWidget>
+#include <valarray>
+
#include <QMutex>
+#include <QWidget>
+
#include <Eigen/Dense>
-#include <valarray>
+
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
@@ -51,24 +54,30 @@ namespace armarx
void updateRequested();
public:
- explicit MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield, ObserverInterfacePrx observer, QWidget* parent = 0);
+ explicit MatrixDatafieldDisplayWidget(DatafieldRefBasePtr matrixDatafield,
+ ObserverInterfacePrx observer,
+ QWidget* parent = 0);
~MatrixDatafieldDisplayWidget() override;
void paintEvent(QPaintEvent*) override;
-
- void setRange(float min, float max)
+ void
+ setRange(float min, float max)
{
this->min = min;
this->max = max;
}
+
void enableOffsetFilter(bool enabled);
QColor getColor(float value, float min, float max);
- void invokeUpdate()
+ void
+ invokeUpdate()
{
emit doUpdate();
}
- void setInfoOverlay(QString infoOverlay)
+
+ void
+ setInfoOverlay(QString infoOverlay)
{
mtx.lock();
this->infoOverlay = infoOverlay;
@@ -92,5 +101,4 @@ namespace armarx
DatafieldRefPtr percentilesDatafield;
ObserverInterfacePrx observer;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
index 72b510ef5e5fe1400f09710a1018e7eaabba93bb..127b10e30d7e2b118ec6850ac60ce04f9dae9584 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.cpp
@@ -22,26 +22,32 @@
* GNU General Public License
*/
#include "MatrixDisplayWidget.h"
-#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_MatrixDisplayWidget.h>
-#include <QPainter>
-#include <pthread.h>
#include <iostream>
+#include <pthread.h>
+
+#include <QPainter>
+
+#include <RobotAPI/gui-plugins/HapticUnitPlugin/ui_MatrixDisplayWidget.h>
+
namespace armarx
{
MatrixDisplayWidget::MatrixDisplayWidget(QWidget* parent) :
- QWidget(parent),
- ui(new Ui::MatrixDisplayWidget)
+ QWidget(parent), ui(new Ui::MatrixDisplayWidget)
{
ui->setupUi(this);
this->min = 0;
this->max = 1;
this->data = MatrixXf(1, 1);
this->data(0, 0) = 0;
- QColor c[] = {QColor::fromHsv(0, 0, 0), QColor::fromHsv(240, 255, 255), QColor::fromHsv(270, 255, 255), QColor::fromHsv(300, 255, 255),
- QColor::fromHsv(0, 255, 255), QColor::fromHsv(30, 255, 255), QColor::fromHsv(60, 255, 255)
- };
+ QColor c[] = {QColor::fromHsv(0, 0, 0),
+ QColor::fromHsv(240, 255, 255),
+ QColor::fromHsv(270, 255, 255),
+ QColor::fromHsv(300, 255, 255),
+ QColor::fromHsv(0, 255, 255),
+ QColor::fromHsv(30, 255, 255),
+ QColor::fromHsv(60, 255, 255)};
this->colors = std::valarray<QColor>(c, sizeof c / sizeof c[0]);
//connect(this, SIGNAL(updateData(MatrixXf)), SLOT(setData(MatrixXf)), Qt::QueuedConnection);
@@ -53,7 +59,8 @@ namespace armarx
delete ui;
}
- void MatrixDisplayWidget::paintEvent(QPaintEvent*)
+ void
+ MatrixDisplayWidget::paintEvent(QPaintEvent*)
{
mtx.lock();
MatrixXf data = this->data;
@@ -83,7 +90,8 @@ namespace armarx
mtx.unlock();
}
- QColor MatrixDisplayWidget::getColor(float value, float min, float max)
+ QColor
+ MatrixDisplayWidget::getColor(float value, float min, float max)
{
value = (value - min) / (max - min) * (colors.size() - 1);
@@ -102,6 +110,8 @@ namespace armarx
float f1 = 1 - f2;
QColor c1 = colors[i];
QColor c2 = colors[i + 1];
- return QColor((int)(c1.red() * f1 + c2.red() * f2), (int)(c1.green() * f1 + c2.green() * f2), (int)(c1.blue() * f1 + c2.blue() * f2));
+ return QColor((int)(c1.red() * f1 + c2.red() * f2),
+ (int)(c1.green() * f1 + c2.green() * f2),
+ (int)(c1.blue() * f1 + c2.blue() * f2));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h
index 562c6f5665fbc88e8ca101a47da2c1fe0acf308b..8fe53fb996b9bc7219b9d43ae26e5fda394a72b5 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDisplayWidget.h
@@ -23,10 +23,12 @@
*/
#pragma once
-#include <QWidget>
+#include <valarray>
+
#include <QMutex>
+#include <QWidget>
+
#include <Eigen/Dense>
-#include <valarray>
using Eigen::MatrixXf;
@@ -45,7 +47,9 @@ namespace armarx
void doUpdate();
public slots:
- void setData(MatrixXf data)
+
+ void
+ setData(MatrixXf data)
{
mtx.lock();
this->data = data;
@@ -58,20 +62,23 @@ namespace armarx
~MatrixDisplayWidget() override;
void paintEvent(QPaintEvent*) override;
-
-
- void setRange(float min, float max)
+ void
+ setRange(float min, float max)
{
this->min = min;
this->max = max;
}
+
QColor getColor(float value, float min, float max);
- void invokeUpdate()
+ void
+ invokeUpdate()
{
emit doUpdate();
}
- void setInfoOverlay(QString infoOverlay)
+
+ void
+ setInfoOverlay(QString infoOverlay)
{
mtx.lock();
this->infoOverlay = infoOverlay;
@@ -86,5 +93,4 @@ namespace armarx
QMutex mtx;
QString infoOverlay;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.cpp b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.cpp
index 8356db80c6d90eed1526323c9e934ca133f02bec..1f3bcae7475aa620bb0530d13f5bc1f278d92b97 100644
--- a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.cpp
@@ -28,6 +28,6 @@ namespace armarx
{
HomogeneousMatrixCalculatorGuiPlugin::HomogeneousMatrixCalculatorGuiPlugin()
{
- addWidget < HomogeneousMatrixCalculatorWidgetController > ();
+ addWidget<HomogeneousMatrixCalculatorWidgetController>();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.h b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.h
index 302e9e0c76a45c44fe503c5373bc56258e0563f9..b5f2ddbc787bab4810c80370319a1100210477b8 100644
--- a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorGuiPlugin.h
@@ -23,8 +23,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -35,7 +36,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT HomogeneousMatrixCalculatorGuiPlugin:
+ class ARMARXCOMPONENT_IMPORT_EXPORT HomogeneousMatrixCalculatorGuiPlugin :
public armarx::ArmarXGuiPlugin
{
Q_OBJECT
@@ -48,5 +49,4 @@ namespace armarx
*/
HomogeneousMatrixCalculatorGuiPlugin();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp
index 78b41fd88ccc8fbe9a397be585af03ce28f1d483..b09c5c1d7d33f74ec4e7774e2d8f4caf6b84eb88 100644
--- a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.cpp
@@ -22,49 +22,57 @@
#include "HomogeneousMatrixCalculatorWidgetController.h"
-#include <Eigen/Dense>
-
#include <string>
-
#include <QDoubleValidator>
#include <QLineEdit>
+#include <Eigen/Dense>
+
using namespace armarx;
HomogeneousMatrixCalculatorWidgetController::HomogeneousMatrixCalculatorWidgetController()
{
widget.setupUi(getWidget());
- std::vector<QLineEdit*> edits
- {
- widget.lineEdit00, widget.lineEdit01, widget.lineEdit02, widget.lineEdit03,
- widget.lineEdit10, widget.lineEdit11, widget.lineEdit12, widget.lineEdit13,
- widget.lineEdit20, widget.lineEdit21, widget.lineEdit22, widget.lineEdit23,
- widget.lineEdit30, widget.lineEdit31, widget.lineEdit32, widget.lineEdit33,
-
- widget.lineEditI00, widget.lineEditI01, widget.lineEditI02, widget.lineEditI03,
- widget.lineEditI10, widget.lineEditI11, widget.lineEditI12, widget.lineEditI13,
- widget.lineEditI20, widget.lineEditI21, widget.lineEditI22, widget.lineEditI23,
- widget.lineEditI30, widget.lineEditI31, widget.lineEditI32, widget.lineEditI33,
-
- widget.lineEditICheck00, widget.lineEditICheck01, widget.lineEditICheck02, widget.lineEditICheck03,
- widget.lineEditICheck10, widget.lineEditICheck11, widget.lineEditICheck12, widget.lineEditICheck13,
- widget.lineEditICheck20, widget.lineEditICheck21, widget.lineEditICheck22, widget.lineEditICheck23,
- widget.lineEditICheck30, widget.lineEditICheck31, widget.lineEditICheck32, widget.lineEditICheck33,
-
- widget.lineEditX, widget.lineEditY, widget.lineEditZ,
- widget.lineEditRX, widget.lineEditRY, widget.lineEditRZ,
-
- widget.lineEditM00, widget.lineEditM01, widget.lineEditM02, widget.lineEditM03,
- widget.lineEditM10, widget.lineEditM11, widget.lineEditM12, widget.lineEditM13,
- widget.lineEditM20, widget.lineEditM21, widget.lineEditM22, widget.lineEditM23,
- widget.lineEditM30, widget.lineEditM31, widget.lineEditM32, widget.lineEditM33,
-
- widget.lineEditR00, widget.lineEditR01, widget.lineEditR02, widget.lineEditR03,
- widget.lineEditR10, widget.lineEditR11, widget.lineEditR12, widget.lineEditR13,
- widget.lineEditR20, widget.lineEditR21, widget.lineEditR22, widget.lineEditR23,
- widget.lineEditR30, widget.lineEditR31, widget.lineEditR32, widget.lineEditR33,
+ std::vector<QLineEdit*> edits{
+ widget.lineEdit00, widget.lineEdit01, widget.lineEdit02,
+ widget.lineEdit03, widget.lineEdit10, widget.lineEdit11,
+ widget.lineEdit12, widget.lineEdit13, widget.lineEdit20,
+ widget.lineEdit21, widget.lineEdit22, widget.lineEdit23,
+ widget.lineEdit30, widget.lineEdit31, widget.lineEdit32,
+ widget.lineEdit33,
+
+ widget.lineEditI00, widget.lineEditI01, widget.lineEditI02,
+ widget.lineEditI03, widget.lineEditI10, widget.lineEditI11,
+ widget.lineEditI12, widget.lineEditI13, widget.lineEditI20,
+ widget.lineEditI21, widget.lineEditI22, widget.lineEditI23,
+ widget.lineEditI30, widget.lineEditI31, widget.lineEditI32,
+ widget.lineEditI33,
+
+ widget.lineEditICheck00, widget.lineEditICheck01, widget.lineEditICheck02,
+ widget.lineEditICheck03, widget.lineEditICheck10, widget.lineEditICheck11,
+ widget.lineEditICheck12, widget.lineEditICheck13, widget.lineEditICheck20,
+ widget.lineEditICheck21, widget.lineEditICheck22, widget.lineEditICheck23,
+ widget.lineEditICheck30, widget.lineEditICheck31, widget.lineEditICheck32,
+ widget.lineEditICheck33,
+
+ widget.lineEditX, widget.lineEditY, widget.lineEditZ,
+ widget.lineEditRX, widget.lineEditRY, widget.lineEditRZ,
+
+ widget.lineEditM00, widget.lineEditM01, widget.lineEditM02,
+ widget.lineEditM03, widget.lineEditM10, widget.lineEditM11,
+ widget.lineEditM12, widget.lineEditM13, widget.lineEditM20,
+ widget.lineEditM21, widget.lineEditM22, widget.lineEditM23,
+ widget.lineEditM30, widget.lineEditM31, widget.lineEditM32,
+ widget.lineEditM33,
+
+ widget.lineEditR00, widget.lineEditR01, widget.lineEditR02,
+ widget.lineEditR03, widget.lineEditR10, widget.lineEditR11,
+ widget.lineEditR12, widget.lineEditR13, widget.lineEditR20,
+ widget.lineEditR21, widget.lineEditR22, widget.lineEditR23,
+ widget.lineEditR30, widget.lineEditR31, widget.lineEditR32,
+ widget.lineEditR33,
};
for (auto edit : edits)
@@ -72,7 +80,6 @@ HomogeneousMatrixCalculatorWidgetController::HomogeneousMatrixCalculatorWidgetCo
edit->setValidator(new QDoubleValidator(this));
edit->setFixedWidth(100);
edit->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
-
}
connect(widget.lineEdit00, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
connect(widget.lineEdit01, SIGNAL(textEdited(QString)), this, SLOT(changed4f()));
@@ -108,47 +115,48 @@ HomogeneousMatrixCalculatorWidgetController::HomogeneousMatrixCalculatorWidgetCo
connect(widget.lineEdit03, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
connect(widget.lineEdit13, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
connect(widget.lineEdit23, SIGNAL(textChanged(QString)), this, SLOT(recalcInvAndProd()));
-
}
+HomogeneousMatrixCalculatorWidgetController::~HomogeneousMatrixCalculatorWidgetController() =
+ default;
-HomogeneousMatrixCalculatorWidgetController::~HomogeneousMatrixCalculatorWidgetController()
- = default;
-
-
-void HomogeneousMatrixCalculatorWidgetController::loadSettings(QSettings* settings)
+void
+HomogeneousMatrixCalculatorWidgetController::loadSettings(QSettings* settings)
{
-
}
-void HomogeneousMatrixCalculatorWidgetController::saveSettings(QSettings* settings)
+void
+HomogeneousMatrixCalculatorWidgetController::saveSettings(QSettings* settings)
{
-
}
-
-void HomogeneousMatrixCalculatorWidgetController::onInitComponent()
+void
+HomogeneousMatrixCalculatorWidgetController::onInitComponent()
{
-
}
-
-void HomogeneousMatrixCalculatorWidgetController::onConnectComponent()
+void
+HomogeneousMatrixCalculatorWidgetController::onConnectComponent()
{
-
}
-Eigen::Matrix4d HomogeneousMatrixCalculatorWidgetController::getMatrix()
+Eigen::Matrix4d
+HomogeneousMatrixCalculatorWidgetController::getMatrix()
{
Eigen::Matrix4d m;
- m << widget.lineEdit00->text().toDouble(), widget.lineEdit01->text().toDouble(), widget.lineEdit02->text().toDouble(), widget.lineEdit03->text().toDouble(),
- widget.lineEdit10->text().toDouble(), widget.lineEdit11->text().toDouble(), widget.lineEdit12->text().toDouble(), widget.lineEdit13->text().toDouble(),
- widget.lineEdit20->text().toDouble(), widget.lineEdit21->text().toDouble(), widget.lineEdit22->text().toDouble(), widget.lineEdit23->text().toDouble(),
- widget.lineEdit30->text().toDouble(), widget.lineEdit31->text().toDouble(), widget.lineEdit32->text().toDouble(), widget.lineEdit33->text().toDouble();
+ m << widget.lineEdit00->text().toDouble(), widget.lineEdit01->text().toDouble(),
+ widget.lineEdit02->text().toDouble(), widget.lineEdit03->text().toDouble(),
+ widget.lineEdit10->text().toDouble(), widget.lineEdit11->text().toDouble(),
+ widget.lineEdit12->text().toDouble(), widget.lineEdit13->text().toDouble(),
+ widget.lineEdit20->text().toDouble(), widget.lineEdit21->text().toDouble(),
+ widget.lineEdit22->text().toDouble(), widget.lineEdit23->text().toDouble(),
+ widget.lineEdit30->text().toDouble(), widget.lineEdit31->text().toDouble(),
+ widget.lineEdit32->text().toDouble(), widget.lineEdit33->text().toDouble();
return m;
}
-void armarx::HomogeneousMatrixCalculatorWidgetController::changed4f()
+void
+armarx::HomogeneousMatrixCalculatorWidgetController::changed4f()
{
Eigen::Matrix4d m = getMatrix();
Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(m.cast<float>());
@@ -158,7 +166,8 @@ void armarx::HomogeneousMatrixCalculatorWidgetController::changed4f()
recalcInvAndProd();
}
-void armarx::HomogeneousMatrixCalculatorWidgetController::changedrpy()
+void
+armarx::HomogeneousMatrixCalculatorWidgetController::changedrpy()
{
float r = static_cast<float>(widget.lineEditRX->text().toDouble());
float p = static_cast<float>(widget.lineEditRY->text().toDouble());
@@ -176,7 +185,8 @@ void armarx::HomogeneousMatrixCalculatorWidgetController::changedrpy()
recalcInvAndProd();
}
-void HomogeneousMatrixCalculatorWidgetController::recalcInv()
+void
+HomogeneousMatrixCalculatorWidgetController::recalcInv()
{
Eigen::Matrix4d om = getMatrix();
Eigen::Matrix4d m = om.inverse();
@@ -215,13 +225,18 @@ void HomogeneousMatrixCalculatorWidgetController::recalcInv()
widget.lineEditICheck33->setText(QString::number(check(3, 3)));
}
-void HomogeneousMatrixCalculatorWidgetController::recalcProd()
+void
+HomogeneousMatrixCalculatorWidgetController::recalcProd()
{
Eigen::Matrix4d m;
- m << widget.lineEditM00->text().toDouble(), widget.lineEditM01->text().toDouble(), widget.lineEditM02->text().toDouble(), widget.lineEditM03->text().toDouble(),
- widget.lineEditM10->text().toDouble(), widget.lineEditM11->text().toDouble(), widget.lineEditM12->text().toDouble(), widget.lineEditM13->text().toDouble(),
- widget.lineEditM20->text().toDouble(), widget.lineEditM21->text().toDouble(), widget.lineEditM22->text().toDouble(), widget.lineEditM23->text().toDouble(),
- widget.lineEditM30->text().toDouble(), widget.lineEditM31->text().toDouble(), widget.lineEditM32->text().toDouble(), widget.lineEditM33->text().toDouble();
+ m << widget.lineEditM00->text().toDouble(), widget.lineEditM01->text().toDouble(),
+ widget.lineEditM02->text().toDouble(), widget.lineEditM03->text().toDouble(),
+ widget.lineEditM10->text().toDouble(), widget.lineEditM11->text().toDouble(),
+ widget.lineEditM12->text().toDouble(), widget.lineEditM13->text().toDouble(),
+ widget.lineEditM20->text().toDouble(), widget.lineEditM21->text().toDouble(),
+ widget.lineEditM22->text().toDouble(), widget.lineEditM23->text().toDouble(),
+ widget.lineEditM30->text().toDouble(), widget.lineEditM31->text().toDouble(),
+ widget.lineEditM32->text().toDouble(), widget.lineEditM33->text().toDouble();
Eigen::Matrix4d r = getMatrix() * m;
widget.lineEditR00->setText(QString::number(r(0, 0)));
widget.lineEditR01->setText(QString::number(r(0, 1)));
@@ -241,7 +256,8 @@ void HomogeneousMatrixCalculatorWidgetController::recalcProd()
widget.lineEditR33->setText(QString::number(r(3, 3)));
}
-void HomogeneousMatrixCalculatorWidgetController::recalcInvAndProd()
+void
+HomogeneousMatrixCalculatorWidgetController::recalcInvAndProd()
{
recalcInv();
recalcProd();
diff --git a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.h b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.h
index 104d0c89be3ff4f5a95511233efaad942330b1a4..608d48718bc19353fc9a010ea16248877bac9b79 100644
--- a/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.h
+++ b/source/RobotAPI/gui-plugins/HomogeneousMatrixCalculator/HomogeneousMatrixCalculatorWidgetController.h
@@ -22,14 +22,14 @@
#pragma once
-#include <RobotAPI/gui-plugins/HomogeneousMatrixCalculator/ui_HomogeneousMatrixCalculatorWidget.h>
-
#include <VirtualRobot/MathTools.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <RobotAPI/gui-plugins/HomogeneousMatrixCalculator/ui_HomogeneousMatrixCalculatorWidget.h>
namespace armarx
{
@@ -51,8 +51,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- HomogeneousMatrixCalculatorWidgetController:
+ class ARMARXCOMPONENT_IMPORT_EXPORT HomogeneousMatrixCalculatorWidgetController :
public ArmarXComponentWidgetControllerTemplate<HomogeneousMatrixCalculatorWidgetController>
{
Q_OBJECT
@@ -82,7 +81,8 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Util.HomogeneousMatrixCalculator";
}
@@ -99,11 +99,14 @@ namespace armarx
Eigen::Matrix4d getMatrix();
- static QIcon GetWidgetIcon()
+ static QIcon
+ GetWidgetIcon()
{
return QIcon("://icons/htmx_calc_icon.svg");
}
- static QIcon GetWidgetCategoryIcon()
+
+ static QIcon
+ GetWidgetCategoryIcon()
{
return QIcon("://icons/tools.svg");
}
@@ -130,5 +133,4 @@ namespace armarx
*/
Ui::HomogeneousMatrixCalculatorWidget widget;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
index f7693cd8a3fbe75738276da724fcf0ce549856d0..974891d7c39f02a4fe31d96b0513cdfeaa3fde4d 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
@@ -21,28 +21,26 @@
*/
#include "KinematicUnitConfigDialog.h"
-#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
-#include <QTimer>
-#include <QPushButton>
#include <QMessageBox>
+#include <QPushButton>
+#include <QTimer>
+#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_KinematicUnitConfigDialog.h>
-// ArmarX
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+// ArmarX
#include <IceUtil/UUID.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
using namespace armarx;
KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget* parent) :
- QDialog(parent),
- ui(new Ui::KinematicUnitConfigDialog),
- uuid(IceUtil::generateUUID())
+ QDialog(parent), ui(new Ui::KinematicUnitConfigDialog), uuid(IceUtil::generateUUID())
{
ui->setupUi(this);
@@ -54,8 +52,14 @@ KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget* parent) :
proxyFinder->setSearchMask("*KinematicUnit|KinematicUnit*");
ui->gridLayout->addWidget(proxyFinder, 0, 1, 1, 2);
- connect(proxyFinder->getProxyNameComboBox(), SIGNAL(currentIndexChanged(int)), this, SLOT(selectionChanged(int)));
- connect(proxyFinder->getProxyNameComboBox(), SIGNAL(editTextChanged(QString)), this, SLOT(proxyNameChanged(QString)));
+ connect(proxyFinder->getProxyNameComboBox(),
+ SIGNAL(currentIndexChanged(int)),
+ this,
+ SLOT(selectionChanged(int)));
+ connect(proxyFinder->getProxyNameComboBox(),
+ SIGNAL(editTextChanged(QString)),
+ this,
+ SLOT(proxyNameChanged(QString)));
}
KinematicUnitConfigDialog::~KinematicUnitConfigDialog()
@@ -63,23 +67,25 @@ KinematicUnitConfigDialog::~KinematicUnitConfigDialog()
delete ui;
}
-void KinematicUnitConfigDialog::onInitComponent()
+void
+KinematicUnitConfigDialog::onInitComponent()
{
proxyFinder->setIceManager(getIceManager());
}
-void KinematicUnitConfigDialog::onConnectComponent()
+void
+KinematicUnitConfigDialog::onConnectComponent()
{
-
}
-void KinematicUnitConfigDialog::onExitComponent()
+void
+KinematicUnitConfigDialog::onExitComponent()
{
QObject::disconnect();
-
}
-void KinematicUnitConfigDialog::verifyConfig()
+void
+KinematicUnitConfigDialog::verifyConfig()
{
if (proxyFinder->getSelectedProxyName().trimmed().length() == 0)
{
@@ -91,14 +97,16 @@ void KinematicUnitConfigDialog::verifyConfig()
}
}
-void KinematicUnitConfigDialog::updateSubconfig(std::string kinematicUnitName)
+void
+KinematicUnitConfigDialog::updateSubconfig(std::string kinematicUnitName)
{
try
{
ARMARX_INFO << "Connecting to KinematicUnitProxy " << kinematicUnitName;
- KinematicUnitInterfacePrx kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+ KinematicUnitInterfacePrx kinematicUnitInterfacePrx =
+ getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
std::string topicName = kinematicUnitInterfacePrx->getReportTopicName();
std::string robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
std::string rfile = kinematicUnitInterfacePrx->getRobotFilename();
@@ -112,7 +120,8 @@ void KinematicUnitConfigDialog::updateSubconfig(std::string kinematicUnitName)
}
}
-void KinematicUnitConfigDialog::selectionChanged(int nr)
+void
+KinematicUnitConfigDialog::selectionChanged(int nr)
{
ARMARX_LOG << "Selected entry:" << nr;
ui->labelTopic->setText("");
@@ -125,16 +134,13 @@ void KinematicUnitConfigDialog::selectionChanged(int nr)
std::string kinematicUnitName = proxyFinder->getSelectedProxyName().toStdString();
updateSubconfig(kinematicUnitName);
-
}
-void KinematicUnitConfigDialog::proxyNameChanged(QString kinematicUnitName)
+void
+KinematicUnitConfigDialog::proxyNameChanged(QString kinematicUnitName)
{
ui->labelTopic->setText("");
ui->labelRobotModel->setText("");
ui->labelRNS->setText("");
updateSubconfig(kinematicUnitName.toStdString());
}
-
-
-
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
index a3fee64e230937b74c5861176f17f11e7f3e0097..017eafc541df0b3e535a8c833b22623007bc24c8 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
@@ -25,9 +25,10 @@
#include <QDialog>
#include <QFileDialog>
-#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/IceManager.h>
#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/services/tasks/RunningTask.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
namespace Ui
@@ -37,9 +38,7 @@ namespace Ui
namespace armarx
{
- class KinematicUnitConfigDialog :
- public QDialog,
- virtual public ManagedIceObject
+ class KinematicUnitConfigDialog : public QDialog, virtual public ManagedIceObject
{
Q_OBJECT
@@ -48,10 +47,12 @@ namespace armarx
~KinematicUnitConfigDialog() override;
// inherited from ManagedIceObject
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "KinematicUnitConfigDialog" + uuid;
}
+
void onInitComponent() override;
void onConnectComponent() override;
void onExitComponent() override;
@@ -69,11 +70,9 @@ namespace armarx
void updateSubconfig(std::string kinematicUnitName);
private:
-
IceProxyFinderBase* proxyFinder;
Ui::KinematicUnitConfigDialog* ui;
std::string uuid;
friend class KinematicUnitWidgetController;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index a5c4e264d190647807344cfdca73a812ac6b3f57..13ac96809ec87016fda0d078fd79ceb5479ad074 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -25,6 +25,11 @@
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
#include <VirtualRobot/XML/RobotIO.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
@@ -44,13 +49,6 @@
#include <RobotAPI/interface/core/NameValueMap.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Visualization/VisualizationFactory.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
-
-
#include "KinematicUnitConfigDialog.h"
// Qt headers
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index f569285ac3ba1ccefd86a88d4e7cfa2db47865eb..9059bac86b4765bd51810305e72503bd8e8a2b68 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -24,39 +24,37 @@
#pragma once
/* ArmarX headers */
-#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_kinematicunitguiplugin.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include "ArmarXCore/core/services/tasks/RunningTask.h"
+#include <ArmarXCore/core/Component.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include "ArmarXCore/core/services/tasks/RunningTask.h"
-#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+#include <RobotAPI/gui-plugins/KinematicUnitPlugin/ui_kinematicunitguiplugin.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
/* Qt headers */
-#include <QMainWindow>
-#include <QToolBar>
-
+#include <mutex>
-#include <Inventor/sensors/SoTimerSensor.h>
-#include <Inventor/nodes/SoNode.h>
-#include <Inventor/nodes/SoSeparator.h>
-#include <Inventor/nodes/SoEventCallback.h>
-#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
-#include <Inventor/Qt/SoQt.h>
+#include <boost/circular_buffer.hpp>
+#include <QMainWindow>
#include <QStyledItemDelegate>
-
-#include <ArmarXCore/core/util/IceReportSkipper.h>
+#include <QToolBar>
#include <VirtualRobot/VirtualRobot.h>
-#include <mutex>
-#include <boost/circular_buffer.hpp>
-
#include <ArmarXCore/core/time.h>
+#include <ArmarXCore/core/util/IceReportSkipper.h>
+
+#include <Inventor/Qt/SoQt.h>
+#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
+#include <Inventor/nodes/SoEventCallback.h>
+#include <Inventor/nodes/SoNode.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/sensors/SoTimerSensor.h>
namespace armarx
{
@@ -67,8 +65,7 @@ namespace armarx
\brief This plugin provides a generic widget showing position and velocity of all joints. Optionally a 3d robot model can be visualized.
\see KinematicUnitWidget
*/
- class KinematicUnitGuiPlugin :
- public ArmarXGuiPlugin
+ class KinematicUnitGuiPlugin : public ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -76,16 +73,18 @@ namespace armarx
public:
KinematicUnitGuiPlugin();
- QString getPluginName() override
+ QString
+ getPluginName() override
{
return "KinematicUnitGuiPlugin";
}
};
- class RangeValueDelegate :
- public QStyledItemDelegate
+ class RangeValueDelegate : public QStyledItemDelegate
{
- void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override;
+ void paint(QPainter* painter,
+ const QStyleOptionViewItem& option,
+ const QModelIndex& index) const override;
};
/*!
@@ -111,7 +110,7 @@ namespace armarx
*/
class KinematicUnitWidgetController :
public ArmarXComponentWidgetControllerTemplate<KinematicUnitWidgetController>
- // public KinematicUnitListener
+ // public KinematicUnitListener
{
Q_OBJECT
public:
@@ -152,11 +151,14 @@ namespace armarx
void onExitComponent() override;
// inherited of ArmarXWidget
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "RobotControl.KinematicUnitGUI";
}
- static QIcon GetWidgetIcon()
+
+ static QIcon
+ GetWidgetIcon()
{
return QIcon("://icons/kinematic_icon.svg");
}
@@ -215,7 +217,8 @@ namespace armarx
void updateJointAnglesTable(const NameValueMap& reportedJointAngles);
void updateJointVelocitiesTable(const NameValueMap& reportedJointVelocities);
void updateJointTorquesTable(const NameValueMap& reportedJointTorques);
- void updateJointCurrentsTable(const NameValueMap& reportedJointCurrents, const NameStatusMap& reportedJointStatuses);
+ void updateJointCurrentsTable(const NameValueMap& reportedJointCurrents,
+ const NameStatusMap& reportedJointStatuses);
void updateMotorTemperaturesTable(const NameValueMap& reportedMotorTemperatures);
void updateJointStatusesTable(const NameStatusMap& reportedJointStatuses);
void updateControlModesTable(const NameControlModeMap& reportedJointControlModes);
@@ -235,7 +238,7 @@ namespace armarx
Ui::KinematicUnitGuiPlugin ui;
// ice proxies
- KinematicUnitInterfacePrx kinematicUnitInterfacePrx;// send commands to kinematic unit
+ KinematicUnitInterfacePrx kinematicUnitInterfacePrx; // send commands to kinematic unit
bool verbose;
@@ -273,12 +276,12 @@ namespace armarx
private:
-
std::recursive_mutex mutexNodeSet;
// init stuff
VirtualRobot::RobotPtr loadRobotFile(std::string fileName);
VirtualRobot::CoinVisualizationPtr getCoinVisualization(VirtualRobot::RobotPtr robot);
- VirtualRobot::RobotNodeSetPtr getRobotNodeSet(VirtualRobot::RobotPtr robot, std::string nodeSetName);
+ VirtualRobot::RobotNodeSetPtr getRobotNodeSet(VirtualRobot::RobotPtr robot,
+ std::string nodeSetName);
bool initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet);
bool initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet);
@@ -318,10 +321,9 @@ namespace armarx
ControlMode getSelectedControlMode() const;
void setControlModeRadioButtonGroup(const ControlMode& controlMode);
-
};
- using FloatVector = ::std::vector< ::Ice::Float>;
+ using FloatVector = ::std::vector<::Ice::Float>;
using KinematicUnitGuiPluginPtr = std::shared_ptr<KinematicUnitWidgetController>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.cpp b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.cpp
index 2cf355c5645dab13b884bdf7bebb725a96525486..df3ca073e4c07c7ede07a68c29c194b665a92533 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.cpp
@@ -28,5 +28,5 @@ using namespace armarx;
LaserScannerPluginGuiPlugin::LaserScannerPluginGuiPlugin()
{
- addWidget < LaserScannerPluginWidgetController > ();
+ addWidget<LaserScannerPluginWidgetController>();
}
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.h b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.h
index 8422e95dc91d127d3019044c3198c8576d73e7e3..8c373095e44b515545e982f806940fab367bb3cf 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginGuiPlugin.h
@@ -23,8 +23,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -35,8 +36,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT LaserScannerPluginGuiPlugin:
- public armarx::ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT LaserScannerPluginGuiPlugin : public armarx::ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -48,5 +48,4 @@ namespace armarx
*/
LaserScannerPluginGuiPlugin();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
index 5f391cb974af80cf9f724f7ecf530e2603c2ba51..e7523807edee08969c966b824ab34ed74e026b3a 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.cpp
@@ -22,11 +22,11 @@
#include "LaserScannerPluginWidgetController.h"
-#include <QGraphicsView>
-#include <QGraphicsLineItem>
-
#include <string>
+#include <QGraphicsLineItem>
+#include <QGraphicsView>
+
using namespace armarx;
LaserScannerPluginWidgetController::LaserScannerPluginWidgetController()
@@ -34,34 +34,36 @@ LaserScannerPluginWidgetController::LaserScannerPluginWidgetController()
widget.setupUi(getWidget());
}
-
LaserScannerPluginWidgetController::~LaserScannerPluginWidgetController()
{
-
}
-
-void LaserScannerPluginWidgetController::loadSettings(QSettings* settings)
+void
+LaserScannerPluginWidgetController::loadSettings(QSettings* settings)
{
-
}
-void LaserScannerPluginWidgetController::saveSettings(QSettings* settings)
+void
+LaserScannerPluginWidgetController::saveSettings(QSettings* settings)
{
-
}
-
-void LaserScannerPluginWidgetController::onInitComponent()
+void
+LaserScannerPluginWidgetController::onInitComponent()
{
usingProxy(laserScannerUnitName);
- connect(this, SIGNAL(newSensorValuesReported()), this, SLOT(onNewSensorValuesReported()), Qt::QueuedConnection);
- connect(widget.deviceComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(onDeviceSelected(int)));
+ connect(this,
+ SIGNAL(newSensorValuesReported()),
+ this,
+ SLOT(onNewSensorValuesReported()),
+ Qt::QueuedConnection);
+ connect(
+ widget.deviceComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(onDeviceSelected(int)));
}
-
-void LaserScannerPluginWidgetController::onConnectComponent()
+void
+LaserScannerPluginWidgetController::onConnectComponent()
{
laserScannerUnit = getProxy<LaserScannerUnitInterfacePrx>(laserScannerUnitName);
std::string topicName = laserScannerUnit->getReportTopicName();
@@ -70,7 +72,8 @@ void LaserScannerPluginWidgetController::onConnectComponent()
laserScanners = laserScannerUnit->getConnectedDevices();
}
-QPointer<QDialog> LaserScannerPluginWidgetController::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+LaserScannerPluginWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
@@ -80,7 +83,8 @@ QPointer<QDialog> LaserScannerPluginWidgetController::getConfigDialog(QWidget* p
return qobject_cast<SimpleConfigDialog*>(dialog);
}
-void LaserScannerPluginWidgetController::configured()
+void
+LaserScannerPluginWidgetController::configured()
{
if (dialog)
{
@@ -88,7 +92,12 @@ void LaserScannerPluginWidgetController::configured()
}
}
-void LaserScannerPluginWidgetController::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& newScan, const TimestampBasePtr& timestamp, const Ice::Current& c)
+void
+LaserScannerPluginWidgetController::reportSensorValues(const std::string& device,
+ const std::string& name,
+ const LaserScan& newScan,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c)
{
{
std::unique_lock lock(scanMutex);
@@ -101,7 +110,8 @@ void LaserScannerPluginWidgetController::reportSensorValues(const std::string& d
emit newSensorValuesReported();
}
-void LaserScannerPluginWidgetController::onNewSensorValuesReported()
+void
+LaserScannerPluginWidgetController::onNewSensorValuesReported()
{
QComboBox* deviceBox = widget.deviceComboBox;
@@ -139,7 +149,8 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
auto line = [&](float angle, float d)
{
float di = d * r;
- QGraphicsEllipseItem* item = scene->addEllipse(-di, -di, 2 * di, 2 * di, stepPen, stepBrush);
+ QGraphicsEllipseItem* item =
+ scene->addEllipse(-di, -di, 2 * di, 2 * di, stepPen, stepBrush);
// Angles for Qt ellipse are in 16th of degree (who thought that would be a great idea?)
item->setStartAngle(std::round(16.0f * angle * 180.0 / M_PI) + 90 * 16);
item->setSpanAngle(std::round(stepSize * 16.0f * 180.0 / M_PI));
@@ -172,14 +183,15 @@ void LaserScannerPluginWidgetController::onNewSensorValuesReported()
for (int ringIndex = 1; ringIndex <= maxNumberOfRings; ++ringIndex)
{
- float ri = 1.0f * ringIndex / maxNumberOfRings * r;
+ float ri = 1.0f * ringIndex / maxNumberOfRings * r;
scene->addEllipse(-ri, -ri, 2 * ri, 2 * ri, QPen(QColor(200, 200, 200)));
}
view->fitInView(scene->itemsBoundingRect(), Qt::KeepAspectRatio);
}
-void LaserScannerPluginWidgetController::onDeviceSelected(int index)
+void
+LaserScannerPluginWidgetController::onDeviceSelected(int index)
{
if (index < 0 && index >= widget.deviceComboBox->count())
{
@@ -196,5 +208,4 @@ void LaserScannerPluginWidgetController::onDeviceSelected(int index)
widget.stepSizeLabel->setText(QString::number(scanner.stepSize));
}
}
-
}
diff --git a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
index 6d87e4fc65c46a9045e13383bb39b8700d1446d2..b478ce4b913e063911f980699a796cd6a67c1a54 100644
--- a/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
+++ b/source/RobotAPI/gui-plugins/LaserScannerPlugin/LaserScannerPluginWidgetController.h
@@ -22,16 +22,16 @@
#pragma once
-#include <RobotAPI/gui-plugins/LaserScannerPlugin/ui_LaserScannerPluginWidget.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <mutex>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
-#include <mutex>
+#include <RobotAPI/gui-plugins/LaserScannerPlugin/ui_LaserScannerPluginWidget.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
namespace armarx
{
@@ -50,8 +50,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- LaserScannerPluginWidgetController:
+ class ARMARXCOMPONENT_IMPORT_EXPORT LaserScannerPluginWidgetController :
public ArmarXComponentWidgetControllerTemplate<LaserScannerPluginWidgetController>,
public armarx::LaserScannerUnitListener
{
@@ -82,7 +81,8 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "RobotControl.LaserScannerGUI";
}
@@ -101,8 +101,10 @@ namespace armarx
void configured() override;
- void reportSensorValues(const std::string& device, const std::string& name,
- const LaserScan& scan, const TimestampBasePtr& timestamp,
+ void reportSensorValues(const std::string& device,
+ const std::string& name,
+ const LaserScan& scan,
+ const TimestampBasePtr& timestamp,
const Ice::Current& c) override;
public slots:
void onNewSensorValuesReported();
@@ -129,5 +131,4 @@ namespace armarx
std::unique_ptr<QGraphicsScene> scene;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.cpp
index 01cff7dcbe628fc900c62a5ad74c75b3aa6fab4d..c04b194330049097d933fcc12ba313b549bddf40 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.cpp
@@ -28,6 +28,6 @@ namespace armarx
{
ObjectPoseGuiPlugin::ObjectPoseGuiPlugin()
{
- addWidget < ObjectPoseGuiWidgetController > ();
+ addWidget<ObjectPoseGuiWidgetController>();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.h b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.h
index 69625e6dc30760ebca74ba87db6b9338316185ac..ebe9735005d95b5677a643a6098f2093bf1ad191 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiPlugin.h
@@ -22,8 +22,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -34,8 +35,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT ObjectPoseGuiPlugin:
- public armarx::ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT ObjectPoseGuiPlugin : public armarx::ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -47,4 +47,4 @@ namespace armarx
*/
ObjectPoseGuiPlugin();
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp
index cd15a2e3a54a9010ee38c1486986b8f3c890da11..e4d4500977eab069aafd17bb019970b8bc13a0a1 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.cpp
@@ -21,15 +21,13 @@
*/
#include "PlatformUnitConfigDialog.h"
-#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitConfigDialog.h>
#include <IceUtil/UUID.h>
+#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitConfigDialog.h>
armarx::PlatformUnitConfigDialog::PlatformUnitConfigDialog(QWidget* parent) :
- QDialog(parent),
- ui(new Ui::PlatformUnitConfigDialog),
- uuid(IceUtil::generateUUID())
+ QDialog(parent), ui(new Ui::PlatformUnitConfigDialog), uuid(IceUtil::generateUUID())
{
ui->setupUi(this);
finder = new IceProxyFinder<PlatformUnitInterfacePrx>(this);
@@ -42,18 +40,19 @@ armarx::PlatformUnitConfigDialog::~PlatformUnitConfigDialog()
delete ui;
}
-
-
-void armarx::PlatformUnitConfigDialog::onInitComponent()
+void
+armarx::PlatformUnitConfigDialog::onInitComponent()
{
finder->setIceManager(getIceManager());
}
-void armarx::PlatformUnitConfigDialog::onConnectComponent()
+void
+armarx::PlatformUnitConfigDialog::onConnectComponent()
{
}
-std::string armarx::PlatformUnitConfigDialog::getDefaultName() const
+std::string
+armarx::PlatformUnitConfigDialog::getDefaultName() const
{
return "PlatformUnitConfigDialog" + uuid;
}
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h
index 89b829f8cfa49734b420deb353fbbbf2a8c91c9e..0cdf61bf8d228046cae710eae3fef81df2491209 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitConfigDialog.h
@@ -25,7 +25,9 @@
#include <QDialog>
#include <ArmarXCore/core/ManagedIceObject.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
+
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
namespace Ui
@@ -35,9 +37,7 @@ namespace Ui
namespace armarx
{
- class PlatformUnitConfigDialog :
- public QDialog,
- virtual public ManagedIceObject
+ class PlatformUnitConfigDialog : public QDialog, virtual public ManagedIceObject
{
Q_OBJECT
@@ -57,7 +57,5 @@ namespace armarx
void onInitComponent() override;
void onConnectComponent() override;
std::string getDefaultName() const override;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index a7c2374677fde81a140b3479a3dd626101d79525..5f753c22add36e3b73e94b429a9b7813c953fed2 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
@@ -22,44 +22,46 @@
* GNU General Public License
*/
#include "PlatformUnitGuiPlugin.h"
-#include "PlatformUnitConfigDialog.h"
-#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitConfigDialog.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <Eigen/Geometry>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitConfigDialog.h>
+
+#include "PlatformUnitConfigDialog.h"
+
// Qt headers
-#include <Qt>
-#include <QtGlobal>
-#include <QPushButton>
+#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
-#include <QHBoxLayout>
+#include <QPushButton>
+#include <Qt>
+#include <QtGlobal>
//std
-#include <RobotAPI/interface/core/RobotLocalization.h>
-#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
-#include <memory>
#include <cmath>
+#include <memory>
-using namespace armarx;
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
+#include <RobotAPI/interface/core/RobotLocalization.h>
+using namespace armarx;
PlatformUnitGuiPlugin::PlatformUnitGuiPlugin()
{
addWidget<PlatformUnitWidget>();
}
-
PlatformUnitWidget::PlatformUnitWidget() :
- platformUnitProxyName("PlatformUnit"), // overwritten in loadSettings() anyway?
+ platformUnitProxyName("PlatformUnit"), // overwritten in loadSettings() anyway?
platformName("Platform"),
- speedCtrl {nullptr},
- rotaCtrl {nullptr},
- ctrlEvaluationTimer {},
- platformRotation {0},
- platformMoves {false}
+ speedCtrl{nullptr},
+ rotaCtrl{nullptr},
+ ctrlEvaluationTimer{},
+ platformRotation{0},
+ platformMoves{false}
{
// init gui
ui.setupUi(getWidget());
@@ -82,39 +84,44 @@ PlatformUnitWidget::PlatformUnitWidget() :
connect(&stopPlatformTimer, SIGNAL(timeout()), this, SLOT(stopPlatform()));
- connect(getWidget().data(), SIGNAL(commandKeyPressed(int)), this, SLOT(controlPlatformWithKeyboard(int)));
- connect(getWidget().data(), SIGNAL(commandKeyReleased(int)), this, SLOT(stopPlatformWithKeyboard(int)));
+ connect(getWidget().data(),
+ SIGNAL(commandKeyPressed(int)),
+ this,
+ SLOT(controlPlatformWithKeyboard(int)));
+ connect(getWidget().data(),
+ SIGNAL(commandKeyReleased(int)),
+ this,
+ SLOT(stopPlatformWithKeyboard(int)));
}
-
-void PlatformUnitWidget::onInitComponent()
+void
+PlatformUnitWidget::onInitComponent()
{
usingProxy(platformUnitProxyName);
usingTopic("GlobalRobotPoseLocalization");
// ARMARX_INFO << "Listening on Topic: " << platformName + "State";
-
}
-
-void PlatformUnitWidget::onConnectComponent()
+void
+PlatformUnitWidget::onConnectComponent()
{
platformUnitProxy = getProxy<PlatformUnitInterfacePrx>(platformUnitProxyName);
connectSlots();
}
-
-void PlatformUnitWidget::onDisconnectComponent()
+void
+PlatformUnitWidget::onDisconnectComponent()
{
stopControlTimer();
}
-
-void PlatformUnitWidget::onExitComponent()
+void
+PlatformUnitWidget::onExitComponent()
{
}
-
-QPointer<QDialog> PlatformUnitWidget::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+PlatformUnitWidget::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
@@ -125,46 +132,62 @@ QPointer<QDialog> PlatformUnitWidget::getConfigDialog(QWidget* parent)
return qobject_cast<PlatformUnitConfigDialog*>(dialog);
}
-
-void PlatformUnitWidget::configured()
+void
+PlatformUnitWidget::configured()
{
platformUnitProxyName = dialog->finder->getSelectedProxyName().toStdString();
platformName = dialog->ui->editPlatformName->text().toStdString();
}
-
-void PlatformUnitWidget::loadSettings(QSettings* settings)
+void
+PlatformUnitWidget::loadSettings(QSettings* settings)
{
- platformUnitProxyName = settings->value("platformUnitProxyName", QString::fromStdString(platformUnitProxyName)).toString().toStdString();
- platformName = settings->value("platformName", QString::fromStdString(platformName)).toString().toStdString();
+ platformUnitProxyName =
+ settings->value("platformUnitProxyName", QString::fromStdString(platformUnitProxyName))
+ .toString()
+ .toStdString();
+ platformName = settings->value("platformName", QString::fromStdString(platformName))
+ .toString()
+ .toStdString();
}
-
-void PlatformUnitWidget::saveSettings(QSettings* settings)
+void
+PlatformUnitWidget::saveSettings(QSettings* settings)
{
settings->setValue("platformUnitProxyName", QString::fromStdString(platformUnitProxyName));
settings->setValue("platformName", QString::fromStdString(platformName));
}
-
-void PlatformUnitWidget::connectSlots()
+void
+PlatformUnitWidget::connectSlots()
{
connect(ui.buttonMoveToPosition, SIGNAL(clicked()), this, SLOT(moveTo()), Qt::UniqueConnection);
- connect(&ctrlEvaluationTimer, SIGNAL(timeout()), this, SLOT(controlTimerTick()), Qt::UniqueConnection);
- connect(&keyboardVelocityTimer, SIGNAL(timeout()), this, SLOT(keyboardVelocityControl()), Qt::UniqueConnection);
+ connect(&ctrlEvaluationTimer,
+ SIGNAL(timeout()),
+ this,
+ SLOT(controlTimerTick()),
+ Qt::UniqueConnection);
+ connect(&keyboardVelocityTimer,
+ SIGNAL(timeout()),
+ this,
+ SLOT(keyboardVelocityControl()),
+ Qt::UniqueConnection);
connect(speedCtrl, SIGNAL(pressed()), this, SLOT(startControlTimer()), Qt::UniqueConnection);
- connect(speedCtrl, SIGNAL(pressed()), &keyboardVelocityTimer, SLOT(stop()), Qt::UniqueConnection);
+ connect(
+ speedCtrl, SIGNAL(pressed()), &keyboardVelocityTimer, SLOT(stop()), Qt::UniqueConnection);
connect(rotaCtrl, SIGNAL(pressed()), this, SLOT(startControlTimer()), Qt::UniqueConnection);
- connect(rotaCtrl, SIGNAL(pressed()), &keyboardVelocityTimer, SLOT(stop()), Qt::UniqueConnection);
+ connect(
+ rotaCtrl, SIGNAL(pressed()), &keyboardVelocityTimer, SLOT(stop()), Qt::UniqueConnection);
connect(speedCtrl, SIGNAL(released()), this, SLOT(stopPlatform()), Qt::UniqueConnection);
connect(speedCtrl, SIGNAL(released()), this, SLOT(stopControlTimer()), Qt::UniqueConnection);
connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopPlatform()), Qt::UniqueConnection);
connect(rotaCtrl, SIGNAL(released()), this, SLOT(stopControlTimer()), Qt::UniqueConnection);
- connect(ui.buttonStopPlatform, SIGNAL(pressed()), this, SLOT(stopPlatform()), Qt::UniqueConnection);
+ connect(
+ ui.buttonStopPlatform, SIGNAL(pressed()), this, SLOT(stopPlatform()), Qt::UniqueConnection);
}
-
-void PlatformUnitWidget::moveTo()
+void
+PlatformUnitWidget::moveTo()
{
ARMARX_LOG << "Moving Platform";
::Ice::Float positionX = ui.editTargetPositionX->text().toFloat();
@@ -175,38 +198,39 @@ void PlatformUnitWidget::moveTo()
platformUnitProxy->moveTo(positionX, positionY, rotation, posAcc, rotAcc);
}
-
-void PlatformUnitWidget::setNewPlatformPoseLabels(float x, float y, float alpha)
+void
+PlatformUnitWidget::setNewPlatformPoseLabels(float x, float y, float alpha)
{
ui.labelCurrentPositionX->setText(QString::number(x));
ui.labelCurrentPositionY->setText(QString::number(y));
ui.labelCurrentRotation->setText(QString::number(alpha));
}
-
-void PlatformUnitWidget::setNewTargetPoseLabels(float x, float y, float alpha)
+void
+PlatformUnitWidget::setNewTargetPoseLabels(float x, float y, float alpha)
{
ui.editTargetPositionX->setText(QString::number(x));
ui.editTargetPositionY->setText(QString::number(y));
ui.editTargetRotation->setText(QString::number(alpha));
}
-
-void PlatformUnitWidget::startControlTimer()
+void
+PlatformUnitWidget::startControlTimer()
{
- ctrlEvaluationTimer.start(CONTROL_TICK_RATE); //tickrate in ms
+ ctrlEvaluationTimer.start(CONTROL_TICK_RATE); //tickrate in ms
}
-
-void PlatformUnitWidget::stopControlTimer()
+void
+PlatformUnitWidget::stopControlTimer()
{
ctrlEvaluationTimer.stop();
speedCtrl->setNibble({0, 0});
rotaCtrl->setNibble({0, 0});
}
-
-void PlatformUnitWidget::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&)
+void
+PlatformUnitWidget::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped,
+ const ::Ice::Current&)
{
const Eigen::Isometry3f global_T_robot(transformStamped.transform);
const float x = global_T_robot.translation().x();
@@ -214,18 +238,19 @@ void PlatformUnitWidget::reportGlobalRobotPose(const ::armarx::TransformStamped&
const float yaw = simox::math::mat3f_to_rpy(global_T_robot.linear()).z();
// moved to qt thread for thread safety
- QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, x), Q_ARG(float, y), Q_ARG(float, yaw));
+ QMetaObject::invokeMethod(
+ this, "setNewPlatformPoseLabels", Q_ARG(float, x), Q_ARG(float, y), Q_ARG(float, yaw));
platformRotation = yaw;
}
-
-void PlatformUnitWidget::stopPlatform()
+void
+PlatformUnitWidget::stopPlatform()
{
platformUnitProxy->stopPlatform();
}
-
-void PlatformUnitWidget::controlPlatformWithKeyboard(int key)
+void
+PlatformUnitWidget::controlPlatformWithKeyboard(int key)
{
pressedKeys.insert(key);
if (!ctrlEvaluationTimer.isActive())
@@ -239,8 +264,8 @@ void PlatformUnitWidget::controlPlatformWithKeyboard(int key)
}
}
-
-void PlatformUnitWidget::stopPlatformWithKeyboard(int key)
+void
+PlatformUnitWidget::stopPlatformWithKeyboard(int key)
{
pressedKeys.remove(key);
@@ -251,8 +276,8 @@ void PlatformUnitWidget::stopPlatformWithKeyboard(int key)
}
}
-
-void PlatformUnitWidget::keyboardVelocityControl()
+void
+PlatformUnitWidget::keyboardVelocityControl()
{
if (!pressedKeys.contains(Qt::Key_A) && !pressedKeys.contains(Qt::Key_D))
{
@@ -318,8 +343,8 @@ void PlatformUnitWidget::keyboardVelocityControl()
rotaCtrl->setNibble(QPointF(currentKeyboardVelocityAlpha, -y));
}
-
-QPointer<QWidget> PlatformUnitWidget::getWidget()
+QPointer<QWidget>
+PlatformUnitWidget::getWidget()
{
if (!__widget)
{
@@ -329,31 +354,30 @@ QPointer<QWidget> PlatformUnitWidget::getWidget()
return __widget;
}
-
-void PlatformUnitWidget::controlTimerTick()
+void
+PlatformUnitWidget::controlTimerTick()
{
float translationFactor = ui.maxTranslationSpeed->value();
float rotationFactor = ui.maxRotationSpeed->value() * -1;
- float rotationVel = rotaCtrl->getRotation() / M_PI_2 * rotationFactor;
- ARMARX_INFO << deactivateSpam(0.5)
- << "Translation speed: (" << speedCtrl->getPosition().x() * translationFactor
- << ", " << speedCtrl->getPosition().y() * translationFactor << ")"
- << ", \t rotation speed: " << (rotationVel);
+ float rotationVel = rotaCtrl->getRotation() / M_PI_2 * rotationFactor;
+ ARMARX_INFO << deactivateSpam(0.5) << "Translation speed: ("
+ << speedCtrl->getPosition().x() * translationFactor << ", "
+ << speedCtrl->getPosition().y() * translationFactor << ")"
+ << ", \t rotation speed: " << (rotationVel);
platformUnitProxy->move(speedCtrl->getPosition().x() * translationFactor,
-1 * speedCtrl->getPosition().y() * translationFactor,
rotationVel);
- if (speedCtrl->getPosition().x() == 0
- && speedCtrl->getPosition().y() == 0
- && rotaCtrl->getRotation() == 0)
+ if (speedCtrl->getPosition().x() == 0 && speedCtrl->getPosition().y() == 0 &&
+ rotaCtrl->getRotation() == 0)
{
stopControlTimer();
}
}
-
-void KeyboardPlatformHookWidget::keyPressEvent(QKeyEvent* event)
+void
+KeyboardPlatformHookWidget::keyPressEvent(QKeyEvent* event)
{
switch (event->key())
{
@@ -371,8 +395,8 @@ void KeyboardPlatformHookWidget::keyPressEvent(QKeyEvent* event)
QWidget::keyPressEvent(event);
}
-
-void KeyboardPlatformHookWidget::keyReleaseEvent(QKeyEvent* event)
+void
+KeyboardPlatformHookWidget::keyReleaseEvent(QKeyEvent* event)
{
switch (event->key())
{
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
index 7587750d212e51f88666dffcf236556370b432c3..9a146ed9fe0648c4a87532b23e215884d865ebc7 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
@@ -24,21 +24,21 @@
#pragma once
/* ArmarX headers */
-#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/JoystickControlWidget.h>
#include <ArmarXCore/core/Component.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/JoystickControlWidget.h>
+#include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitGuiPlugin.h>
#include <RobotAPI/interface/core/RobotLocalization.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
/* Qt headers */
-#include <QMainWindow>
-#include <QTimer>
-
#include <string>
+#include <QMainWindow>
+#include <QTimer>
namespace armarx
{
@@ -47,7 +47,7 @@ namespace armarx
{
Q_OBJECT
public:
- KeyboardPlatformHookWidget(QWidget* parent = NULL): QWidget(parent)
+ KeyboardPlatformHookWidget(QWidget* parent = NULL) : QWidget(parent)
{
setFocusPolicy(Qt::ClickFocus);
}
@@ -69,15 +69,16 @@ namespace armarx
\brief This plugin provides a widget with which the PlatformUnit can be controlled.
\see PlatformUnitWidget
*/
- class PlatformUnitGuiPlugin :
- public ArmarXGuiPlugin
+ class PlatformUnitGuiPlugin : public ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
public:
PlatformUnitGuiPlugin();
- QString getPluginName() override
+
+ QString
+ getPluginName() override
{
return "PlatformUnitGuiPlugin";
}
@@ -107,8 +108,10 @@ namespace armarx
Q_OBJECT
public:
PlatformUnitWidget();
+
~PlatformUnitWidget() override
- {}
+ {
+ }
// inherited from Component
void onInitComponent() override;
@@ -117,14 +120,18 @@ namespace armarx
void onExitComponent() override;
// slice interface implementation
- void reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current& = ::Ice::emptyCurrent) override;
-
+ void reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped,
+ const ::Ice::Current& = ::Ice::emptyCurrent) override;
+
// inherited of ArmarXWidget
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "RobotControl.PlatformUnitGUI";
}
- static QIcon GetWidgetIcon()
+
+ static QIcon
+ GetWidgetIcon()
{
return QIcon("://icons/retro_joystick2.svg");
}
@@ -222,7 +229,7 @@ namespace armarx
// ArmarXWidgetController interface
public:
QPointer<QWidget> getWidget() override;
-
};
+
using PlatformUnitGuiPluginPtr = std::shared_ptr<PlatformUnitWidget>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.cpp
index f3b9fc45f7eb16532ef018be82b974d67f62fec3..fa0367ae1fb0881f616f0883d1efa459ce3eb827 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.cpp
@@ -21,10 +21,10 @@
*/
#include "ControlDevicesWidget.h"
-#include "StyleSheets.h"
-
#include <QCheckBox>
+#include "StyleSheets.h"
+
namespace armarx
{
ControlDevicesWidget::ControlDevicesWidget(QWidget* parent) :
@@ -32,31 +32,38 @@ namespace armarx
{
ui->treeWidget->setColumnCount(8);
QTreeWidgetItem* head = ui->treeWidget->headerItem();
- head->setText(idxName, "Name");
- head->setText(idxTags, "Tags");
- head->setText(idxMode, "Mode");
+ head->setText(idxName, "Name");
+ head->setText(idxTags, "Tags");
+ head->setText(idxMode, "Mode");
head->setText(idxHWMode, "HardwareMode");
- head->setText(idxAct, "Act");
- head->setText(idxReq, "Req");
- head->setText(idxType, "Target Type");
- head->setText(idxVal, "Target Value");
+ head->setText(idxAct, "Act");
+ head->setText(idxReq, "Req");
+ head->setText(idxType, "Target Type");
+ head->setText(idxVal, "Target Value");
- head->setToolTip(idxName, "The control device's name (green: ok, red: missmatch between requested and active mode, orange: active/requested device not found in the displayed list)");
- head->setToolTip(idxTags, "The control device's tags");
- head->setToolTip(idxMode, "The control device's control mode");
+ head->setToolTip(
+ idxName,
+ "The control device's name (green: ok, red: missmatch between requested and active "
+ "mode, orange: active/requested device not found in the displayed list)");
+ head->setToolTip(idxTags, "The control device's tags");
+ head->setToolTip(idxMode, "The control device's control mode");
head->setToolTip(idxHWMode, "The control device's hardware control mode");
- head->setToolTip(idxAct, "Whether the control mode is active (green: ok, red: missmatch between requested and active mode)");
- head->setToolTip(idxReq, "Whether the control mode is requested (green: ok, red: missmatch between requested and active mode)");
- head->setToolTip(idxType, "The control mode's target type");
- head->setToolTip(idxVal, "The control mode's target value");
+ head->setToolTip(idxAct,
+ "Whether the control mode is active (green: ok, red: missmatch between "
+ "requested and active mode)");
+ head->setToolTip(idxReq,
+ "Whether the control mode is requested (green: ok, red: missmatch between "
+ "requested and active mode)");
+ head->setToolTip(idxType, "The control mode's target type");
+ head->setToolTip(idxVal, "The control mode's target value");
- ui->treeWidget->setColumnWidth(idxName, 150);
- ui->treeWidget->setColumnWidth(idxTags, 100);
- ui->treeWidget->setColumnWidth(idxMode, 225);
+ ui->treeWidget->setColumnWidth(idxName, 150);
+ ui->treeWidget->setColumnWidth(idxTags, 100);
+ ui->treeWidget->setColumnWidth(idxMode, 225);
ui->treeWidget->setColumnWidth(idxHWMode, 110);
- ui->treeWidget->setColumnWidth(idxAct, 40);
- ui->treeWidget->setColumnWidth(idxReq, 40);
- ui->treeWidget->setColumnWidth(idxType, 350);
+ ui->treeWidget->setColumnWidth(idxAct, 40);
+ ui->treeWidget->setColumnWidth(idxReq, 40);
+ ui->treeWidget->setColumnWidth(idxType, 350);
ui->treeWidget->header()->setResizeMode(idxAct, QHeaderView::Fixed);
ui->treeWidget->header()->setResizeMode(idxReq, QHeaderView::Fixed);
}
@@ -66,9 +73,10 @@ namespace armarx
delete ui;
}
- void ControlDevicesWidget::controlDeviceStatusChanged(const ControlDeviceStatusSeq& allStatus)
+ void
+ ControlDevicesWidget::controlDeviceStatusChanged(const ControlDeviceStatusSeq& allStatus)
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex, std::defer_lock};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex, std::defer_lock};
if (!guard.try_lock_for(std::chrono::microseconds(100)))
{
return;
@@ -86,14 +94,16 @@ namespace armarx
}
}
- void ControlDevicesWidget::clearAll()
+ void
+ ControlDevicesWidget::clearAll()
{
entries.clear();
statusUpdates.clear();
resetData.clear();
}
- void ControlDevicesWidget::doContentUpdate()
+ void
+ ControlDevicesWidget::doContentUpdate()
{
for (const auto& pair : statusUpdates)
{
@@ -102,16 +112,18 @@ namespace armarx
statusUpdates.clear();
}
- void ControlDevicesWidget::getResetData()
+ void
+ ControlDevicesWidget::getResetData()
{
auto temp = robotUnit->getControlDeviceDescriptions();
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
resetData = std::move(temp);
}
}
- bool ControlDevicesWidget::addOneFromResetData()
+ bool
+ ControlDevicesWidget::addOneFromResetData()
{
if (resetData.empty())
{
@@ -122,9 +134,10 @@ namespace armarx
return false;
}
- void ControlDevicesWidget::add(const ControlDeviceDescription& desc)
+ void
+ ControlDevicesWidget::add(const ControlDeviceDescription& desc)
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
if (entries.count(desc.deviceName))
{
return;
@@ -132,10 +145,11 @@ namespace armarx
entries[desc.deviceName] = new ControlDevicesWidgetEntry(*this, *(ui->treeWidget), desc);
}
- void ControlDevicesWidget::update(const ControlDeviceStatus& status)
+ void
+ ControlDevicesWidget::update(const ControlDeviceStatus& status)
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
- if (!robotUnit || ! robotUnit->isRunning())
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
+ if (!robotUnit || !robotUnit->isRunning())
{
return;
}
@@ -146,8 +160,10 @@ namespace armarx
entries.at(status.deviceName)->update(status);
}
- ControlDevicesWidgetEntry::ControlDevicesWidgetEntry(ControlDevicesWidget& parent, QTreeWidget& treeWidget, const ControlDeviceDescription& desc)
- : QObject {&parent}
+ ControlDevicesWidgetEntry::ControlDevicesWidgetEntry(ControlDevicesWidget& parent,
+ QTreeWidget& treeWidget,
+ const ControlDeviceDescription& desc) :
+ QObject{&parent}
{
header = new QTreeWidgetItem;
treeWidget.addTopLevelItem(header);
@@ -161,7 +177,7 @@ namespace armarx
connect(boxTags, SIGNAL(clicked(bool)), this, SLOT(hideTagList(bool)));
for (const auto& tag : desc.tags)
{
- QTreeWidgetItem* child = new QTreeWidgetItem {{QString::fromStdString(tag)}};
+ QTreeWidgetItem* child = new QTreeWidgetItem{{QString::fromStdString(tag)}};
treeWidget.addTopLevelItem(child);
tags.emplace_back(child);
}
@@ -171,17 +187,21 @@ namespace armarx
ControllerEntry& subEntry = subEntries[nameToType.first];
subEntry.child = new QTreeWidgetItem;
header->addChild(subEntry.child);
- subEntry.child->setText(ControlDevicesWidget::idxMode, QString::fromStdString(nameToType.first));
- subEntry.child->setText(ControlDevicesWidget::idxHWMode, QString::fromStdString(nameToType.second.hardwareControlMode));
+ subEntry.child->setText(ControlDevicesWidget::idxMode,
+ QString::fromStdString(nameToType.first));
+ subEntry.child->setText(ControlDevicesWidget::idxHWMode,
+ QString::fromStdString(nameToType.second.hardwareControlMode));
subEntry.child->setText(ControlDevicesWidget::idxAct, " ");
subEntry.child->setText(ControlDevicesWidget::idxReq, " ");
- subEntry.child->setText(ControlDevicesWidget::idxType, QString::fromStdString(nameToType.second.targetType));
+ subEntry.child->setText(ControlDevicesWidget::idxType,
+ QString::fromStdString(nameToType.second.targetType));
subEntry.value = new VariantWidget;
treeWidget.setItemWidget(subEntry.child, ControlDevicesWidget::idxVal, subEntry.value);
}
}
- void ControlDevicesWidgetEntry::update(const ControlDeviceStatus& status)
+ void
+ ControlDevicesWidgetEntry::update(const ControlDeviceStatus& status)
{
auto colorNew = (status.activeControlMode == status.requestedControlMode) ? green() : red();
bool newDevsNotFound = false;
@@ -190,7 +210,8 @@ namespace armarx
if (subEntries.count(activeMode))
{
subEntries.at(activeMode).child->setText(ControlDevicesWidget::idxAct, " ");
- subEntries.at(activeMode).child->setBackgroundColor(ControlDevicesWidget::idxAct, transparent());
+ subEntries.at(activeMode)
+ .child->setBackgroundColor(ControlDevicesWidget::idxAct, transparent());
}
activeMode = status.activeControlMode;
if (subEntries.count(activeMode))
@@ -204,7 +225,8 @@ namespace armarx
if (subEntries.count(requestedMode))
{
subEntries.at(requestedMode).child->setText(ControlDevicesWidget::idxReq, " ");
- subEntries.at(requestedMode).child->setBackgroundColor(ControlDevicesWidget::idxReq, transparent());
+ subEntries.at(requestedMode)
+ .child->setBackgroundColor(ControlDevicesWidget::idxReq, transparent());
}
requestedMode = status.requestedControlMode;
if (subEntries.count(requestedMode))
@@ -218,13 +240,16 @@ namespace armarx
//set the color here, since active may change without requested changen (or the other way) -> above there is no update
if (subEntries.count(requestedMode))
{
- subEntries.at(requestedMode).child->setBackgroundColor(ControlDevicesWidget::idxReq, colorNew);
+ subEntries.at(requestedMode)
+ .child->setBackgroundColor(ControlDevicesWidget::idxReq, colorNew);
}
if (subEntries.count(activeMode))
{
- subEntries.at(activeMode).child->setBackgroundColor(ControlDevicesWidget::idxAct, colorNew);
+ subEntries.at(activeMode)
+ .child->setBackgroundColor(ControlDevicesWidget::idxAct, colorNew);
}
- header->setBackgroundColor(ControlDevicesWidget::idxName, newDevsNotFound ? orange() : colorNew);
+ header->setBackgroundColor(ControlDevicesWidget::idxName,
+ newDevsNotFound ? orange() : colorNew);
}
for (const auto& pair : status.controlTargetValues)
{
@@ -236,12 +261,14 @@ namespace armarx
}
}
- bool ControlDevicesWidgetEntry::matchName(const QString& name)
+ bool
+ ControlDevicesWidgetEntry::matchName(const QString& name)
{
return header->text(ControlDevicesWidget::idxName).contains(name, Qt::CaseInsensitive);
}
- bool ControlDevicesWidgetEntry::matchTag(const QString& tag)
+ bool
+ ControlDevicesWidgetEntry::matchTag(const QString& tag)
{
for (const auto& tagit : tags)
{
@@ -253,12 +280,14 @@ namespace armarx
return false;
}
- std::set<QTreeWidgetItem*> ControlDevicesWidgetEntry::matchMode(const QString& mode)
+ std::set<QTreeWidgetItem*>
+ ControlDevicesWidgetEntry::matchMode(const QString& mode)
{
std::set<QTreeWidgetItem*> hits;
for (const auto& pair : subEntries)
{
- if (pair.second.child->text(ControlDevicesWidget::idxMode).contains(mode, Qt::CaseInsensitive))
+ if (pair.second.child->text(ControlDevicesWidget::idxMode)
+ .contains(mode, Qt::CaseInsensitive))
{
hits.emplace(pair.second.child);
}
@@ -266,12 +295,14 @@ namespace armarx
return hits;
}
- std::set<QTreeWidgetItem*> ControlDevicesWidgetEntry::isActiveState(const QString& state)
+ std::set<QTreeWidgetItem*>
+ ControlDevicesWidgetEntry::isActiveState(const QString& state)
{
std::set<QTreeWidgetItem*> hits;
for (const auto& pair : subEntries)
{
- if (pair.second.child->text(ControlDevicesWidget::idxAct).contains(state, Qt::CaseInsensitive))
+ if (pair.second.child->text(ControlDevicesWidget::idxAct)
+ .contains(state, Qt::CaseInsensitive))
{
hits.emplace(pair.second.child);
}
@@ -279,12 +310,14 @@ namespace armarx
return hits;
}
- std::set<QTreeWidgetItem*> ControlDevicesWidgetEntry::isRequestedState(const QString& state)
+ std::set<QTreeWidgetItem*>
+ ControlDevicesWidgetEntry::isRequestedState(const QString& state)
{
std::set<QTreeWidgetItem*> hits;
for (const auto& pair : subEntries)
{
- if (pair.second.child->text(ControlDevicesWidget::idxReq).contains(state, Qt::CaseInsensitive))
+ if (pair.second.child->text(ControlDevicesWidget::idxReq)
+ .contains(state, Qt::CaseInsensitive))
{
hits.emplace(pair.second.child);
}
@@ -292,12 +325,14 @@ namespace armarx
return hits;
}
- std::set<QTreeWidgetItem*> ControlDevicesWidgetEntry::matchTargetType(const QString& type)
+ std::set<QTreeWidgetItem*>
+ ControlDevicesWidgetEntry::matchTargetType(const QString& type)
{
std::set<QTreeWidgetItem*> hits;
for (const auto& pair : subEntries)
{
- if (pair.second.child->text(ControlDevicesWidget::idxType).contains(type, Qt::CaseInsensitive))
+ if (pair.second.child->text(ControlDevicesWidget::idxType)
+ .contains(type, Qt::CaseInsensitive))
{
hits.emplace(pair.second.child);
}
@@ -305,7 +340,8 @@ namespace armarx
return hits;
}
- void ControlDevicesWidgetEntry::setVisible(bool vis)
+ void
+ ControlDevicesWidgetEntry::setVisible(bool vis)
{
if (!vis)
{
@@ -314,7 +350,8 @@ namespace armarx
header->setHidden(!vis);
}
- void ControlDevicesWidgetEntry::setChildVis(bool vis, std::set<QTreeWidgetItem*> children)
+ void
+ ControlDevicesWidgetEntry::setChildVis(bool vis, std::set<QTreeWidgetItem*> children)
{
for (const auto& pair : subEntries)
{
@@ -329,11 +366,12 @@ namespace armarx
}
}
- void ControlDevicesWidgetEntry::hideTagList(bool hide)
+ void
+ ControlDevicesWidgetEntry::hideTagList(bool hide)
{
for (QTreeWidgetItem* tag : tags)
{
tag->setHidden(hide);
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.h
index c55c991a4728165e468a2edfc91203918b91b587..5a58c4668d64c98c5cee372da227a1b0c14d77e5 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.h
@@ -22,26 +22,27 @@
#pragma once
-#include <mutex>
#include <atomic>
+#include <mutex>
-#include <QWidget>
+#include <QCheckBox>
+#include <QComboBox>
+#include <QFormLayout>
+#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
-#include <QHBoxLayout>
-#include <QFormLayout>
#include <QPushButton>
#include <QTreeWidget>
-#include <QCheckBox>
-#include <QComboBox>
#include <QTreeWidgetItem>
+#include <QWidget>
-#include <ArmarXGui/libraries/WidgetDescription/WidgetDescription.h>
#include <ArmarXGui/libraries/VariantWidget/VariantWidget.h>
+#include <ArmarXGui/libraries/WidgetDescription/WidgetDescription.h>
+
+#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_ControlDevicesWidget.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "RobotUnitWidgetBase.h"
-#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_ControlDevicesWidget.h>
namespace armarx
{
@@ -59,6 +60,7 @@ namespace armarx
void doContentUpdate() override;
void getResetData() override;
bool addOneFromResetData() override;
+
private:
void add(const ControlDeviceDescription& desc);
void update(const ControlDeviceStatus& status);
@@ -83,11 +85,9 @@ namespace armarx
Q_OBJECT
public:
- ControlDevicesWidgetEntry(
- ControlDevicesWidget& parent,
- QTreeWidget& treeWidget,
- const ControlDeviceDescription& desc
- );
+ ControlDevicesWidgetEntry(ControlDevicesWidget& parent,
+ QTreeWidget& treeWidget,
+ const ControlDeviceDescription& desc);
void update(const ControlDeviceStatus& status);
@@ -112,6 +112,7 @@ namespace armarx
QTreeWidgetItem* child;
VariantWidget* value;
};
+
std::map<std::string, ControllerEntry> subEntries;
std::string activeMode;
std::string requestedMode;
@@ -120,4 +121,4 @@ namespace armarx
std::vector<QTreeWidgetItem*> tags;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
index 274d42e7487db9cb0086f30cb509e18bc7a98ef6..4496bb82b6436bc930963d8307f8d43d15459689 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
@@ -20,25 +20,32 @@
* GNU General Public License
*/
-#include <filesystem>
-
#include "NJointControllerClassesWidget.h"
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/util/StringHelpers.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <QGridLayout>
+#include <filesystem>
+
#include <QDir>
+#include <QGridLayout>
#include <QSortFilterProxyModel>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
+
namespace armarx
{
NJointControllerClassesWidget::NJointControllerClassesWidget(QWidget* parent) :
RobotUnitWidgetTemplateBase("NJointControllerClassesWidget", parent)
{
connect(ui->pushButtonLoadLib, SIGNAL(released()), this, SLOT(loadLibClicked()));
- connect(ui->comboBoxPackage, SIGNAL(currentIndexChanged(QString)), this, SLOT(packageEditChanged()));
- connect(ui->comboBoxPackage, SIGNAL(editTextChanged(QString)), this, SLOT(packageEditChanged()));
+ connect(ui->comboBoxPackage,
+ SIGNAL(currentIndexChanged(QString)),
+ this,
+ SLOT(packageEditChanged()));
+ connect(ui->comboBoxPackage,
+ SIGNAL(editTextChanged(QString)),
+ this,
+ SLOT(packageEditChanged()));
ui->comboBoxPackage->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
ui->comboBoxPackage->setFixedWidth(300);
@@ -46,13 +53,13 @@ namespace armarx
{
using namespace std::filesystem;
std::string homeDir = QDir::homePath().toStdString();
- path p = path {homeDir} / ".cmake" / "packages";
+ path p = path{homeDir} / ".cmake" / "packages";
if (is_directory(p))
{
for (const path& entry : directory_iterator(p))
{
const std::string pkg = entry.filename().string();
- if (CMakePackageFinder {pkg, "", true} .packageFound())
+ if (CMakePackageFinder{pkg, "", true}.packageFound())
{
ui->comboBoxPackage->addItem(QString::fromStdString(pkg));
}
@@ -79,7 +86,6 @@ namespace armarx
head->setToolTip(0, "Controller class name");
ui->treeWidget->header()->setResizeMode(0, QHeaderView::Fixed);
ui->treeWidget->header()->setResizeMode(1, QHeaderView::Fixed);
-
}
NJointControllerClassesWidget::~NJointControllerClassesWidget()
@@ -87,10 +93,11 @@ namespace armarx
delete ui;
}
- void NJointControllerClassesWidget::nJointControllerClassAdded(std::string name)
+ void
+ NJointControllerClassesWidget::nJointControllerClassAdded(std::string name)
{
RobotUnitInterfacePrx ru;
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
if (!robotUnit)
{
return;
@@ -106,7 +113,9 @@ namespace armarx
}
}
- void NJointControllerClassesWidget::updateDefaultNameOnControllerCreated(QString createdName, bool force)
+ void
+ NJointControllerClassesWidget::updateDefaultNameOnControllerCreated(QString createdName,
+ bool force)
{
const auto oldName = getDefaultName();
if (oldName == createdName || force)
@@ -120,32 +129,39 @@ namespace armarx
}
}
- QString NJointControllerClassesWidget::getDefaultName() const
+ QString
+ NJointControllerClassesWidget::getDefaultName() const
{
return QString::number(defaultControllerName);
}
- void NJointControllerClassesWidget::loadSettings(QSettings* settings)
+ void
+ NJointControllerClassesWidget::loadSettings(QSettings* settings)
{
ui->lineEditLibrary->setText(settings->value("classLoadLineEditLib", "").toString());
- ui->comboBoxPackage->lineEdit()->setText(settings->value("classLoadComboBoxPkg", "").toString());
- ui->comboBoxLibrary->lineEdit()->setText(settings->value("classLoadComboBoxLib", "").toString());
+ ui->comboBoxPackage->lineEdit()->setText(
+ settings->value("classLoadComboBoxPkg", "").toString());
+ ui->comboBoxLibrary->lineEdit()->setText(
+ settings->value("classLoadComboBoxLib", "").toString());
}
- void NJointControllerClassesWidget::saveSettings(QSettings* settings)
+ void
+ NJointControllerClassesWidget::saveSettings(QSettings* settings)
{
settings->setValue("classLoadLineEditLib", ui->lineEditLibrary->text());
settings->setValue("classLoadComboBoxPkg", ui->comboBoxPackage->currentText());
settings->setValue("classLoadComboBoxLib", ui->comboBoxLibrary->currentText());
}
- void NJointControllerClassesWidget::clearAll()
+ void
+ NJointControllerClassesWidget::clearAll()
{
entries.clear();
nJointControllerClassDescriptions.clear();
}
- void NJointControllerClassesWidget::doContentUpdate()
+ void
+ NJointControllerClassesWidget::doContentUpdate()
{
for (const auto& pair : nJointControllerClassDescriptions)
{
@@ -154,11 +170,12 @@ namespace armarx
nJointControllerClassDescriptions.clear();
}
- void NJointControllerClassesWidget::getResetData()
+ void
+ NJointControllerClassesWidget::getResetData()
{
auto temp = robotUnit->getNJointControllerClassDescriptions();
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
for (NJointControllerClassDescription& ds : temp)
{
nJointControllerClassDescriptions[ds.className] = std::move(ds);
@@ -166,7 +183,8 @@ namespace armarx
}
}
- bool NJointControllerClassesWidget::addOneFromResetData()
+ bool
+ NJointControllerClassesWidget::addOneFromResetData()
{
if (nJointControllerClassDescriptions.empty())
{
@@ -177,19 +195,20 @@ namespace armarx
return false;
}
- void NJointControllerClassesWidget::filterUpdated()
+ void
+ NJointControllerClassesWidget::filterUpdated()
{
for (auto& entry : entries)
{
NJointControllerClassesWidgetEntry* item = entry.second;
- if (! filterNameActive->isChecked() && !filterRemoteCreationActive->isChecked())
+ if (!filterNameActive->isChecked() && !filterRemoteCreationActive->isChecked())
{
item->setVisible(true);
return;
}
const bool combineOr = (filterCombination->currentText() == "Or");
bool showName = !combineOr; //init to neutral element
- bool showRemote = !combineOr;//init to neutral element
+ bool showRemote = !combineOr; //init to neutral element
if (filterNameActive->isChecked())
{
@@ -217,17 +236,20 @@ namespace armarx
}
}
- void NJointControllerClassesWidget::add(const NJointControllerClassDescription& desc)
+ void
+ NJointControllerClassesWidget::add(const NJointControllerClassDescription& desc)
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
if (entries.count(desc.className))
{
return;
}
- entries[desc.className] = new NJointControllerClassesWidgetEntry(*this, *(ui->treeWidget), desc, robotUnit);
+ entries[desc.className] =
+ new NJointControllerClassesWidgetEntry(*this, *(ui->treeWidget), desc, robotUnit);
}
- void NJointControllerClassesWidget::addFilter()
+ void
+ NJointControllerClassesWidget::addFilter()
{
QTreeWidgetItem* filterHeader = new QTreeWidgetItem;
ui->treeWidget->addTopLevelItem(filterHeader);
@@ -240,8 +262,8 @@ namespace armarx
QGridLayout* l = new QGridLayout;
w->setLayout(l);
l->setContentsMargins(0, 0, 0, 0);
- l->addItem(new QSpacerItem {0, 0, QSizePolicy::MinimumExpanding}, 0, 3);
- l->addWidget(new QLabel {"Combine filters with"}, 0, 0, 1, 1);
+ l->addItem(new QSpacerItem{0, 0, QSizePolicy::MinimumExpanding}, 0, 3);
+ l->addWidget(new QLabel{"Combine filters with"}, 0, 0, 1, 1);
filterCombination = new QComboBox;
filterCombination->addItem("Or");
filterCombination->addItem("And");
@@ -258,7 +280,8 @@ namespace armarx
filterNameInverted->setText("Invert filter");
l->addWidget(filterNameInverted, 1, 2, 1, 1);
connect(filterNameActive, SIGNAL(toggled(bool)), filterName, SLOT(setEnabled(bool)));
- connect(filterNameActive, SIGNAL(toggled(bool)), filterNameInverted, SLOT(setEnabled(bool)));
+ connect(
+ filterNameActive, SIGNAL(toggled(bool)), filterNameInverted, SLOT(setEnabled(bool)));
filterName->setEnabled(false);
filterNameInverted->setEnabled(false);
@@ -270,15 +293,20 @@ namespace armarx
filterRemoteCreation->addItem("With");
filterRemoteCreation->addItem("Without");
l->addWidget(filterRemoteCreation, 2, 1, 1, 1);
- connect(filterRemoteCreationActive, SIGNAL(toggled(bool)), filterRemoteCreation, SLOT(setEnabled(bool)));
+ connect(filterRemoteCreationActive,
+ SIGNAL(toggled(bool)),
+ filterRemoteCreation,
+ SLOT(setEnabled(bool)));
filterRemoteCreationActive->setEnabled(false);
- connect(filterCombination, SIGNAL(currentIndexChanged(QString)), this, SLOT(filterUpdated()));
+ connect(
+ filterCombination, SIGNAL(currentIndexChanged(QString)), this, SLOT(filterUpdated()));
connect(filterNameActive, SIGNAL(clicked(bool)), this, SLOT(filterUpdated()));
connect(filterRemoteCreationActive, SIGNAL(clicked(bool)), this, SLOT(filterUpdated()));
connect(filterNameInverted, SIGNAL(clicked(bool)), this, SLOT(filterUpdated()));
connect(filterName, SIGNAL(textChanged(QString)), this, SLOT(filterUpdated()));
- connect(filterRemoteCreation, SIGNAL(currentIndexChanged(int)), this, SLOT(filterUpdated()));
+ connect(
+ filterRemoteCreation, SIGNAL(currentIndexChanged(int)), this, SLOT(filterUpdated()));
filterNameActive->setChecked(false);
filterRemoteCreationActive->setChecked(false);
@@ -290,21 +318,20 @@ namespace armarx
NJointControllerClassesWidget& parent,
QTreeWidget& treeWidget,
const NJointControllerClassDescription& desc,
- RobotUnitInterfacePrx robotUnit
- ) :
- QObject {&parent},
- className {desc.className},
- classNameQStr {QString::fromStdString(className)},
- robotUnit {robotUnit},
- parent {&parent}
+ RobotUnitInterfacePrx robotUnit) :
+ QObject{&parent},
+ className{desc.className},
+ classNameQStr{QString::fromStdString(className)},
+ robotUnit{robotUnit},
+ parent{&parent}
{
- header = new QTreeWidgetItem {{QString::fromStdString(desc.className)}};
+ header = new QTreeWidgetItem{{QString::fromStdString(desc.className)}};
treeWidget.addTopLevelItem(header);
treeWidget.resizeColumnToContents(0);
QWidget* headerW = new QWidget;
headerW->setLayout(new QHBoxLayout);
headerW->layout()->setContentsMargins(0, 0, 0, 0);
- headerW->layout()->addItem(new QSpacerItem {0, 0, QSizePolicy::MinimumExpanding});
+ headerW->layout()->addItem(new QSpacerItem{0, 0, QSizePolicy::MinimumExpanding});
if (desc.configDescription)
{
//add textfiel + button
@@ -312,13 +339,19 @@ namespace armarx
nameEdit = new QLineEdit;
nameEdit->setText(parent.getDefaultName());
nameEdit->setToolTip("The instance name for the created controller instance");
- headerW->layout()->addWidget(new QLabel {"Controller name"});
+ headerW->layout()->addWidget(new QLabel{"Controller name"});
headerW->layout()->addWidget(nameEdit);
- QPushButton* button = new QPushButton {"Create"};
+ QPushButton* button = new QPushButton{"Create"};
headerW->layout()->addWidget(button);
button->setToolTip("Create a new controller instance of this class");
- connect(button, &QPushButton::clicked, this, &NJointControllerClassesWidgetEntry::createCtrl);
- connect(nameEdit, &QLineEdit::returnPressed, this, &NJointControllerClassesWidgetEntry::createCtrl);
+ connect(button,
+ &QPushButton::clicked,
+ this,
+ &NJointControllerClassesWidgetEntry::createCtrl);
+ connect(nameEdit,
+ &QLineEdit::returnPressed,
+ this,
+ &NJointControllerClassesWidgetEntry::createCtrl);
}
//descr
{
@@ -332,7 +365,7 @@ namespace armarx
creator = WidgetDescription::makeDescribedWidget(desc.configDescription);
creator->layout()->setContentsMargins(0, 0, 0, 0);
compressLay->addWidget(creator);
- compressLay->addItem(new QSpacerItem {0, 0, QSizePolicy::MinimumExpanding});
+ compressLay->addItem(new QSpacerItem{0, 0, QSizePolicy::MinimumExpanding});
treeWidget.setItemWidget(child, 0, compressWid);
}
}
@@ -344,22 +377,26 @@ namespace armarx
dummy->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Minimum);
headerW->layout()->addWidget(dummy);
- headerW->layout()->addWidget(new QLabel {"No remote creation allowed."});
+ headerW->layout()->addWidget(new QLabel{"No remote creation allowed."});
}
treeWidget.setItemWidget(header, 1, headerW);
}
- bool NJointControllerClassesWidgetEntry::matchName(const QString& name)
+ bool
+ NJointControllerClassesWidgetEntry::matchName(const QString& name)
{
return classNameQStr.contains(name, Qt::CaseInsensitive);
}
- bool NJointControllerClassesWidgetEntry::hasRemoteCreation()
+ bool
+ NJointControllerClassesWidgetEntry::hasRemoteCreation()
{
return creator;
}
- void NJointControllerClassesWidgetEntry::updateDefaultName(const QString& oldName, const QString& newName)
+ void
+ NJointControllerClassesWidgetEntry::updateDefaultName(const QString& oldName,
+ const QString& newName)
{
if (nameEdit && nameEdit->text() == oldName)
{
@@ -367,23 +404,26 @@ namespace armarx
}
}
- void NJointControllerClassesWidgetEntry::setVisible(bool vis)
+ void
+ NJointControllerClassesWidgetEntry::setVisible(bool vis)
{
header->setHidden(!vis);
}
- void NJointControllerClassesWidgetEntry::createCtrl()
+ void
+ NJointControllerClassesWidgetEntry::createCtrl()
{
const auto instanceName = nameEdit->text().toStdString();
const auto variants = creator->getVariants();
if (variants.empty())
{
- ARMARX_INFO << "creating " << instanceName << " of class " << className << " with no parameters\n";
+ ARMARX_INFO << "creating " << instanceName << " of class " << className
+ << " with no parameters\n";
}
else
{
std::stringstream ss;
- ss << "creating " << instanceName << " of class " << className << " with parameters:\n";
+ ss << "creating " << instanceName << " of class " << className << " with parameters:\n";
for (const auto& pair : variants)
{
if (pair.second)
@@ -391,7 +431,8 @@ namespace armarx
if (pair.second->data)
{
- ss << " '" << pair.first << "' of type " << pair.second->data->ice_id() << "\n";
+ ss << " '" << pair.first << "' of type " << pair.second->data->ice_id()
+ << "\n";
}
else
{
@@ -409,7 +450,8 @@ namespace armarx
parent->updateDefaultNameOnControllerCreated(nameEdit->text());
}
- void NJointControllerClassesWidget::packageEditChanged()
+ void
+ NJointControllerClassesWidget::packageEditChanged()
{
auto package = ui->comboBoxPackage->currentText();
if (package.isEmpty())
@@ -449,9 +491,10 @@ namespace armarx
std::string shortName = lib.substr(libSubstrStart, libSubstrEnd);
libShortNameToFileName[shortName] = lib;
ui->comboBoxLibrary->addItem(QString::fromStdString(shortName));
- if (libidx == -1 && (lib.find("Controller") != lib.npos || lib.find("controller") != lib.npos))
+ if (libidx == -1 &&
+ (lib.find("Controller") != lib.npos || lib.find("controller") != lib.npos))
{
- libidx = libShortNameToFileName.size() - 1 ;
+ libidx = libShortNameToFileName.size() - 1;
}
ui->comboBoxLibrary->setCurrentIndex(libidx);
}
@@ -459,56 +502,59 @@ namespace armarx
else
{
ui->pushButtonLoadLib->setEnabled(false);
- ui->labelPackageFound->setPixmap(QPixmap(":/icons/dialog-cancel-5.svg").scaled(16, 16));
+ ui->labelPackageFound->setPixmap(
+ QPixmap(":/icons/dialog-cancel-5.svg").scaled(16, 16));
ui->labelPackageFound->setToolTip("Cannot find Package");
}
}
}
- void NJointControllerClassesWidget::loadLibClicked()
+ void
+ NJointControllerClassesWidget::loadLibClicked()
{
- ARMARX_WARNING << "The functionality of dynamically loading libraries during runtime is no longer supported. Use"
+ ARMARX_WARNING << "The functionality of dynamically loading libraries during runtime is no "
+ "longer supported. Use"
"the component properties to load additional libraries.";
return;
-// if (!robotUnit)
-// {
-// return;
-// }
-
-// switch (selectLibMode)
-// {
-// case SelectLibsMode::LineEdit:
-// {
-// auto lib = ui->lineEditLibrary->text().toStdString();
-// if (lib.empty())
-// {
-// return;
-// }
-//// ARMARX_INFO << "requesting to load lib " << lib
-//// << " -> " << robotUnit->loadLibFromPath(lib);
-// }
-// break;
-//
-// case SelectLibsMode::ComboBox:
-// {
-// auto package = ui->comboBoxPackage->currentText().toStdString();
-// auto lib = ui->comboBoxLibrary->currentText().toStdString();
-// if (lib.empty())
-// {
-// return;
-// }
-// if (libShortNameToFileName.count(lib))
-// {
-// lib = libShortNameToFileName.at(lib);
-// }
-//// ARMARX_INFO << "requesting to load lib " << lib << " from package " << package
-//// << " -> " << robotUnit->loadLibFromPackage(package, lib);
-// }
-// break;
-// default:
-// ARMARX_WARNING << "Load lib for given SelectLibsMode nyi";
-// break;
-// }
+ // if (!robotUnit)
+ // {
+ // return;
+ // }
+
+ // switch (selectLibMode)
+ // {
+ // case SelectLibsMode::LineEdit:
+ // {
+ // auto lib = ui->lineEditLibrary->text().toStdString();
+ // if (lib.empty())
+ // {
+ // return;
+ // }
+ //// ARMARX_INFO << "requesting to load lib " << lib
+ //// << " -> " << robotUnit->loadLibFromPath(lib);
+ // }
+ // break;
+ //
+ // case SelectLibsMode::ComboBox:
+ // {
+ // auto package = ui->comboBoxPackage->currentText().toStdString();
+ // auto lib = ui->comboBoxLibrary->currentText().toStdString();
+ // if (lib.empty())
+ // {
+ // return;
+ // }
+ // if (libShortNameToFileName.count(lib))
+ // {
+ // lib = libShortNameToFileName.at(lib);
+ // }
+ //// ARMARX_INFO << "requesting to load lib " << lib << " from package " << package
+ //// << " -> " << robotUnit->loadLibFromPackage(package, lib);
+ // }
+ // break;
+ // default:
+ // ARMARX_WARNING << "Load lib for given SelectLibsMode nyi";
+ // break;
+ // }
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.h
index 47057c1f8381aed2c7cdb71a707401f5d36d9cb7..2590b2f2db60526c4ae437a31ee7038cf991d82e 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.h
@@ -22,33 +22,35 @@
#pragma once
-#include <mutex>
#include <atomic>
+#include <mutex>
-#include <QWidget>
+#include <QCheckBox>
+#include <QComboBox>
+#include <QFormLayout>
+#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
-#include <QHBoxLayout>
-#include <QFormLayout>
#include <QPushButton>
#include <QTreeWidget>
#include <QTreeWidgetItem>
-#include <QCheckBox>
-#include <QComboBox>
+#include <QWidget>
-#include <ArmarXGui/libraries/WidgetDescription/WidgetDescription.h>
#include <ArmarXGui/libraries/VariantWidget/VariantWidget.h>
+#include <ArmarXGui/libraries/WidgetDescription/WidgetDescription.h>
+
+#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_NJointControllerClassesWidget.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "RobotUnitWidgetBase.h"
-#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_NJointControllerClassesWidget.h>
namespace armarx
{
class NJointControllerClassesWidgetEntry;
- class NJointControllerClassesWidget : public RobotUnitWidgetTemplateBase<Ui::NJointControllerClassesWidget>
+ class NJointControllerClassesWidget :
+ public RobotUnitWidgetTemplateBase<Ui::NJointControllerClassesWidget>
{
Q_OBJECT
@@ -63,6 +65,7 @@ namespace armarx
void loadSettings(QSettings* settings) override;
void saveSettings(QSettings* settings) override;
+
protected:
void clearAll() override;
void doContentUpdate() override;
@@ -73,6 +76,7 @@ namespace armarx
void filterUpdated();
void packageEditChanged();
void loadLibClicked();
+
private:
void add(const NJointControllerClassDescription& desc);
void addFilter() override;
@@ -98,7 +102,7 @@ namespace armarx
SelectLibsMode selectLibMode;
std::map<std::string, std::string> libShortNameToFileName;
- int defaultControllerName {0};
+ int defaultControllerName{0};
};
//helper
@@ -106,12 +110,10 @@ namespace armarx
{
Q_OBJECT
public:
- NJointControllerClassesWidgetEntry(
- NJointControllerClassesWidget& parent,
- QTreeWidget& treeWidget,
- const NJointControllerClassDescription& desc,
- RobotUnitInterfacePrx robotUnit
- );
+ NJointControllerClassesWidgetEntry(NJointControllerClassesWidget& parent,
+ QTreeWidget& treeWidget,
+ const NJointControllerClassDescription& desc,
+ RobotUnitInterfacePrx robotUnit);
bool matchName(const QString& name);
bool hasRemoteCreation();
@@ -128,9 +130,9 @@ namespace armarx
std::string className;
QString classNameQStr;
RobotUnitInterfacePrx robotUnit;
- QTreeWidgetItem* header {nullptr};
- QLineEdit* nameEdit {nullptr};
- WidgetDescription::DescribedWidgetBase* creator {nullptr};
+ QTreeWidgetItem* header{nullptr};
+ QLineEdit* nameEdit{nullptr};
+ WidgetDescription::DescribedWidgetBase* creator{nullptr};
NJointControllerClassesWidget* parent;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp
index b5160a688306a3fec1eadada0567cc14e66e552d..f1ff856b44388c96b9da32666380258145930675 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp
@@ -20,10 +20,11 @@
* GNU General Public License
*/
#include "NJointControllersWidget.h"
-#include "StyleSheets.h"
#include <ArmarXCore/core/logging/Logging.h>
+#include "StyleSheets.h"
+
namespace armarx
{
NJointControllersWidget::NJointControllersWidget(QWidget* parent) :
@@ -44,14 +45,24 @@ namespace armarx
head->setText(idxDeactivate, "");
head->setText(idxDelete, "");
- head->setToolTip(idxName, "The controllers instance name (green: ok, red: active and requested state missmatch or there is an error)");
+ head->setToolTip(idxName,
+ "The controllers instance name (green: ok, red: active and requested "
+ "state missmatch or there is an error)");
head->setToolTip(idxClass, "The controllers class name");
- head->setToolTip(idxActive, "Whether the controller is active (green: ok, red: active and requested state missmatch)");
- head->setToolTip(idxRequested, "Whether the controller is requested (green: ok, red: active and requested state missmatch)");
- head->setToolTip(idxError, "Whether the controller is deactivated due to an error (green: ok, red: there is an error)");
- head->setToolTip(idxInternal, "Whether the controller is internal (e.g. used by the kinematic unit)");
+ head->setToolTip(idxActive,
+ "Whether the controller is active (green: ok, red: active and requested "
+ "state missmatch)");
+ head->setToolTip(idxRequested,
+ "Whether the controller is requested (green: ok, red: active and "
+ "requested state missmatch)");
+ head->setToolTip(idxError,
+ "Whether the controller is deactivated due to an error (green: ok, red: "
+ "there is an error)");
+ head->setToolTip(idxInternal,
+ "Whether the controller is internal (e.g. used by the kinematic unit)");
head->setToolTip(idxCtrlDev, "The control devices used by this controller");
- head->setToolTip(idxCtrlMode, "The controll mode for the control devices used by this controller");
+ head->setToolTip(idxCtrlMode,
+ "The controll mode for the control devices used by this controller");
head->setToolTip(idxActivate, "Button to activate the Controller");
head->setToolTip(idxDeactivate, "Button to deactivate the Controller");
head->setToolTip(idxDelete, "Button to delete the Controller");
@@ -76,10 +87,14 @@ namespace armarx
ui->treeWidget->header()->setResizeMode(idxError, QHeaderView::Fixed);
ui->treeWidget->header()->setResizeMode(idxInternal, QHeaderView::Fixed);
- ui->pushButtonStopAll->setIcon(QIcon {QString{":/icons/media-playback-pause.ico"}});
- ui->pushButtonRemoveAll->setIcon(QIcon {QString{":/icons/Trash.svg"}});
- connect(ui->pushButtonStopAll, SIGNAL(clicked()), this, SLOT(onPushButtonStopAll_clicked()));
- connect(ui->pushButtonRemoveAll, SIGNAL(clicked()), this, SLOT(onPushButtonRemoveAll_clicked()));
+ ui->pushButtonStopAll->setIcon(QIcon{QString{":/icons/media-playback-pause.ico"}});
+ ui->pushButtonRemoveAll->setIcon(QIcon{QString{":/icons/Trash.svg"}});
+ connect(
+ ui->pushButtonStopAll, SIGNAL(clicked()), this, SLOT(onPushButtonStopAll_clicked()));
+ connect(ui->pushButtonRemoveAll,
+ SIGNAL(clicked()),
+ this,
+ SLOT(onPushButtonRemoveAll_clicked()));
}
NJointControllersWidget::~NJointControllersWidget()
@@ -87,9 +102,11 @@ namespace armarx
delete ui;
}
- void NJointControllersWidget::nJointControllerStatusChanged(const NJointControllerStatusSeq& allStatus)
+ void
+ NJointControllersWidget::nJointControllerStatusChanged(
+ const NJointControllerStatusSeq& allStatus)
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex, std::defer_lock};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex, std::defer_lock};
if (!guard.try_lock_for(std::chrono::microseconds(100)))
{
return;
@@ -107,16 +124,19 @@ namespace armarx
}
}
- void NJointControllersWidget::add(const NJointControllerDescriptionWithStatus& ds)
+ void
+ NJointControllersWidget::add(const NJointControllerDescriptionWithStatus& ds)
{
if (!entries.count(ds.description.instanceName))
{
- entries[ds.description.instanceName] = new NJointControllersWidgetEntry(*this, *(ui->treeWidget), ds.description);
+ entries[ds.description.instanceName] =
+ new NJointControllersWidgetEntry(*this, *(ui->treeWidget), ds.description);
}
entries.at(ds.description.instanceName)->update(ds.status);
}
- void NJointControllersWidget::onPushButtonStopAll_clicked()
+ void
+ NJointControllersWidget::onPushButtonStopAll_clicked()
{
if (robotUnit)
{
@@ -124,7 +144,8 @@ namespace armarx
}
}
- void NJointControllersWidget::onPushButtonRemoveAll_clicked()
+ void
+ NJointControllersWidget::onPushButtonRemoveAll_clicked()
{
for (auto& pair : entries)
{
@@ -136,14 +157,16 @@ namespace armarx
}
}
catch (...)
- {}
+ {
+ }
}
}
- void NJointControllersWidget::nJointControllerCreated(std::string name)
+ void
+ NJointControllersWidget::nJointControllerCreated(std::string name)
{
RobotUnitInterfacePrx ru;
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
if (isResetting || !robotUnit || entries.count(name))
{
return;
@@ -162,9 +185,10 @@ namespace armarx
}
}
- void NJointControllersWidget::nJointControllerDeleted(std::string name)
+ void
+ NJointControllersWidget::nJointControllerDeleted(std::string name)
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
controllersDeleted.emplace(name);
if (doMetaCall)
{
@@ -172,7 +196,8 @@ namespace armarx
}
}
- void NJointControllersWidget::loadSettings(QSettings* settings)
+ void
+ NJointControllersWidget::loadSettings(QSettings* settings)
{
ui->treeWidget->setColumnWidth(idxName, settings->value("ctrlColWName", 400).toInt());
ui->treeWidget->setColumnWidth(idxClass, settings->value("ctrlColWClass", 200).toInt());
@@ -180,7 +205,8 @@ namespace armarx
ui->treeWidget->setColumnWidth(idxCtrlMode, settings->value("ctrlColWCMod", 150).toInt());
}
- void NJointControllersWidget::saveSettings(QSettings* settings)
+ void
+ NJointControllersWidget::saveSettings(QSettings* settings)
{
settings->setValue("ctrlColWName", ui->treeWidget->columnWidth(idxName));
settings->setValue("ctrlColWClass", ui->treeWidget->columnWidth(idxClass));
@@ -188,7 +214,8 @@ namespace armarx
settings->setValue("ctrlColWCMod", ui->treeWidget->columnWidth(idxCtrlMode));
}
- void NJointControllersWidget::clearAll()
+ void
+ NJointControllersWidget::clearAll()
{
entries.clear();
controllersCreated.clear();
@@ -196,7 +223,8 @@ namespace armarx
controllersDeleted.clear();
}
- void NJointControllersWidget::doContentUpdate()
+ void
+ NJointControllersWidget::doContentUpdate()
{
for (const auto& pair : controllersCreated)
{
@@ -238,11 +266,12 @@ namespace armarx
controllersDeleted.clear();
}
- void NJointControllersWidget::getResetData()
+ void
+ NJointControllersWidget::getResetData()
{
auto temp = robotUnit->getNJointControllerDescriptionsWithStatuses();
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
for (NJointControllerDescriptionWithStatus& ds : temp)
{
controllersCreated[ds.description.instanceName] = std::move(ds);
@@ -250,7 +279,8 @@ namespace armarx
}
}
- bool NJointControllersWidget::addOneFromResetData()
+ bool
+ NJointControllersWidget::addOneFromResetData()
{
if (controllersCreated.empty())
{
@@ -261,16 +291,17 @@ namespace armarx
return false;
}
- NJointControllersWidgetEntry::NJointControllersWidgetEntry(NJointControllersWidget& parent, QTreeWidget& treeWidget, const NJointControllerDescription& desc)
- :
- QObject(&parent),
- deletable {desc.deletable},
- controller {desc.controller}
+ NJointControllersWidgetEntry::NJointControllersWidgetEntry(
+ NJointControllersWidget& parent,
+ QTreeWidget& treeWidget,
+ const NJointControllerDescription& desc) :
+ QObject(&parent), deletable{desc.deletable}, controller{desc.controller}
{
ARMARX_CHECK_EXPRESSION(controller);
header = new QTreeWidgetItem;
treeWidget.addTopLevelItem(header);
- header->setText(NJointControllersWidget::idxName, QString::fromStdString(desc.instanceName));
+ header->setText(NJointControllersWidget::idxName,
+ QString::fromStdString(desc.instanceName));
header->setText(NJointControllersWidget::idxClass, QString::fromStdString(desc.className));
header->setText(NJointControllersWidget::idxActive, "?");
header->setText(NJointControllersWidget::idxRequested, "?");
@@ -278,12 +309,13 @@ namespace armarx
header->setText(NJointControllersWidget::idxInternal, desc.internal ? "X" : " ");
const std::size_t numDev = desc.controlModeAssignment.size();
- const std::size_t numMod = getMapValues<StringStringDictionary, std::set>(desc.controlModeAssignment).size();
+ const std::size_t numMod =
+ getMapValues<StringStringDictionary, std::set>(desc.controlModeAssignment).size();
//device list
{
//buttons
- QWidget* widgDev = new QWidget{&parent};
- QWidget* widgMod = new QWidget{&parent};
+ QWidget* widgDev = new QWidget{&parent};
+ QWidget* widgMod = new QWidget{&parent};
treeWidget.setItemWidget(header, NJointControllersWidget::idxCtrlDev, widgDev);
treeWidget.setItemWidget(header, NJointControllersWidget::idxCtrlMode, widgMod);
QVBoxLayout* layDev = new QVBoxLayout;
@@ -292,8 +324,10 @@ namespace armarx
layMod->setContentsMargins(0, 0, 0, 0);
widgDev->setLayout(layDev);
widgMod->setLayout(layMod);
- boxDev = new QCheckBox{QString::number(numDev) + " Device" + (numDev > 1 ? QString{"s"}: QString{""})};
- boxMod = new QCheckBox{QString::number(numMod) + " Mode" + (numMod > 1 ? QString{"s"}: QString{""})};
+ boxDev = new QCheckBox{QString::number(numDev) + " Device" +
+ (numDev > 1 ? QString{"s"} : QString{""})};
+ boxMod = new QCheckBox{QString::number(numMod) + " Mode" +
+ (numMod > 1 ? QString{"s"} : QString{""})};
layDev->addWidget(boxDev);
layMod->addWidget(boxMod);
boxDev->setStyleSheet(checkboxStyleSheet());
@@ -308,8 +342,10 @@ namespace armarx
{
QTreeWidgetItem* child = new QTreeWidgetItem;
header->addChild(child);
- child->setText(NJointControllersWidget::idxCtrlDev, QString::fromStdString(pair.first));
- child->setText(NJointControllersWidget::idxCtrlMode, QString::fromStdString(pair.second));
+ child->setText(NJointControllersWidget::idxCtrlDev,
+ QString::fromStdString(pair.first));
+ child->setText(NJointControllersWidget::idxCtrlMode,
+ QString::fromStdString(pair.second));
devsToModes.emplace_back(child);
child->setHidden(true);
}
@@ -320,8 +356,8 @@ namespace armarx
dec->setToolTip("Deactivate the Controller");
treeWidget.setItemWidget(header, NJointControllersWidget::idxActivate, act);
treeWidget.setItemWidget(header, NJointControllersWidget::idxDeactivate, dec);
- act->setIcon(QIcon {QString{":/icons/media-playback-start.ico"}});
- dec->setIcon(QIcon {QString{":/icons/media-playback-pause.ico"}});
+ act->setIcon(QIcon{QString{":/icons/media-playback-start.ico"}});
+ dec->setIcon(QIcon{QString{":/icons/media-playback-pause.ico"}});
act->setFixedWidth(40);
dec->setFixedWidth(40);
act->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Minimum);
@@ -333,7 +369,7 @@ namespace armarx
QPushButton* del = new QPushButton;
treeWidget.setItemWidget(header, NJointControllersWidget::idxDelete, del);
del->setToolTip("Delete the Controller");
- del->setIcon(QIcon {QString{":/icons/Trash.svg"}});
+ del->setIcon(QIcon{QString{":/icons/Trash.svg"}});
del->setFixedWidth(40);
del->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Minimum);
connect(del, SIGNAL(clicked(bool)), this, SLOT(deleteController()));
@@ -343,12 +379,18 @@ namespace armarx
{
for (const auto& descr : controller->getFunctionDescriptions())
{
- new NJointControllersWidgetRemoteFunction {treeWidget, *header, descr.first, controller, descr.second}; //adds it self to the widget
+ new NJointControllersWidgetRemoteFunction{
+ treeWidget,
+ *header,
+ descr.first,
+ controller,
+ descr.second}; //adds it self to the widget
}
}
}
- void NJointControllersWidgetEntry::update(const NJointControllerStatus& status)
+ void
+ NJointControllersWidgetEntry::update(const NJointControllerStatus& status)
{
header->setText(NJointControllersWidget::idxActive, status.active ? "X" : " ");
header->setText(NJointControllersWidget::idxRequested, status.requested ? "X" : " ");
@@ -358,35 +400,44 @@ namespace armarx
header->setBackground(NJointControllersWidget::idxRequested, {acReqColor});
header->setBackground(NJointControllersWidget::idxError, {status.error ? red() : green()});
//name
- header->setBackground(NJointControllersWidget::idxName, {status.error ? red() : acReqColor});
+ header->setBackground(NJointControllersWidget::idxName,
+ {status.error ? red() : acReqColor});
}
- bool NJointControllersWidgetEntry::matchName(const QString& name)
+ bool
+ NJointControllersWidgetEntry::matchName(const QString& name)
{
return header->text(NJointControllersWidget::idxName).contains(name, Qt::CaseInsensitive);
}
- bool NJointControllersWidgetEntry::matchClass(const QString& name)
+ bool
+ NJointControllersWidgetEntry::matchClass(const QString& name)
{
return header->text(NJointControllersWidget::idxClass).contains(name, Qt::CaseInsensitive);
}
- bool NJointControllersWidgetEntry::isActiveState(const QString& state)
+ bool
+ NJointControllersWidgetEntry::isActiveState(const QString& state)
{
- return header->text(NJointControllersWidget::idxActive).contains(state, Qt::CaseInsensitive);
+ return header->text(NJointControllersWidget::idxActive)
+ .contains(state, Qt::CaseInsensitive);
}
- bool NJointControllersWidgetEntry::isRequestedState(const QString& state)
+ bool
+ NJointControllersWidgetEntry::isRequestedState(const QString& state)
{
- return header->text(NJointControllersWidget::idxRequested).contains(state, Qt::CaseInsensitive);
+ return header->text(NJointControllersWidget::idxRequested)
+ .contains(state, Qt::CaseInsensitive);
}
- bool NJointControllersWidgetEntry::isErrorState(const QString& state)
+ bool
+ NJointControllersWidgetEntry::isErrorState(const QString& state)
{
return header->text(NJointControllersWidget::idxError).contains(state, Qt::CaseInsensitive);
}
- bool NJointControllersWidgetEntry::matchDevice(const QString& dev)
+ bool
+ NJointControllersWidgetEntry::matchDevice(const QString& dev)
{
for (const auto& elem : devsToModes)
{
@@ -398,11 +449,13 @@ namespace armarx
return false;
}
- bool NJointControllersWidgetEntry::matchMode(const QString& mode)
+ bool
+ NJointControllersWidgetEntry::matchMode(const QString& mode)
{
for (const auto& elem : devsToModes)
{
- if (elem->text(NJointControllersWidget::idxCtrlMode).contains(mode, Qt::CaseInsensitive))
+ if (elem->text(NJointControllersWidget::idxCtrlMode)
+ .contains(mode, Qt::CaseInsensitive))
{
return true;
}
@@ -410,7 +463,8 @@ namespace armarx
return false;
}
- void NJointControllersWidgetEntry::deleteContent()
+ void
+ NJointControllersWidgetEntry::deleteContent()
{
for (auto item : devsToModes)
{
@@ -419,31 +473,36 @@ namespace armarx
delete header;
}
- void NJointControllersWidgetEntry::setVisible(bool vis)
+ void
+ NJointControllersWidgetEntry::setVisible(bool vis)
{
setDeviceListVisible(vis);
header->setHidden(!vis);
}
- void NJointControllersWidgetEntry::activateController()
+ void
+ NJointControllersWidgetEntry::activateController()
{
ARMARX_CHECK_EXPRESSION(controller);
controller->activateController();
}
- void NJointControllersWidgetEntry::deactivateController()
+ void
+ NJointControllersWidgetEntry::deactivateController()
{
ARMARX_CHECK_EXPRESSION(controller);
controller->deactivateController();
}
- void NJointControllersWidgetEntry::deleteController()
+ void
+ NJointControllersWidgetEntry::deleteController()
{
ARMARX_CHECK_EXPRESSION(controller);
controller->deleteController();
}
- void NJointControllersWidgetEntry::hideDeviceList()
+ void
+ NJointControllersWidgetEntry::hideDeviceList()
{
QCheckBox* box = dynamic_cast<QCheckBox*>(sender());
if (box)
@@ -461,7 +520,9 @@ namespace armarx
}
}
}
- void NJointControllersWidgetEntry::setDeviceListVisible(bool vis)
+
+ void
+ NJointControllersWidgetEntry::setDeviceListVisible(bool vis)
{
boxDev->setChecked(vis);
boxMod->setChecked(vis);
@@ -476,10 +537,8 @@ namespace armarx
QTreeWidgetItem& header,
const std::string& name,
const NJointControllerInterfacePrx& ctrl,
- const WidgetDescription::WidgetPtr& w
- ):
- functionName {name},
- ctrl {ctrl}
+ const WidgetDescription::WidgetPtr& w) :
+ functionName{name}, ctrl{ctrl}
{
//tree widget item
{
@@ -495,7 +554,7 @@ namespace armarx
//exec button
{
QPushButton* execute = new QPushButton;
- execute->setIcon(QIcon {QString{":/icons/media-playback-start.ico"}});
+ execute->setIcon(QIcon{QString{":/icons/media-playback-start.ico"}});
execute->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Minimum);
execute->setFixedWidth(40);
execute->setToolTip("Execute '" + QString::fromStdString(name) + "' function");
@@ -503,14 +562,16 @@ namespace armarx
l->addWidget(execute);
}
//param change box
- execOnParamChange = new QCheckBox {"Call function on parameter changes"};
- execOnParamChange->setToolTip("Execute '" + QString::fromStdString(name) + "' function whenever a parameter changes");
+ execOnParamChange = new QCheckBox{"Call function on parameter changes"};
+ execOnParamChange->setToolTip("Execute '" + QString::fromStdString(name) +
+ "' function whenever a parameter changes");
l->addWidget(execOnParamChange);
if (!w)
{
//this way we always have the correct horizontal spacing
execOnParamChange->setFixedHeight(0);
- execOnParamChange->setSizePolicy(execOnParamChange->sizePolicy().horizontalPolicy(), QSizePolicy::Fixed);
+ execOnParamChange->setSizePolicy(execOnParamChange->sizePolicy().horizontalPolicy(),
+ QSizePolicy::Fixed);
}
//name
l->addWidget(new QLabel{"Remote function: " + QString::fromStdString(name)});
@@ -534,20 +595,23 @@ namespace armarx
params->setContentsMargins(0, 0, 0, 0);
compress->addWidget(params);
//add spacer
- compress->addItem(new QSpacerItem {0, 0, QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding});
+ compress->addItem(new QSpacerItem{
+ 0, 0, QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding});
}
}
- void NJointControllersWidgetRemoteFunction::execFunction()
+ void
+ NJointControllersWidgetRemoteFunction::execFunction()
{
if (paramValues.empty())
{
- ARMARX_INFO << deactivateSpam(1, functionName) << "calling function " << functionName << " with no parameters\n";
+ ARMARX_INFO << deactivateSpam(1, functionName) << "calling function " << functionName
+ << " with no parameters\n";
}
else
{
std::stringstream ss;
- ss << "calling function " << functionName << " with parameters:\n";
+ ss << "calling function " << functionName << " with parameters:\n";
for (const auto& pair : paramValues)
{
if (pair.second)
@@ -555,7 +619,8 @@ namespace armarx
if (pair.second->data)
{
- ss << " '" << pair.first << "' of type " << pair.second->data->ice_id() << "\n";
+ ss << " '" << pair.first << "' of type " << pair.second->data->ice_id()
+ << "\n";
}
else
{
@@ -572,7 +637,8 @@ namespace armarx
ctrl->callDescribedFunction(functionName, paramValues);
}
- void NJointControllersWidgetRemoteFunction::valueChangedSlot(std::string name, VariantBasePtr value)
+ void
+ NJointControllersWidgetRemoteFunction::valueChangedSlot(std::string name, VariantBasePtr value)
{
paramValues[std::move(name)] = std::move(value);
if (execOnParamChange->isChecked())
@@ -580,4 +646,4 @@ namespace armarx
execFunction();
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.h
index f1c9a4ea405a82a62b2f7bc66b99f2a4606df180..3556499a8c33d2a1bae66e68a98fba63918b732e 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.h
@@ -22,27 +22,29 @@
#pragma once
-#include <mutex>
#include <atomic>
+#include <mutex>
-#include <QWidget>
+#include <QCheckBox>
+#include <QFormLayout>
+#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
-#include <QHBoxLayout>
-#include <QFormLayout>
#include <QPushButton>
#include <QTreeWidget>
#include <QTreeWidgetItem>
-#include <QCheckBox>
+#include <QWidget>
#include <ArmarXCore/core/util/algorithm.h>
-#include <ArmarXGui/libraries/WidgetDescription/WidgetDescription.h>
+
#include <ArmarXGui/libraries/VariantWidget/VariantWidget.h>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <ArmarXGui/libraries/WidgetDescription/WidgetDescription.h>
+
+#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_NJointControllersWidget.h>
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "RobotUnitWidgetBase.h"
-#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_NJointControllersWidget.h>
namespace armarx
{
@@ -82,17 +84,17 @@ namespace armarx
void onPushButtonRemoveAll_clicked();
public:
- static constexpr int idxName = 0;
- static constexpr int idxClass = 1;
- static constexpr int idxActive = 2;
- static constexpr int idxRequested = 3;
- static constexpr int idxError = 4;
- static constexpr int idxInternal = 5;
- static constexpr int idxCtrlDev = 6;
- static constexpr int idxCtrlMode = 7;
- static constexpr int idxActivate = 8;
+ static constexpr int idxName = 0;
+ static constexpr int idxClass = 1;
+ static constexpr int idxActive = 2;
+ static constexpr int idxRequested = 3;
+ static constexpr int idxError = 4;
+ static constexpr int idxInternal = 5;
+ static constexpr int idxCtrlDev = 6;
+ static constexpr int idxCtrlMode = 7;
+ static constexpr int idxActivate = 8;
static constexpr int idxDeactivate = 9;
- static constexpr int idxDelete = 10;
+ static constexpr int idxDelete = 10;
};
class NJointControllersWidgetEntry : public QObject
@@ -101,11 +103,9 @@ namespace armarx
public:
friend class NJointControllersWidget;
- NJointControllersWidgetEntry(
- NJointControllersWidget& parent,
- QTreeWidget& treeWidget,
- const NJointControllerDescription& desc
- );
+ NJointControllersWidgetEntry(NJointControllersWidget& parent,
+ QTreeWidget& treeWidget,
+ const NJointControllerDescription& desc);
void update(const NJointControllerStatus& status);
@@ -128,6 +128,7 @@ namespace armarx
void hideDeviceList();
void setDeviceListVisible(bool vis);
+
private:
const bool deletable;
QCheckBox* boxDev;
@@ -135,21 +136,21 @@ namespace armarx
QTreeWidgetItem* header;
NJointControllerInterfacePrx controller;
std::vector<QTreeWidgetItem*> devsToModes;
-
};
- class NJointControllersWidgetRemoteFunction : public WidgetDescription::ValueChangedListenerInterface
+ class NJointControllersWidgetRemoteFunction :
+ public WidgetDescription::ValueChangedListenerInterface
{
Q_OBJECT
public:
- NJointControllersWidgetRemoteFunction(
- QTreeWidget& treeWidget,
- QTreeWidgetItem& header,
- const std::string& functionName,
- const NJointControllerInterfacePrx& ctrl,
- const WidgetDescription::WidgetPtr& w);
+ NJointControllersWidgetRemoteFunction(QTreeWidget& treeWidget,
+ QTreeWidgetItem& header,
+ const std::string& functionName,
+ const NJointControllerInterfacePrx& ctrl,
+ const WidgetDescription::WidgetPtr& w);
private slots:
void execFunction();
+
private:
QTreeWidgetItem* functionHeader;
@@ -164,4 +165,4 @@ namespace armarx
public slots:
void valueChangedSlot(std::string name, VariantBasePtr value) override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
index 9141512a800d382cbe917659b1fd236b7598bae8..ccce3b687b4ce5995393d9b7104859fd6dd77947 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
@@ -20,34 +20,36 @@
* GNU General Public License
*/
#include "RobotUnitWidgetBase.h"
-#include <ArmarXCore/core/exceptions/Exception.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <thread>
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx
{
- RobotUnitWidgetBase::RobotUnitWidgetBase(QString name, QWidget* parent) :
- QWidget(parent)
+ RobotUnitWidgetBase::RobotUnitWidgetBase(QString name, QWidget* parent) : QWidget(parent)
{
setObjectName(name);
}
RobotUnitWidgetBase::~RobotUnitWidgetBase()
{
- while (_resetThread.joinable() && ! _resetThread.try_join_for(boost::chrono::milliseconds{100}))
+ while (_resetThread.joinable() &&
+ !_resetThread.try_join_for(boost::chrono::milliseconds{100}))
{
ARMARX_INFO << "Joining reset thread";
}
}
- void RobotUnitWidgetBase::updateContent()
+ void
+ RobotUnitWidgetBase::updateContent()
{
if (isResetting)
{
return;
}
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
resetCount = 0;
getResettigLabel().setText("Resetting... " + QString::number(resetCount));
getStackedWidget().setCurrentIndex(0);
@@ -58,16 +60,18 @@ namespace armarx
doContentUpdate();
}
- void RobotUnitWidgetBase::reset(RobotUnitInterfacePrx ru)
+ void
+ RobotUnitWidgetBase::reset(RobotUnitInterfacePrx ru)
{
isResetting = true;
gotResetData = false;
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
robotUnit = ru;
QMetaObject::invokeMethod(this, "doReset", Qt::QueuedConnection);
}
- void RobotUnitWidgetBase::setVisible(bool visible)
+ void
+ RobotUnitWidgetBase::setVisible(bool visible)
{
doMetaCall = visible;
if (visible)
@@ -77,40 +81,42 @@ namespace armarx
QWidget::setVisible(visible);
}
- void RobotUnitWidgetBase::doReset()
+ void
+ RobotUnitWidgetBase::doReset()
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
clearAll();
getTreeWidget().clear();
addFilter();
if (robotUnit)
{
- if (!_resetThread.joinable() || _resetThread.try_join_for(boost::chrono::milliseconds{10}))
+ if (!_resetThread.joinable() ||
+ _resetThread.try_join_for(boost::chrono::milliseconds{10}))
{
resetTimerId = startTimer(0);
getStackedWidget().setCurrentIndex(1);
- _resetThread = boost::thread
- {
+ _resetThread = boost::thread{
[&]
{
- ARMARX_INFO << "Resetting of widget '" << objectName().toStdString() << "' started";
+ ARMARX_INFO << "Resetting of widget '" << objectName().toStdString()
+ << "' started";
try
{
getResetData();
}
catch (...)
{
- ARMARX_WARNING << "Resetting of widget '" << objectName().toStdString() << "' failed: "
- << armarx::GetHandledExceptionString();
+ ARMARX_WARNING << "Resetting of widget '" << objectName().toStdString()
+ << "' failed: " << armarx::GetHandledExceptionString();
}
gotResetData = true;
if (doMetaCall)
{
QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
}
- ARMARX_INFO << "Resetting of widget '" << objectName().toStdString() << "' done!";
- }
- };
+ ARMARX_INFO << "Resetting of widget '" << objectName().toStdString()
+ << "' done!";
+ }};
}
}
else
@@ -123,12 +129,13 @@ namespace armarx
}
}
- void RobotUnitWidgetBase::timerEvent(QTimerEvent*)
+ void
+ RobotUnitWidgetBase::timerEvent(QTimerEvent*)
{
if (isResetting && gotResetData)
{
getResettigLabel().setText("Resetting... " + QString::number(++resetCount));
- isResetting = ! addOneFromResetData();
+ isResetting = !addOneFromResetData();
if (!isResetting)
{
killTimer(resetTimerId);
@@ -140,4 +147,4 @@ namespace armarx
}
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.h
index 5490027d100494de5f2f3b3e674dc3bbb392a26c..16668b77d3e16ecfbfd4d789ba0de3520ebf2836 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.h
@@ -22,22 +22,22 @@
#pragma once
-#include <mutex>
#include <atomic>
+#include <mutex>
#include <boost/thread.hpp>
-#include <QWidget>
+#include <QCheckBox>
+#include <QComboBox>
+#include <QFormLayout>
+#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
-#include <QHBoxLayout>
-#include <QFormLayout>
#include <QPushButton>
-#include <QTreeWidget>
-#include <QCheckBox>
-#include <QComboBox>
-#include <QStackedWidget>
#include <QSettings>
+#include <QStackedWidget>
+#include <QTreeWidget>
+#include <QWidget>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
@@ -53,8 +53,15 @@ namespace armarx
~RobotUnitWidgetBase();
void reset(RobotUnitInterfacePrx ru);
- virtual void loadSettings(QSettings*) {}
- virtual void saveSettings(QSettings*) {}
+ virtual void
+ loadSettings(QSettings*)
+ {
+ }
+
+ virtual void
+ saveSettings(QSettings*)
+ {
+ }
void setVisible(bool visible) override;
protected slots:
@@ -66,7 +73,12 @@ namespace armarx
virtual QTreeWidget& getTreeWidget() = 0;
virtual QStackedWidget& getStackedWidget() = 0;
virtual QLabel& getResettigLabel() = 0;
- virtual void addFilter() {}
+
+ virtual void
+ addFilter()
+ {
+ }
+
virtual void clearAll() = 0;
virtual void doContentUpdate() = 0;
@@ -80,40 +92,45 @@ namespace armarx
RobotUnitInterfacePrx robotUnit;
mutable std::recursive_timed_mutex mutex;
- std::atomic_bool gotResetData {false};
- int resetTimerId {0};
- int resetCount {0};
- std::atomic_bool isResetting {false};
- std::atomic_bool doMetaCall {false};
+ std::atomic_bool gotResetData{false};
+ int resetTimerId{0};
+ int resetCount{0};
+ std::atomic_bool isResetting{false};
+ std::atomic_bool doMetaCall{false};
+
private:
boost::thread _resetThread;
};
- template<class UI>
+ template <class UI>
class RobotUnitWidgetTemplateBase : public RobotUnitWidgetBase
{
public:
- RobotUnitWidgetTemplateBase(QString name, QWidget* parent):
- RobotUnitWidgetBase(name, parent),
- ui {new UI}
+ RobotUnitWidgetTemplateBase(QString name, QWidget* parent) :
+ RobotUnitWidgetBase(name, parent), ui{new UI}
{
ui->setupUi(this);
}
protected:
- QTreeWidget& getTreeWidget() final override
+ QTreeWidget&
+ getTreeWidget() final override
{
return *(ui->treeWidget);
}
- QStackedWidget& getStackedWidget() final override
+
+ QStackedWidget&
+ getStackedWidget() final override
{
return *(ui->stackedWidget);
}
- QLabel& getResettigLabel() final override
+
+ QLabel&
+ getResettigLabel() final override
{
return *(ui->labelResetting);
}
UI* ui;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.cpp
index 8d01966fb49bb97311126ae13f8d5bcca885c24c..08c8322bb63d537d8927d2ba0ca96300b7e60991 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.cpp
@@ -21,10 +21,10 @@
*/
#include "SensorDevicesWidget.h"
-#include "StyleSheets.h"
-
#include <QCheckBox>
+#include "StyleSheets.h"
+
namespace armarx
{
SensorDevicesWidget::SensorDevicesWidget(QWidget* parent) :
@@ -50,9 +50,10 @@ namespace armarx
delete ui;
}
- void SensorDevicesWidget::sensorDeviceStatusChanged(const SensorDeviceStatusSeq& allStatus)
+ void
+ SensorDevicesWidget::sensorDeviceStatusChanged(const SensorDeviceStatusSeq& allStatus)
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex, std::defer_lock};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex, std::defer_lock};
if (!guard.try_lock_for(std::chrono::microseconds(100)))
{
return;
@@ -70,14 +71,16 @@ namespace armarx
}
}
- void SensorDevicesWidget::clearAll()
+ void
+ SensorDevicesWidget::clearAll()
{
entries.clear();
statusUpdates.clear();
resetData.clear();
}
- void SensorDevicesWidget::doContentUpdate()
+ void
+ SensorDevicesWidget::doContentUpdate()
{
for (const auto& pair : statusUpdates)
{
@@ -90,16 +93,18 @@ namespace armarx
statusUpdates.clear();
}
- void SensorDevicesWidget::getResetData()
+ void
+ SensorDevicesWidget::getResetData()
{
auto temp = robotUnit->getSensorDeviceDescriptions();
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
resetData = std::move(temp);
}
}
- bool SensorDevicesWidget::addOneFromResetData()
+ bool
+ SensorDevicesWidget::addOneFromResetData()
{
if (resetData.empty())
{
@@ -110,17 +115,20 @@ namespace armarx
return false;
}
- void SensorDevicesWidget::add(const SensorDeviceDescription& desc)
+ void
+ SensorDevicesWidget::add(const SensorDeviceDescription& desc)
{
- std::unique_lock<std::recursive_timed_mutex> guard {mutex};
+ std::unique_lock<std::recursive_timed_mutex> guard{mutex};
if (!entries.count(desc.deviceName))
{
entries[desc.deviceName] = new SensorDevicesWidgetEntry(*this, *(ui->treeWidget), desc);
}
}
- SensorDevicesWidgetEntry::SensorDevicesWidgetEntry(SensorDevicesWidget& parent, QTreeWidget& treeWidget, const SensorDeviceDescription& desc):
- QObject {&parent}
+ SensorDevicesWidgetEntry::SensorDevicesWidgetEntry(SensorDevicesWidget& parent,
+ QTreeWidget& treeWidget,
+ const SensorDeviceDescription& desc) :
+ QObject{&parent}
{
header = new QTreeWidgetItem;
treeWidget.addTopLevelItem(header);
@@ -134,30 +142,34 @@ namespace armarx
connect(boxTags, SIGNAL(clicked(bool)), this, SLOT(hideTagList(bool)));
for (const auto& tag : desc.tags)
{
- QTreeWidgetItem* child = new QTreeWidgetItem {{QString::fromStdString(tag)}};
+ QTreeWidgetItem* child = new QTreeWidgetItem{{QString::fromStdString(tag)}};
treeWidget.addTopLevelItem(child);
tags.emplace_back(child);
}
}
- header->setText(SensorDevicesWidget::idxValType, QString::fromStdString(desc.sensorValueType));
+ header->setText(SensorDevicesWidget::idxValType,
+ QString::fromStdString(desc.sensorValueType));
value = new VariantWidget;
QTreeWidgetItem* valchild = new QTreeWidgetItem;
header->addChild(valchild);
treeWidget.setItemWidget(valchild, SensorDevicesWidget::idxValue, value);
}
- void SensorDevicesWidgetEntry::update(const SensorDeviceStatus& status)
+ void
+ SensorDevicesWidgetEntry::update(const SensorDeviceStatus& status)
{
value->update(status.sensorValue);
value->layout()->setContentsMargins(0, 0, 0, 0);
}
- bool SensorDevicesWidgetEntry::matchName(const QString& name)
+ bool
+ SensorDevicesWidgetEntry::matchName(const QString& name)
{
return header->text(SensorDevicesWidget::idxName).contains(name, Qt::CaseInsensitive);
}
- bool SensorDevicesWidgetEntry::matchTag(const QString& tag)
+ bool
+ SensorDevicesWidgetEntry::matchTag(const QString& tag)
{
for (const auto& tagit : tags)
{
@@ -169,12 +181,14 @@ namespace armarx
return false;
}
- bool SensorDevicesWidgetEntry::matchValueType(const QString& type)
+ bool
+ SensorDevicesWidgetEntry::matchValueType(const QString& type)
{
return header->text(SensorDevicesWidget::idxValType).contains(type, Qt::CaseInsensitive);
}
- void SensorDevicesWidgetEntry::setVisible(bool vis)
+ void
+ SensorDevicesWidgetEntry::setVisible(bool vis)
{
if (!vis)
{
@@ -183,7 +197,8 @@ namespace armarx
header->setHidden(!vis);
}
- void SensorDevicesWidgetEntry::hideTagList(bool hide)
+ void
+ SensorDevicesWidgetEntry::hideTagList(bool hide)
{
for (QTreeWidgetItem* tag : tags)
{
@@ -191,4 +206,4 @@ namespace armarx
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.h
index c79f7c52357c339e3e508a60fba6d7beff32073f..ac2aa2604c1110a9a82f8dab6224445c3ac9a865 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.h
@@ -22,25 +22,25 @@
#pragma once
-#include <mutex>
#include <atomic>
+#include <mutex>
-#include <QWidget>
+#include <QFormLayout>
+#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
-#include <QHBoxLayout>
-#include <QFormLayout>
#include <QPushButton>
#include <QTreeWidget>
#include <QTreeWidgetItem>
-#include <QTreeWidgetItem>
+#include <QWidget>
-#include <ArmarXGui/libraries/WidgetDescription/WidgetDescription.h>
#include <ArmarXGui/libraries/VariantWidget/VariantWidget.h>
+#include <ArmarXGui/libraries/WidgetDescription/WidgetDescription.h>
+
+#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_SensorDevicesWidget.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "RobotUnitWidgetBase.h"
-#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_SensorDevicesWidget.h>
namespace armarx
{
@@ -58,6 +58,7 @@ namespace armarx
void doContentUpdate() override;
void getResetData() override;
bool addOneFromResetData() override;
+
private:
void add(const SensorDeviceDescription& desc);
@@ -66,10 +67,10 @@ namespace armarx
SensorDeviceDescriptionSeq resetData;
public:
- static constexpr int idxName = 0;
- static constexpr int idxTags = 1;
+ static constexpr int idxName = 0;
+ static constexpr int idxTags = 1;
static constexpr int idxValType = 2;
- static constexpr int idxValue = 3;
+ static constexpr int idxValue = 3;
};
class SensorDevicesWidgetEntry : QObject
@@ -77,11 +78,9 @@ namespace armarx
Q_OBJECT
public:
- SensorDevicesWidgetEntry(
- SensorDevicesWidget& parent,
- QTreeWidget& treeWidget,
- const SensorDeviceDescription& desc
- );
+ SensorDevicesWidgetEntry(SensorDevicesWidget& parent,
+ QTreeWidget& treeWidget,
+ const SensorDeviceDescription& desc);
void update(const SensorDeviceStatus& status);
@@ -100,5 +99,4 @@ namespace armarx
VariantWidget* value;
std::vector<QTreeWidgetItem*> tags;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/StyleSheets.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/StyleSheets.h
index 76fdae08c6766f3aeb0b9dce22fe0266748fde99..ffe09b3938ee57bae1d3fe025f4f875bba1f5f43 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/StyleSheets.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/StyleSheets.h
@@ -22,68 +22,73 @@
#pragma once
-#include <QString>
#include <QColor>
+#include <QString>
namespace armarx
{
- inline QString checkboxStyleSheet()
+ inline QString
+ checkboxStyleSheet()
{
- return
- "QCheckBox::indicator {\n" \
- " width: 10px;\n" \
- " height: 10px;\n" \
- "}\n" \
- "QCheckBox::indicator:checked\n" \
- "{\n" \
- " image: url(:/icons/TriangleBlackDown.svg);\n" \
- "}\n" \
- "QCheckBox::indicator:unchecked\n" \
- "{\n" \
- " image: url(:/icons/TriangleBlackRight.svg);\n" \
- "}\n" \
- "QCheckBox::indicator:checked:hover\n" \
- "{\n" \
- " image: url(:/icons/TriangleBlackDown.svg);\n" \
- "}\n" \
- "QCheckBox::indicator:unchecked:hover\n" \
- "{\n" \
- " image: url(:/icons/TriangleBlackRight.svg);\n" \
- "}\n" \
- "QCheckBox::indicator:checked:pressed\n" \
- "{\n" \
- " image: url(:/icons/TriangleBlackDown.svg);\n" \
- "}\n" \
- "QCheckBox::indicator:unchecked:pressed\n" \
- "{\n" \
- " image: url(:/icons/TriangleBlackRight.svg);\n" \
- "}\n" \
- "QCheckBox::indicator:checked:disabled\n" \
- "{\n" \
- " image: url(:/icons/TriangleGrayDown.svg);\n" \
- "}\n" \
- "QCheckBox::indicator:unchecked:disabled\n" \
- "{\n" \
- " image: url(:/icons/TriangleGrayRight.svg);\n" \
- "}";
+ return "QCheckBox::indicator {\n"
+ " width: 10px;\n"
+ " height: 10px;\n"
+ "}\n"
+ "QCheckBox::indicator:checked\n"
+ "{\n"
+ " image: url(:/icons/TriangleBlackDown.svg);\n"
+ "}\n"
+ "QCheckBox::indicator:unchecked\n"
+ "{\n"
+ " image: url(:/icons/TriangleBlackRight.svg);\n"
+ "}\n"
+ "QCheckBox::indicator:checked:hover\n"
+ "{\n"
+ " image: url(:/icons/TriangleBlackDown.svg);\n"
+ "}\n"
+ "QCheckBox::indicator:unchecked:hover\n"
+ "{\n"
+ " image: url(:/icons/TriangleBlackRight.svg);\n"
+ "}\n"
+ "QCheckBox::indicator:checked:pressed\n"
+ "{\n"
+ " image: url(:/icons/TriangleBlackDown.svg);\n"
+ "}\n"
+ "QCheckBox::indicator:unchecked:pressed\n"
+ "{\n"
+ " image: url(:/icons/TriangleBlackRight.svg);\n"
+ "}\n"
+ "QCheckBox::indicator:checked:disabled\n"
+ "{\n"
+ " image: url(:/icons/TriangleGrayDown.svg);\n"
+ "}\n"
+ "QCheckBox::indicator:unchecked:disabled\n"
+ "{\n"
+ " image: url(:/icons/TriangleGrayRight.svg);\n"
+ "}";
}
-
- inline QColor green()
+ inline QColor
+ green()
{
return {0, 255, 0};
}
- inline QColor red()
+
+ inline QColor
+ red()
{
return {240, 128, 128};
}
- inline QColor orange()
+
+ inline QColor
+ orange()
{
return {255, 193, 37};
}
- inline QColor transparent()
+
+ inline QColor
+ transparent()
{
return {0, 0, 0, 0};
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
index 5e117d1e62dfaa3c0dd00b996d0aaff662e6b5fd..c27502aefd08deaebac99f40a3b5f8f6510caee9 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
@@ -21,20 +21,21 @@
*/
#include "RobotUnitPluginWidgetController.h"
-#include "../QWidgets/StyleSheets.h"
-#include <string>
-#include <regex>
#include <filesystem>
+#include <regex>
+#include <string>
+#include <QAction>
#include <QDir>
#include <QSortFilterProxyModel>
-#include <QAction>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
+#include "../QWidgets/StyleSheets.h"
+
using namespace armarx;
RobotUnitPluginWidgetController::RobotUnitPluginWidgetController()
@@ -70,31 +71,55 @@ RobotUnitPluginWidgetController::RobotUnitPluginWidgetController()
nJointControllerClasses = new NJointControllerClassesWidget;
widget.groupBoxNJointCtrlClasses->layout()->addWidget(nJointControllerClasses);
widget.groupBoxNJointCtrlClasses->setVisible(false);
- nJointControllerClasses->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Expanding);
+ nJointControllerClasses->setSizePolicy(QSizePolicy::MinimumExpanding,
+ QSizePolicy::Expanding);
nJointControllerClasses->setVisible(false);
}
//update logging
{
widget.groupBoxLogging->setVisible(false);
- widget.groupBoxLogging->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Expanding);
-
- connect(widget.pushButtonLoggingStart, SIGNAL(clicked()), this, SLOT(on_pushButtonLoggingStart_clicked()));
- connect(widget.pushButtonLoggingStop, SIGNAL(clicked()), this, SLOT(on_pushButtonLoggingStop_clicked()));
- connect(widget.pushButtonLoggingMark1, SIGNAL(clicked()), this, SLOT(on_pushButtonLoggingMark1_clicked()));
- connect(widget.pushButtonLoggingMark2, SIGNAL(clicked()), this, SLOT(on_pushButtonLoggingMark2_clicked()));
- connect(widget.pushButtonLoggingMark3, SIGNAL(clicked()), this, SLOT(on_pushButtonLoggingMark3_clicked()));
- connect(widget.lineEditLoggingFilter, SIGNAL(textChanged(const QString&)),
- this, SLOT(on_lineEditLoggingFilter_textChanged(const QString&)));
- connect(widget.treeWidgetLoggingNames, SIGNAL(itemChanged(QTreeWidgetItem*, int)),
- this, SLOT(on_treeWidgetLoggingNames_itemChanged(QTreeWidgetItem*, int)));
+ widget.groupBoxLogging->setSizePolicy(QSizePolicy::MinimumExpanding,
+ QSizePolicy::Expanding);
+
+ connect(widget.pushButtonLoggingStart,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_pushButtonLoggingStart_clicked()));
+ connect(widget.pushButtonLoggingStop,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_pushButtonLoggingStop_clicked()));
+ connect(widget.pushButtonLoggingMark1,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_pushButtonLoggingMark1_clicked()));
+ connect(widget.pushButtonLoggingMark2,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_pushButtonLoggingMark2_clicked()));
+ connect(widget.pushButtonLoggingMark3,
+ SIGNAL(clicked()),
+ this,
+ SLOT(on_pushButtonLoggingMark3_clicked()));
+ connect(widget.lineEditLoggingFilter,
+ SIGNAL(textChanged(const QString&)),
+ this,
+ SLOT(on_lineEditLoggingFilter_textChanged(const QString&)));
+ connect(widget.treeWidgetLoggingNames,
+ SIGNAL(itemChanged(QTreeWidgetItem*, int)),
+ this,
+ SLOT(on_treeWidgetLoggingNames_itemChanged(QTreeWidgetItem*, int)));
}
updateToolBarActionCheckedState();
}
-
-void RobotUnitPluginWidgetController::loadSettings(QSettings* settings)
+void
+RobotUnitPluginWidgetController::loadSettings(QSettings* settings)
{
- robotUnitProxyName = settings->value("robotUnitProxyName", QString::fromStdString(robotUnitProxyName)).toString().toStdString();
+ robotUnitProxyName =
+ settings->value("robotUnitProxyName", QString::fromStdString(robotUnitProxyName))
+ .toString()
+ .toStdString();
nJointControllerClasses->loadSettings(settings);
nJointControllers->loadSettings(settings);
controlDevices->loadSettings(settings);
@@ -106,7 +131,8 @@ void RobotUnitPluginWidgetController::loadSettings(QSettings* settings)
updateToolBarActionCheckedState();
}
-void RobotUnitPluginWidgetController::saveSettings(QSettings* settings)
+void
+RobotUnitPluginWidgetController::saveSettings(QSettings* settings)
{
settings->setValue("robotUnitProxyName", QString::fromStdString(robotUnitProxyName));
nJointControllerClasses->saveSettings(settings);
@@ -119,18 +145,22 @@ void RobotUnitPluginWidgetController::saveSettings(QSettings* settings)
settings->setValue("classVisible", widget.groupBoxNJointCtrlClasses->isVisible());
}
-void RobotUnitPluginWidgetController::onInitComponent()
+void
+RobotUnitPluginWidgetController::onInitComponent()
{
usingProxy(robotUnitProxyName);
ARMARX_INFO << "RobotUnitPluginWidgetController::onInitComponent()" << std::flush;
QMetaObject::invokeMethod(this, "startOnConnectTimer", Qt::QueuedConnection);
}
-void RobotUnitPluginWidgetController::onExitComponent()
+
+void
+RobotUnitPluginWidgetController::onExitComponent()
{
QMetaObject::invokeMethod(this, "stopOnConnectTimer", Qt::QueuedConnection);
}
-void RobotUnitPluginWidgetController::onConnectComponent()
+void
+RobotUnitPluginWidgetController::onConnectComponent()
{
std::lock_guard guard{robotUnitPrxMutex};
robotUnitPrx = getProxy<RobotUnitInterfacePrx>(robotUnitProxyName);
@@ -140,7 +170,8 @@ void RobotUnitPluginWidgetController::onConnectComponent()
timerLastIterationRuWasRunning = false;
}
-void RobotUnitPluginWidgetController::onDisconnectComponent()
+void
+RobotUnitPluginWidgetController::onDisconnectComponent()
{
std::lock_guard guard{robotUnitPrxMutex};
unsubscribeFromTopic(listenerTopicName);
@@ -151,7 +182,8 @@ void RobotUnitPluginWidgetController::onDisconnectComponent()
QMetaObject::invokeMethod(this, "refreshSensorDevicesClicked", Qt::QueuedConnection);
}
-QPointer<QDialog> RobotUnitPluginWidgetController::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+RobotUnitPluginWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
@@ -161,12 +193,14 @@ QPointer<QDialog> RobotUnitPluginWidgetController::getConfigDialog(QWidget* pare
return qobject_cast<SimpleConfigDialog*>(dialog);
}
-void RobotUnitPluginWidgetController::configured()
+void
+RobotUnitPluginWidgetController::configured()
{
robotUnitProxyName = dialog->getProxyName("RobotUnit");
}
-QPointer<QWidget> RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidget* parent)
+QPointer<QWidget>
+RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidget* parent)
{
if (customToolbar)
{
@@ -177,9 +211,11 @@ QPointer<QWidget> RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidg
customToolbar = new QToolBar(parent);
customToolbar->setToolButtonStyle(Qt::ToolButtonTextBesideIcon);
customToolbar->setIconSize(QSize(16, 16));
- customToolbar->addAction(QIcon(":/icons/view-refresh-7.png"), "", this, SLOT(refreshControlDevicesClicked()))
- ->setToolTip("Update the list of Control Devices");
- showCDevs = new QAction {"Control Devices", customToolbar};
+ customToolbar
+ ->addAction(
+ QIcon(":/icons/view-refresh-7.png"), "", this, SLOT(refreshControlDevicesClicked()))
+ ->setToolTip("Update the list of Control Devices");
+ showCDevs = new QAction{"Control Devices", customToolbar};
showCDevs->setCheckable(true);
showCDevs->setToolTip("Hide/Show the list of Control Devices");
connect(showCDevs, SIGNAL(toggled(bool)), widget.groupBoxCDev, SLOT(setVisible(bool)));
@@ -187,9 +223,11 @@ QPointer<QWidget> RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidg
customToolbar->addAction(showCDevs);
customToolbar->addSeparator();
- customToolbar->addAction(QIcon(":/icons/view-refresh-7.png"), "", this, SLOT(refreshSensorDevicesClicked()))
- ->setToolTip("Update the list of Sensor Devices");
- showSDevs = new QAction {"Sensor Devices", customToolbar};
+ customToolbar
+ ->addAction(
+ QIcon(":/icons/view-refresh-7.png"), "", this, SLOT(refreshSensorDevicesClicked()))
+ ->setToolTip("Update the list of Sensor Devices");
+ showSDevs = new QAction{"Sensor Devices", customToolbar};
showSDevs->setCheckable(true);
showSDevs->setToolTip("Hide/Show the list of Sensor Devices");
connect(showSDevs, SIGNAL(toggled(bool)), widget.groupBoxSDev, SLOT(setVisible(bool)));
@@ -197,110 +235,147 @@ QPointer<QWidget> RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidg
customToolbar->addAction(showSDevs);
customToolbar->addSeparator();
- customToolbar->addAction(QIcon(":/icons/view-refresh-7.png"), "", this, SLOT(refreshNJointControllersClicked()))
- ->setToolTip("Update the list of NJointControllers");
- showNJoint = new QAction {"NJointControllers", customToolbar};
+ customToolbar
+ ->addAction(QIcon(":/icons/view-refresh-7.png"),
+ "",
+ this,
+ SLOT(refreshNJointControllersClicked()))
+ ->setToolTip("Update the list of NJointControllers");
+ showNJoint = new QAction{"NJointControllers", customToolbar};
showNJoint->setCheckable(true);
showNJoint->setToolTip("Hide/Show the list of NJointControllers");
- connect(showNJoint, SIGNAL(toggled(bool)), widget.groupBoxNJointCtrl, SLOT(setVisible(bool)));
+ connect(
+ showNJoint, SIGNAL(toggled(bool)), widget.groupBoxNJointCtrl, SLOT(setVisible(bool)));
connect(showNJoint, SIGNAL(toggled(bool)), nJointControllers, SLOT(setVisible(bool)));
customToolbar->addAction(showNJoint);
customToolbar->addSeparator();
- customToolbar->addAction(QIcon(":/icons/view-refresh-7.png"), "", this, SLOT(refreshNJointControllerClassesClicked()))
- ->setToolTip("Update the list of NJointController Classes");
- showNJointClasses = new QAction {"NJointController Classes", customToolbar};
+ customToolbar
+ ->addAction(QIcon(":/icons/view-refresh-7.png"),
+ "",
+ this,
+ SLOT(refreshNJointControllerClassesClicked()))
+ ->setToolTip("Update the list of NJointController Classes");
+ showNJointClasses = new QAction{"NJointController Classes", customToolbar};
showNJointClasses->setCheckable(true);
showNJointClasses->setToolTip("Hide/Show the list of NJointControllers Classes");
- connect(showNJointClasses, SIGNAL(toggled(bool)), widget.groupBoxNJointCtrlClasses, SLOT(setVisible(bool)));
- connect(showNJointClasses, SIGNAL(toggled(bool)), nJointControllerClasses, SLOT(setVisible(bool)));
+ connect(showNJointClasses,
+ SIGNAL(toggled(bool)),
+ widget.groupBoxNJointCtrlClasses,
+ SLOT(setVisible(bool)));
+ connect(showNJointClasses,
+ SIGNAL(toggled(bool)),
+ nJointControllerClasses,
+ SLOT(setVisible(bool)));
customToolbar->addAction(showNJointClasses);
customToolbar->addSeparator();
- customToolbar->addAction(QIcon(":/icons/view-refresh-7.png"), "", this, SLOT(refreshLogging()));
- showLogging = new QAction {"Logging", customToolbar};
+ customToolbar->addAction(
+ QIcon(":/icons/view-refresh-7.png"), "", this, SLOT(refreshLogging()));
+ showLogging = new QAction{"Logging", customToolbar};
showLogging->setCheckable(true);
showLogging->setToolTip("Hide/Show the logging pane");
connect(showLogging, SIGNAL(toggled(bool)), widget.groupBoxLogging, SLOT(setVisible(bool)));
customToolbar->addAction(showLogging);
customToolbar->addSeparator();
- customToolbar->addAction(
- QIcon(":/icons/document-save.svg"), "Write log",
- [&]
- {
- std::lock_guard guard{robotUnitPrxMutex};
- robotUnitPrx->writeRecentIterationsToFile("/tmp/RobotUnitLog-{DateTime}");
- }
- )
- ->setToolTip("Writes the log to /tmp/");
+ customToolbar
+ ->addAction(QIcon(":/icons/document-save.svg"),
+ "Write log",
+ [&]
+ {
+ std::lock_guard guard{robotUnitPrxMutex};
+ robotUnitPrx->writeRecentIterationsToFile(
+ "/tmp/RobotUnitLog-{DateTime}");
+ })
+ ->setToolTip("Writes the log to /tmp/");
}
updateToolBarActionCheckedState();
return customToolbar.data();
}
-void RobotUnitPluginWidgetController::nJointControllerStatusChanged(const NJointControllerStatusSeq& status, const Ice::Current&)
+void
+RobotUnitPluginWidgetController::nJointControllerStatusChanged(
+ const NJointControllerStatusSeq& status,
+ const Ice::Current&)
{
nJointControllers->nJointControllerStatusChanged(status);
}
-void RobotUnitPluginWidgetController::controlDeviceStatusChanged(const ControlDeviceStatusSeq& status, const Ice::Current&)
+void
+RobotUnitPluginWidgetController::controlDeviceStatusChanged(const ControlDeviceStatusSeq& status,
+ const Ice::Current&)
{
controlDevices->controlDeviceStatusChanged(status);
}
-void RobotUnitPluginWidgetController::sensorDeviceStatusChanged(const SensorDeviceStatusSeq& status, const Ice::Current&)
+void
+RobotUnitPluginWidgetController::sensorDeviceStatusChanged(const SensorDeviceStatusSeq& status,
+ const Ice::Current&)
{
sensorDevices->sensorDeviceStatusChanged(status);
}
-void RobotUnitPluginWidgetController::nJointControllerClassAdded(const std::string& name, const Ice::Current&)
+void
+RobotUnitPluginWidgetController::nJointControllerClassAdded(const std::string& name,
+ const Ice::Current&)
{
nJointControllerClasses->nJointControllerClassAdded(name);
}
-void RobotUnitPluginWidgetController::nJointControllerCreated(const std::string& name, const Ice::Current&)
+void
+RobotUnitPluginWidgetController::nJointControllerCreated(const std::string& name,
+ const Ice::Current&)
{
nJointControllers->nJointControllerCreated(name);
}
-void RobotUnitPluginWidgetController::nJointControllerDeleted(const std::string& name, const Ice::Current&)
+void
+RobotUnitPluginWidgetController::nJointControllerDeleted(const std::string& name,
+ const Ice::Current&)
{
nJointControllers->nJointControllerDeleted(name);
}
-void RobotUnitPluginWidgetController::refreshNJointControllersClicked()
+void
+RobotUnitPluginWidgetController::refreshNJointControllersClicked()
{
std::lock_guard guard{robotUnitPrxMutex};
nJointControllers->reset(robotUnitPrx);
}
-void RobotUnitPluginWidgetController::refreshNJointControllerClassesClicked()
+void
+RobotUnitPluginWidgetController::refreshNJointControllerClassesClicked()
{
std::lock_guard guard{robotUnitPrxMutex};
nJointControllerClasses->reset(robotUnitPrx);
}
-void RobotUnitPluginWidgetController::refreshControlDevicesClicked()
+void
+RobotUnitPluginWidgetController::refreshControlDevicesClicked()
{
std::lock_guard guard{robotUnitPrxMutex};
controlDevices->reset(robotUnitPrx);
}
-void RobotUnitPluginWidgetController::refreshSensorDevicesClicked()
+void
+RobotUnitPluginWidgetController::refreshSensorDevicesClicked()
{
std::lock_guard guard{robotUnitPrxMutex};
sensorDevices->reset(robotUnitPrx);
}
-void RobotUnitPluginWidgetController::startOnConnectTimer()
+void
+RobotUnitPluginWidgetController::startOnConnectTimer()
{
if (!timerId)
{
timerId = startTimer(100);
}
}
-void RobotUnitPluginWidgetController::stopOnConnectTimer()
+
+void
+RobotUnitPluginWidgetController::stopOnConnectTimer()
{
if (timerId)
{
@@ -309,7 +384,8 @@ void RobotUnitPluginWidgetController::stopOnConnectTimer()
}
}
-void RobotUnitPluginWidgetController::updateToolBarActionCheckedState()
+void
+RobotUnitPluginWidgetController::updateToolBarActionCheckedState()
{
if (customToolbar)
{
@@ -320,7 +396,8 @@ void RobotUnitPluginWidgetController::updateToolBarActionCheckedState()
}
}
-void armarx::RobotUnitPluginWidgetController::timerEvent(QTimerEvent*)
+void
+armarx::RobotUnitPluginWidgetController::timerEvent(QTimerEvent*)
{
std::lock_guard guard{robotUnitPrxMutex};
if (robotUnitPrx && robotUnitPrx->isRunning())
@@ -347,7 +424,8 @@ void armarx::RobotUnitPluginWidgetController::timerEvent(QTimerEvent*)
}
}
-void armarx::RobotUnitPluginWidgetController::refreshLogging()
+void
+armarx::RobotUnitPluginWidgetController::refreshLogging()
{
std::lock_guard guard{robotUnitPrxMutex};
on_pushButtonLoggingStop_clicked();
@@ -358,7 +436,7 @@ void armarx::RobotUnitPluginWidgetController::refreshLogging()
const auto allLoggingNames = robotUnitPrx->getLoggingNames();
std::map<std::string, QTreeWidgetItem*> separators;
- auto add = [&](const std::string & name, const std::string & display, auto parent)
+ auto add = [&](const std::string& name, const std::string& display, auto parent)
{
QTreeWidgetItem* i = new QTreeWidgetItem(parent);
i->setText(0, QString::fromStdString(display) + "*");
@@ -378,7 +456,7 @@ void armarx::RobotUnitPluginWidgetController::refreshLogging()
{
ARMARX_CHECK_EXPRESSION(separators.count(reassembled));
auto parent = separators.at(reassembled);
- reassembled += (reassembled.empty() ? p : "." + p) ;
+ reassembled += (reassembled.empty() ? p : "." + p);
if (!separators.count(reassembled))
{
add(reassembled, p, parent);
@@ -386,10 +464,10 @@ void armarx::RobotUnitPluginWidgetController::refreshLogging()
}
loggingData.allItems.back()->setText(0, QString::fromStdString(parts.back()));
}
-
}
-void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked()
+void
+armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked()
{
std::lock_guard guard{robotUnitPrxMutex};
if (!robotUnitPrx || loggingData.handle || loggingData.streamingHandler)
@@ -404,11 +482,12 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked(
case Qt::Checked:
if (it->childCount() == 0)
{
- loggingNames.emplace_back(it->data(0, Qt::ToolTipRole).toString().toStdString());
+ loggingNames.emplace_back(
+ it->data(0, Qt::ToolTipRole).toString().toStdString());
}
else
{
- for (int i = 0; i < it->childCount(); ++i)
+ for (int i = 0; i < it->childCount(); ++i)
{
recurseChildren(it->child(i));
}
@@ -417,7 +496,7 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked(
case Qt::Unchecked:
break;
case Qt::PartiallyChecked:
- for (int i = 0; i < it->childCount(); ++i)
+ for (int i = 0; i < it->childCount(); ++i)
{
recurseChildren(it->child(i));
}
@@ -434,7 +513,7 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked(
loggingData.streamingHandler =
make_shared<RobotUnitDataStreamingReceiver>(this, robotUnitPrx, cfg);
- FileSystemPathBuilder pb {saveFormatString};
+ FileSystemPathBuilder pb{saveFormatString};
loggingData.logStream.open(pb.getPath());
loggingData.logStream << ";iteration;timestamp;TimeSinceLastIteration";
const auto& entries = loggingData.streamingHandler->getDataDescription().entries;
@@ -443,9 +522,7 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked(
for (const auto& [name, desc] : entries)
{
loggingData.logStream << ';' << name;
- str << " " << name
- << " -> type " << desc.type
- << ", index " << desc.index << '\n';
+ str << " " << name << " -> type " << desc.type << ", index " << desc.index << '\n';
}
loggingData.logStream << std::endl;
ARMARX_INFO << str.str();
@@ -462,7 +539,8 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked(
widget.pushButtonLoggingStop->setEnabled(true);
}
-void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStop_clicked()
+void
+armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStop_clicked()
{
std::lock_guard guard{robotUnitPrxMutex};
widget.pushButtonLoggingStart->setEnabled(true);
@@ -478,11 +556,12 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStop_clicked()
{
loggingData.logStream.close();
}
- loggingData.handle = nullptr;
+ loggingData.handle = nullptr;
loggingData.streamingHandler = nullptr;
}
-void armarx::RobotUnitPluginWidgetController::on_lineEditLoggingFilter_textChanged(const QString& arg1)
+void
+armarx::RobotUnitPluginWidgetController::on_lineEditLoggingFilter_textChanged(const QString& arg1)
{
std::regex reg;
try
@@ -492,12 +571,14 @@ void armarx::RobotUnitPluginWidgetController::on_lineEditLoggingFilter_textChang
catch (...)
{
//broken regex
- widget.lineEditLoggingFilter->setStyleSheet("QLineEdit { background: rgb(255, 150, 150); selection-background-color: rgb(255, 0, 0); }");
+ widget.lineEditLoggingFilter->setStyleSheet(
+ "QLineEdit { background: rgb(255, 150, 150); selection-background-color: rgb(255, 0, "
+ "0); }");
return;
}
widget.lineEditLoggingFilter->setStyleSheet("QLineEdit {}");
- std::function<void(QTreeWidgetItem* item)> setVisible = [&](auto it)
+ std::function<void(QTreeWidgetItem * item)> setVisible = [&](auto it)
{
it->setHidden(false);
auto parent = it->parent();
@@ -531,7 +612,10 @@ void armarx::RobotUnitPluginWidgetController::on_lineEditLoggingFilter_textChang
widget.treeWidgetLoggingNames->blockSignals(false);
}
-void armarx::RobotUnitPluginWidgetController::on_treeWidgetLoggingNames_itemChanged(QTreeWidgetItem* item, int)
+void
+armarx::RobotUnitPluginWidgetController::on_treeWidgetLoggingNames_itemChanged(
+ QTreeWidgetItem* item,
+ int)
{
widget.treeWidgetLoggingNames->blockSignals(true);
loggingData.updateCheckStateDownward(item, item->checkState(0), true);
@@ -539,7 +623,8 @@ void armarx::RobotUnitPluginWidgetController::on_treeWidgetLoggingNames_itemChan
widget.treeWidgetLoggingNames->blockSignals(false);
}
-void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark1_clicked()
+void
+armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark1_clicked()
{
if (!loggingData.handle)
{
@@ -550,7 +635,8 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark1_clicked(
robotUnitPrx->addMarkerToRtLog(loggingData.handle, mark);
}
-void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark2_clicked()
+void
+armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark2_clicked()
{
if (!loggingData.handle)
{
@@ -561,7 +647,8 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark2_clicked(
robotUnitPrx->addMarkerToRtLog(loggingData.handle, mark);
}
-void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark3_clicked()
+void
+armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark3_clicked()
{
if (!loggingData.handle)
{
@@ -572,7 +659,9 @@ void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark3_clicked(
robotUnitPrx->addMarkerToRtLog(loggingData.handle, mark);
}
-void RobotUnitPluginWidgetController::LoggingData::updateCheckStateUpward(QTreeWidgetItem* item, bool recurseParents)
+void
+RobotUnitPluginWidgetController::LoggingData::updateCheckStateUpward(QTreeWidgetItem* item,
+ bool recurseParents)
{
//set item state depending on its childrens state and maybe recurse parent
for (; item; item = recurseParents ? item->parent() : nullptr)
@@ -584,7 +673,7 @@ void RobotUnitPluginWidgetController::LoggingData::updateCheckStateUpward(QTreeW
bool anyChecked = false;
bool anyUnchecked = false;
bool anyTri = false;
- for (int i = 0; i < item->childCount(); ++i)
+ for (int i = 0; i < item->childCount(); ++i)
{
auto c = item->child(i);
if (c->isHidden())
@@ -620,10 +709,10 @@ void RobotUnitPluginWidgetController::LoggingData::updateCheckStateUpward(QTreeW
}
}
-void RobotUnitPluginWidgetController::LoggingData::updateCheckStateDownward(
- QTreeWidgetItem* item,
- Qt::CheckState state,
- bool recurseChildren)
+void
+RobotUnitPluginWidgetController::LoggingData::updateCheckStateDownward(QTreeWidgetItem* item,
+ Qt::CheckState state,
+ bool recurseChildren)
{
if (item->isHidden())
{
@@ -635,7 +724,7 @@ void RobotUnitPluginWidgetController::LoggingData::updateCheckStateDownward(
{
return;
}
- for (int i = 0; i < item->childCount(); ++i)
+ for (int i = 0; i < item->childCount(); ++i)
{
auto c = item->child(i);
if (c->isHidden())
@@ -647,7 +736,8 @@ void RobotUnitPluginWidgetController::LoggingData::updateCheckStateDownward(
}
}
-void RobotUnitPluginWidgetController::LoggingData::localLogging()
+void
+RobotUnitPluginWidgetController::LoggingData::localLogging()
{
if (!streamingHandler)
{
@@ -658,40 +748,37 @@ void RobotUnitPluginWidgetController::LoggingData::localLogging()
auto& data = streamingHandler->getDataBuffer();
if (data.empty())
{
- ARMARX_INFO_S << ::deactivateSpam()
- << "No streaming data received!";
+ ARMARX_INFO_S << ::deactivateSpam() << "No streaming data received!";
return;
}
- ARMARX_INFO_S << ::deactivateSpam(1)
- << "Writing " << data.size() << " timesteps";
+ ARMARX_INFO_S << ::deactivateSpam(1) << "Writing " << data.size() << " timesteps";
const auto& descr = streamingHandler->getDataDescription();
for (const auto& step : data)
{
for (const auto& [name, desc] : descr.entries)
{
- logStream << ';' << step.iterationId
- << ';' << step.timestampUSec
- << ';' << step.timesSinceLastIterationUSec;
+ logStream << ';' << step.iterationId << ';' << step.timestampUSec << ';'
+ << step.timesSinceLastIterationUSec;
using enum_t = RobotUnitDataStreaming::DataEntryType;
switch (desc.type)
{
- case enum_t::NodeTypeBool :
- logStream << ';' << step.bools .at(desc.index);
+ case enum_t::NodeTypeBool:
+ logStream << ';' << step.bools.at(desc.index);
break;
- case enum_t::NodeTypeByte :
- logStream << ';' << step.bytes .at(desc.index);
+ case enum_t::NodeTypeByte:
+ logStream << ';' << step.bytes.at(desc.index);
break;
- case enum_t::NodeTypeShort :
- logStream << ';' << step.shorts .at(desc.index);
+ case enum_t::NodeTypeShort:
+ logStream << ';' << step.shorts.at(desc.index);
break;
- case enum_t::NodeTypeInt :
- logStream << ';' << step.ints .at(desc.index);
+ case enum_t::NodeTypeInt:
+ logStream << ';' << step.ints.at(desc.index);
break;
- case enum_t::NodeTypeLong :
- logStream << ';' << step.longs .at(desc.index);
+ case enum_t::NodeTypeLong:
+ logStream << ';' << step.longs.at(desc.index);
break;
- case enum_t::NodeTypeFloat :
- logStream << ';' << step.floats .at(desc.index);
+ case enum_t::NodeTypeFloat:
+ logStream << ';' << step.floats.at(desc.index);
break;
case enum_t::NodeTypeDouble:
logStream << ';' << step.doubles.at(desc.index);
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
index 3d348859306decbd0273bef8fae10a256166dea6..e0c4cad82a66d591568103561c7d9b1e191321e5 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
@@ -22,35 +22,34 @@
#pragma once
-#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_RobotUnitPluginWidget.h>
-
-#include <mutex>
-#include <map>
-#include <string>
#include <chrono>
#include <fstream>
+#include <map>
+#include <mutex>
+#include <string>
-#include <QWidget>
+#include <QGroupBox>
+#include <QPointer>
#include <QPushButton>
+#include <QToolBar>
#include <QTreeWidget>
#include <QTreeWidgetItem>
-#include <QPointer>
-#include <QToolBar>
-#include <QGroupBox>
-
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <QWidget>
#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+#include <RobotAPI/gui-plugins/RobotUnitPlugin/ui_RobotUnitPluginWidget.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include "../QWidgets/ControlDevicesWidget.h"
#include "../QWidgets/NJointControllerClassesWidget.h"
#include "../QWidgets/NJointControllersWidget.h"
-#include "../QWidgets/ControlDevicesWidget.h"
#include "../QWidgets/SensorDevicesWidget.h"
namespace armarx
@@ -73,7 +72,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT RobotUnitPluginWidgetController:
+ class ARMARXCOMPONENT_IMPORT_EXPORT RobotUnitPluginWidgetController :
public ArmarXComponentWidgetControllerTemplate<RobotUnitPluginWidgetController>,
public RobotUnitListener
{
@@ -89,7 +88,8 @@ namespace armarx
void saveSettings(QSettings* settings) override;
/// @brief Returns the Widget name displayed in the ArmarXGui to create an instance of this class.
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "RobotControl.RobotUnitGUI";
}
@@ -111,21 +111,28 @@ namespace armarx
/// @brief Callback called when the config dialog is closed.
void configured() override;
- static QIcon GetWidgetIcon()
+ static QIcon
+ GetWidgetIcon()
{
return QIcon(":/robotunitplugin/icons/robot_unit_icon.svg");
}
- static QIcon GetWidgetCategoryIcon()
+
+ static QIcon
+ GetWidgetCategoryIcon()
{
return QIcon(":/robotunitplugin/icons/robot_unit_icon.svg");
}
+
QPointer<QWidget> getCustomTitlebarWidget(QWidget* parent) override;
// RobotUnitListener interface
public:
- void nJointControllerStatusChanged(const NJointControllerStatusSeq& status, const Ice::Current&) override;
- void controlDeviceStatusChanged(const ControlDeviceStatusSeq& status, const Ice::Current&) override;
- void sensorDeviceStatusChanged(const SensorDeviceStatusSeq& status, const Ice::Current&) override;
+ void nJointControllerStatusChanged(const NJointControllerStatusSeq& status,
+ const Ice::Current&) override;
+ void controlDeviceStatusChanged(const ControlDeviceStatusSeq& status,
+ const Ice::Current&) override;
+ void sensorDeviceStatusChanged(const SensorDeviceStatusSeq& status,
+ const Ice::Current&) override;
void nJointControllerClassAdded(const std::string& name, const Ice::Current&) override;
void nJointControllerCreated(const std::string& name, const Ice::Current&) override;
void nJointControllerDeleted(const std::string& name, const Ice::Current&) override;
@@ -160,7 +167,7 @@ namespace armarx
QPointer<SimpleConfigDialog> dialog;
std::string robotUnitProxyName;
- std::recursive_mutex robotUnitPrxMutex;
+ std::recursive_mutex robotUnitPrxMutex;
RobotUnitInterfacePrx robotUnitPrx;
std::string listenerTopicName;
@@ -179,19 +186,21 @@ namespace armarx
struct LoggingData
{
- QTreeWidgetItem* top;
- std::vector<QTreeWidgetItem*> allItems;
- SimpleRemoteReferenceCounterBasePtr handle;
+ QTreeWidgetItem* top;
+ std::vector<QTreeWidgetItem*> allItems;
+ SimpleRemoteReferenceCounterBasePtr handle;
RobotUnitDataStreamingReceiverPtr streamingHandler;
- std::ofstream logStream;
+ std::ofstream logStream;
static void updateCheckStateUpward(QTreeWidgetItem* item, bool recurseParents);
- static void updateCheckStateDownward(QTreeWidgetItem* item, Qt::CheckState state, bool recurseChildren);
+ static void updateCheckStateDownward(QTreeWidgetItem* item,
+ Qt::CheckState state,
+ bool recurseChildren);
void localLogging();
};
+
LoggingData loggingData;
int timerId = 0;
std::atomic_bool timerLastIterationRuWasRunning = false;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPluginGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPluginGuiPlugin.cpp
index 54753cf0d502bb7775dc583364eeb24834643188..3f907558c942a07965a0559ad055c3e49ba01d55 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPluginGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPluginGuiPlugin.cpp
@@ -28,5 +28,5 @@ using namespace armarx;
RobotUnitPluginGuiPlugin::RobotUnitPluginGuiPlugin()
{
- addWidget < RobotUnitPluginWidgetController > ();
+ addWidget<RobotUnitPluginWidgetController>();
}
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPluginGuiPlugin.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPluginGuiPlugin.h
index 4a251ad6022a35eb092125713355815bcf6f8b0e..2e03a54e80ffd732a05278d2988aca2cfbce1d76 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPluginGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPluginGuiPlugin.h
@@ -23,8 +23,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -35,8 +36,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT RobotUnitPluginGuiPlugin:
- public armarx::ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT RobotUnitPluginGuiPlugin : public armarx::ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -48,5 +48,4 @@ namespace armarx
*/
RobotUnitPluginGuiPlugin();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
index d13aaf8d1234f45d5fa17e0dbec4d066f3b4e32c..5d7f0f217c402543bd2e46a35097f9ad542a354a 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
@@ -23,47 +23,47 @@
*/
#include "RobotViewerGuiPlugin.h"
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <VirtualRobot/XML/RobotIO.h>
-
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/ArmarXManager.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <VirtualRobot/MathTools.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/CollisionDetection/CollisionModel.h>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Visualization/VisualizationFactory.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
-
// Qt headers
-#include <Qt>
-#include <QtGlobal>
-#include <QPushButton>
#include <QCheckBox>
#include <QClipboard>
+#include <QPushButton>
#include <QScrollBar>
+#include <Qt>
+#include <QtGlobal>
-#include <Inventor/SoDB.h>
-#include <Inventor/Qt/SoQt.h>
-#include <ArmarXGui/libraries/StructuralJson/JsonWriter.h>
#include <ArmarXCore/util/json/JSONObject.h>
+#include <ArmarXGui/libraries/StructuralJson/JsonWriter.h>
+
+#include <Inventor/Qt/SoQt.h>
+#include <Inventor/SoDB.h>
+
// System
#include <stdio.h>
-#include <string>
-#include <string.h>
#include <stdlib.h>
-#include <iostream>
-#include <cmath>
+#include <string.h>
+#include <cmath>
#include <filesystem>
+#include <iostream>
+#include <string>
#define TIMER_MS 33
#define ROBOTSTATE_NAME_DEFAULT "RobotStateComponent"
@@ -81,8 +81,7 @@ enum RobotStateOutputType
eRobotStateOutputTypeSize
};
-const QString ROBOT_STATE_OUTPUT_TYPE_NAMES[eRobotStateOutputTypeSize] =
-{
+const QString ROBOT_STATE_OUTPUT_TYPE_NAMES[eRobotStateOutputTypeSize] = {
"Joint Configuration",
"Framed Position (TCP)",
"Framed Orientation (TCP)",
@@ -94,12 +93,8 @@ RobotViewerGuiPlugin::RobotViewerGuiPlugin()
addWidget<RobotViewerWidgetController>();
}
-
-RobotViewerWidgetController::RobotViewerWidgetController()
- : rootVisu(nullptr)
- , robotVisu(nullptr)
- , timerSensor(nullptr)
- , debugLayerVisu(nullptr)
+RobotViewerWidgetController::RobotViewerWidgetController() :
+ rootVisu(nullptr), robotVisu(nullptr), timerSensor(nullptr), debugLayerVisu(nullptr)
{
ui.setupUi(getWidget());
getWidget()->setEnabled(false);
@@ -113,8 +108,8 @@ RobotViewerWidgetController::RobotViewerWidgetController()
outputTypes->setCurrentIndex(0);
}
-
-void RobotViewerWidgetController::onInitComponent()
+void
+RobotViewerWidgetController::onInitComponent()
{
bool createDebugDrawer = true;
verbose = true;
@@ -133,7 +128,8 @@ void RobotViewerWidgetController::onInitComponent()
{
std::string debugDrawerComponentName = "RobotViewerGUIDebugDrawer_" + getName();
ARMARX_INFO << "Creating component " << debugDrawerComponentName;
- debugDrawer = Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
+ debugDrawer =
+ Component::create<DebugDrawerComponent>(getIceProperties(), debugDrawerComponentName);
if (mutex3D)
{
@@ -160,7 +156,8 @@ void RobotViewerWidgetController::onInitComponent()
showRoot(true);
}
-void RobotViewerWidgetController::onConnectComponent()
+void
+RobotViewerWidgetController::onConnectComponent()
{
robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
usingTopic(robotStateComponentPrx->getRobotStateTopicName());
@@ -193,9 +190,10 @@ void RobotViewerWidgetController::onConnectComponent()
auto pathsString = project.getDataDir();
ARMARX_VERBOSE << "Data paths of ArmarX package " << projectName << ": " << pathsString;
Ice::StringSeq projectIncludePaths = Split(pathsString, ";,", true, true);
- ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
-
+ ARMARX_VERBOSE << "Result: Data paths of ArmarX package " << projectName << ": "
+ << projectIncludePaths;
+ includePaths.insert(
+ includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
rfile = robotStateComponentPrx->getRobotFilename();
@@ -271,7 +269,8 @@ void RobotViewerWidgetController::onConnectComponent()
enableMainWidgetAsync(true);
}
-void RobotViewerWidgetController::onDisconnectComponent()
+void
+RobotViewerWidgetController::onDisconnectComponent()
{
ARMARX_INFO << "Disconnecting component";
@@ -301,8 +300,8 @@ void RobotViewerWidgetController::onDisconnectComponent()
ARMARX_INFO << "Disconnecting component: finished";
}
-
-void RobotViewerWidgetController::onExitComponent()
+void
+RobotViewerWidgetController::onExitComponent()
{
enableMainWidgetAsync(false);
@@ -341,43 +340,49 @@ void RobotViewerWidgetController::onExitComponent()
*/
}
-QPointer<QDialog> RobotViewerWidgetController::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+RobotViewerWidgetController::getConfigDialog(QWidget* parent)
{
if (!dialog)
{
dialog = new SimpleConfigDialog(parent);
- dialog->addProxyFinder<RobotStateComponentInterfacePrx>({"RobotStateComponent", "", "RobotState*"});
+ dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
+ {"RobotStateComponent", "", "RobotState*"});
}
return qobject_cast<SimpleConfigDialog*>(dialog);
}
-
-
-void RobotViewerWidgetController::configured()
+void
+RobotViewerWidgetController::configured()
{
robotStateComponentName = dialog->getProxyName("RobotStateComponent");
}
-
-void RobotViewerWidgetController::loadSettings(QSettings* settings)
+void
+RobotViewerWidgetController::loadSettings(QSettings* settings)
{
- robotStateComponentName = settings->value("RobotStateComponent", QString::fromStdString(ROBOTSTATE_NAME_DEFAULT)).toString().toStdString();
+ robotStateComponentName =
+ settings->value("RobotStateComponent", QString::fromStdString(ROBOTSTATE_NAME_DEFAULT))
+ .toString()
+ .toStdString();
- bool showRob = settings->value("showRobot", QVariant(true)).toBool();
- bool fullMod = settings->value("fullModel", QVariant(true)).toBool();
+ bool showRob = settings->value("showRobot", QVariant(true)).toBool();
+ bool fullMod = settings->value("fullModel", QVariant(true)).toBool();
ui.cbRobot->setChecked(showRob);
ui.radioButtonFull->setChecked(fullMod);
ui.radioButtonCol->setChecked(!fullMod);
}
-void RobotViewerWidgetController::saveSettings(QSettings* settings)
+void
+RobotViewerWidgetController::saveSettings(QSettings* settings)
{
settings->setValue("RobotStateComponent", QString::fromStdString(robotStateComponentName));
settings->setValue("showRobot", ui.cbRobot->isChecked());
settings->setValue("fullModel", ui.radioButtonFull->isChecked());
}
-void RobotViewerWidgetController::setRobotVisu(bool colModel)
+void
+RobotViewerWidgetController::setRobotVisu(bool colModel)
{
robotVisu->removeAllChildren();
@@ -407,7 +412,8 @@ void RobotViewerWidgetController::setRobotVisu(bool colModel)
}
}
-void RobotViewerWidgetController::showVisuLayers(bool show)
+void
+RobotViewerWidgetController::showVisuLayers(bool show)
{
if (debugDrawer)
{
@@ -422,7 +428,8 @@ void RobotViewerWidgetController::showVisuLayers(bool show)
}
}
-void RobotViewerWidgetController::showRoot(bool show)
+void
+RobotViewerWidgetController::showRoot(bool show)
{
if (!debugDrawer)
{
@@ -443,7 +450,8 @@ void RobotViewerWidgetController::showRoot(bool show)
}
}
-void RobotViewerWidgetController::showRobot(bool show)
+void
+RobotViewerWidgetController::showRobot(bool show)
{
if (!robotVisu)
{
@@ -461,12 +469,15 @@ void RobotViewerWidgetController::showRobot(bool show)
rootVisu->removeChild(robotVisu);
}
}
-SoNode* RobotViewerWidgetController::getScene()
+
+SoNode*
+RobotViewerWidgetController::getScene()
{
return rootVisu;
}
-void RobotViewerWidgetController::timerCB(void* data, SoSensor* sensor)
+void
+RobotViewerWidgetController::timerCB(void* data, SoSensor* sensor)
{
RobotViewerWidgetController* controller = static_cast<RobotViewerWidgetController*>(data);
@@ -478,30 +489,53 @@ void RobotViewerWidgetController::timerCB(void* data, SoSensor* sensor)
controller->updateRobotVisu();
}
-
-void RobotViewerWidgetController::connectSlots()
+void
+RobotViewerWidgetController::connectSlots()
{
// Robot viewer GUI (top part)
connect(this, SIGNAL(robotStatusUpdated()), this, SLOT(updateRobotVisu()));
- connect(ui.cbDebugLayer, SIGNAL(toggled(bool)), this, SLOT(showVisuLayers(bool)), Qt::QueuedConnection);
+ connect(ui.cbDebugLayer,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(showVisuLayers(bool)),
+ Qt::QueuedConnection);
connect(ui.cbRoot, SIGNAL(toggled(bool)), this, SLOT(showRoot(bool)), Qt::QueuedConnection);
connect(ui.cbRobot, SIGNAL(toggled(bool)), this, SLOT(showRobot(bool)), Qt::QueuedConnection);
- connect(ui.radioButtonCol, SIGNAL(toggled(bool)), this, SLOT(colModel(bool)), Qt::QueuedConnection);
- connect(ui.radioButtonFull, SIGNAL(toggled(bool)), this, SLOT(colModel(bool)), Qt::QueuedConnection);
- connect(ui.horizontalSliderCollisionModelInflation, SIGNAL(sliderMoved(int)), this, SLOT(inflateCollisionModel(int)), Qt::QueuedConnection);
+ connect(
+ ui.radioButtonCol, SIGNAL(toggled(bool)), this, SLOT(colModel(bool)), Qt::QueuedConnection);
+ connect(ui.radioButtonFull,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(colModel(bool)),
+ Qt::QueuedConnection);
+ connect(ui.horizontalSliderCollisionModelInflation,
+ SIGNAL(sliderMoved(int)),
+ this,
+ SLOT(inflateCollisionModel(int)),
+ Qt::QueuedConnection);
// Copy state GUI (bottom part)
- connect(ui.kinematicChainComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStateSettings(int)));
- connect(ui.frameComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStateSettings(int)));
- connect(ui.outputTypeComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStateSettings(int)));
+ connect(ui.kinematicChainComboBox,
+ SIGNAL(currentIndexChanged(int)),
+ this,
+ SLOT(updateStateSettings(int)));
+ connect(
+ ui.frameComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStateSettings(int)));
+ connect(ui.outputTypeComboBox,
+ SIGNAL(currentIndexChanged(int)),
+ this,
+ SLOT(updateStateSettings(int)));
connect(ui.copyToClipboardButton, SIGNAL(clicked()), this, SLOT(copyToClipboard()));
- connect(this, SIGNAL(configurationChanged()), this, SLOT(onConfigurationChanged()), Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(configurationChanged()),
+ this,
+ SLOT(onConfigurationChanged()),
+ Qt::QueuedConnection);
}
-
-
-VirtualRobot::RobotPtr RobotViewerWidgetController::loadRobotFile(std::string fileName)
+VirtualRobot::RobotPtr
+RobotViewerWidgetController::loadRobotFile(std::string fileName)
{
VirtualRobot::RobotPtr robot;
@@ -520,8 +554,8 @@ VirtualRobot::RobotPtr RobotViewerWidgetController::loadRobotFile(std::string fi
return robot;
}
-
-void RobotViewerWidgetController::colModel(bool c)
+void
+RobotViewerWidgetController::colModel(bool c)
{
bool colModel = false;
@@ -534,12 +568,14 @@ void RobotViewerWidgetController::colModel(bool c)
setRobotVisu(colModel);
}
-void RobotViewerWidgetController::updateStateSettings(int)
+void
+RobotViewerWidgetController::updateStateSettings(int)
{
updateState();
}
-void RobotViewerWidgetController::copyToClipboard()
+void
+RobotViewerWidgetController::copyToClipboard()
{
QClipboard* clipboard = QApplication::clipboard();
if (clipboard)
@@ -553,8 +589,9 @@ void RobotViewerWidgetController::copyToClipboard()
}
}
-static std::string writeJointConfigurationToJson(VirtualRobot::Robot& robot,
- VirtualRobot::RobotNodeSetPtr const& robotNodeSet)
+static std::string
+writeJointConfigurationToJson(VirtualRobot::Robot& robot,
+ VirtualRobot::RobotNodeSetPtr const& robotNodeSet)
{
int indenting = 2; // Magic value for correct indenting
JsonWriter writer(indenting);
@@ -585,13 +622,18 @@ static std::string writeJointConfigurationToJson(VirtualRobot::Robot& robot,
}
template <typename FrameType>
-static std::string writeFramedTCP(VirtualRobot::RobotPtr const& robot, VirtualRobot::RobotNodeSetPtr const& nodeSet, std::string const& frameName, const std::string& tcpName)
+static std::string
+writeFramedTCP(VirtualRobot::RobotPtr const& robot,
+ VirtualRobot::RobotNodeSetPtr const& nodeSet,
+ std::string const& frameName,
+ const std::string& tcpName)
{
if (nodeSet)
{
auto tcp = tcpName.empty() ? nodeSet->getTCP() : robot->getRobotNode(tcpName);
Eigen::Matrix4f tcpMatrix = tcp->getPoseInRootFrame();
- IceInternal::Handle<FrameType> position = new FrameType(tcpMatrix, robot->getRootNode()->getName(), robot->getName());
+ IceInternal::Handle<FrameType> position =
+ new FrameType(tcpMatrix, robot->getRootNode()->getName(), robot->getName());
position->changeFrame(robot, frameName);
JSONObjectPtr object = new JSONObject;
@@ -604,7 +646,8 @@ static std::string writeFramedTCP(VirtualRobot::RobotPtr const& robot, VirtualRo
}
}
-void RobotViewerWidgetController::updateState()
+void
+RobotViewerWidgetController::updateState()
{
if (!robot)
{
@@ -633,7 +676,8 @@ void RobotViewerWidgetController::updateState()
// Set the locale so that the floats get converted correctly
const char* oldLocale = std::setlocale(LC_ALL, "en_US.UTF-8");
- std::string tcpName = ui.tcpComboBox->currentIndex() == 0 ? "" : ui.tcpComboBox->currentText().toStdString();
+ std::string tcpName =
+ ui.tcpComboBox->currentIndex() == 0 ? "" : ui.tcpComboBox->currentText().toStdString();
std::string output;
switch (outputType)
{
@@ -675,7 +719,8 @@ void RobotViewerWidgetController::updateState()
std::setlocale(LC_ALL, oldLocale);
}
-void RobotViewerWidgetController::onConfigurationChanged()
+void
+RobotViewerWidgetController::onConfigurationChanged()
{
if (ui.autoUpdateCheckBox->isChecked())
{
@@ -683,7 +728,8 @@ void RobotViewerWidgetController::onConfigurationChanged()
}
}
-void RobotViewerWidgetController::updateRobotVisu()
+void
+RobotViewerWidgetController::updateRobotVisu()
{
std::unique_lock lock(*mutex3D);
@@ -722,7 +768,8 @@ void RobotViewerWidgetController::updateRobotVisu()
emit configurationChanged();
}
-void RobotViewerWidgetController::inflateCollisionModel(int inflationValueMM)
+void
+RobotViewerWidgetController::inflateCollisionModel(int inflationValueMM)
{
std::unique_lock lock(*mutex3D);
@@ -734,7 +781,8 @@ void RobotViewerWidgetController::inflateCollisionModel(int inflationValueMM)
setRobotVisu(true);
}
-void RobotViewerWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
+void
+RobotViewerWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
{
//ARMARX_IMPORTANT << "set mutex3d :" << mutex3D.get();
this->mutex3D = mutex3D;
@@ -745,7 +793,11 @@ void RobotViewerWidgetController::setMutex3D(RecursiveMutexPtr const& mutex3D)
}
}
-void armarx::RobotViewerWidgetController::reportGlobalRobotRootPose(const FramedPoseBasePtr& pose, Ice::Long timestamp, bool poseChanged, const Ice::Current&)
+void
+armarx::RobotViewerWidgetController::reportGlobalRobotRootPose(const FramedPoseBasePtr& pose,
+ Ice::Long timestamp,
+ bool poseChanged,
+ const Ice::Current&)
{
std::unique_lock lock(*mutex3D);
if (!robotStateComponentPrx || !robot)
@@ -760,7 +812,11 @@ void armarx::RobotViewerWidgetController::reportGlobalRobotRootPose(const Framed
}
}
-void armarx::RobotViewerWidgetController::reportJointValues(const NameValueMap& jointAngles, Ice::Long, bool aValueChanged, const Ice::Current&)
+void
+armarx::RobotViewerWidgetController::reportJointValues(const NameValueMap& jointAngles,
+ Ice::Long,
+ bool aValueChanged,
+ const Ice::Current&)
{
std::unique_lock lock(*mutex3D);
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
index ebb76786af86a2dff64ea0880547a376a5ae1d06..15540e1aaf8961f653165d4e68fc60da7adb2374 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.h
@@ -24,15 +24,15 @@
#pragma once
/* ArmarX headers */
-#include <RobotAPI/gui-plugins/RobotViewerPlugin/ui_RobotViewerGuiPlugin.h>
-#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <ArmarXCore/core/Component.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+#include <RobotAPI/gui-plugins/RobotViewerPlugin/ui_RobotViewerGuiPlugin.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
/* Qt headers */
#include <QMainWindow>
@@ -51,8 +51,7 @@ namespace armarx
Further, DebugDrawer methods are available.
\see RobotViewerWidget
*/
- class RobotViewerGuiPlugin :
- public ArmarXGuiPlugin
+ class RobotViewerGuiPlugin : public ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -60,7 +59,8 @@ namespace armarx
public:
RobotViewerGuiPlugin();
- QString getPluginName() override
+ QString
+ getPluginName() override
{
return "RobotViewerGuiPlugin";
}
@@ -84,9 +84,11 @@ namespace armarx
{
Q_OBJECT
public:
-
RobotViewerWidgetController();
- ~RobotViewerWidgetController() override {}
+
+ ~RobotViewerWidgetController() override
+ {
+ }
// inherited from Component
void onInitComponent() override;
@@ -95,7 +97,8 @@ namespace armarx
void onExitComponent() override;
// inherited from ArmarXWidget
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "Visualization.RobotViewerGUI";
}
@@ -158,7 +161,6 @@ namespace armarx
void onConfigurationChanged();
private:
-
// init stuff
VirtualRobot::RobotPtr loadRobotFile(std::string fileName);
@@ -169,15 +171,23 @@ namespace armarx
// ArmarXWidgetController interface
public:
- static QIcon GetWidgetIcon()
+ static QIcon
+ GetWidgetIcon()
{
return QIcon(":icons/armarx-gui.png");
}
// RobotStateListenerInterface interface
public:
- void reportGlobalRobotRootPose(const FramedPoseBasePtr& pose, Ice::Long timestamp, bool poseChanged, const Ice::Current&) override;
- void reportJointValues(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current&) override;
+ void reportGlobalRobotRootPose(const FramedPoseBasePtr& pose,
+ Ice::Long timestamp,
+ bool poseChanged,
+ const Ice::Current&) override;
+ void reportJointValues(const NameValueMap& jointAngles,
+ Ice::Long timestamp,
+ bool aValueChanged,
+ const Ice::Current&) override;
};
+
using RobotViewerGuiPluginPtr = std::shared_ptr<RobotViewerWidgetController>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
index be74b8d21b39b76d9847876d2b806e9c43c74f75..281d6a79f9bf0f3ba6071414dae375693944b384 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.cpp
@@ -24,28 +24,27 @@
//VirtualRobot
-#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/LinkedCoordinate.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
// C++ includes
#include <sstream>
// Qt includes
-#include <QScrollBar>
-#include <QLineEdit>
#include <QDialogButtonBox>
+#include <QLineEdit>
+#include <QScrollBar>
using namespace armarx;
using namespace VirtualRobot;
-
-TCPMover::TCPMover() :
- robotRequested(false)
+TCPMover::TCPMover() : robotRequested(false)
{
@@ -72,27 +71,27 @@ TCPMover::TCPMover() :
connect(ui.btnRight, SIGNAL(clicked()), this, SLOT(moveLeft()));
connect(ui.btnStop, SIGNAL(clicked()), this, SLOT(stopMoving()));
connect(ui.btnResetJoinAngles, SIGNAL(clicked()), this, SLOT(reset()));
-
}
TCPMover::~TCPMover()
{
-
}
-
-
-void armarx::TCPMover::loadSettings(QSettings* settings)
+void
+armarx::TCPMover::loadSettings(QSettings* settings)
{
- tcpMoverUnitName = settings->value("TCPControlUnitName", "TCPControlUnit").toString().toStdString();
+ tcpMoverUnitName =
+ settings->value("TCPControlUnitName", "TCPControlUnit").toString().toStdString();
}
-void armarx::TCPMover::saveSettings(QSettings* settings)
+void
+armarx::TCPMover::saveSettings(QSettings* settings)
{
settings->setValue("TCPControlUnitName", tcpMoverUnitName.c_str());
}
-QPointer<QDialog> TCPMover::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+TCPMover::getConfigDialog(QWidget* parent)
{
if (!configDialog)
{
@@ -102,15 +101,16 @@ QPointer<QDialog> TCPMover::getConfigDialog(QWidget* parent)
return qobject_cast<TCPMoverConfigDialog*>(configDialog);
}
-void TCPMover::configured()
+void
+TCPMover::configured()
{
- tcpMoverUnitName = qobject_cast<TCPMoverConfigDialog*>(getConfigDialog())->proxyFinder->getSelectedProxyName().toStdString();
+ tcpMoverUnitName = qobject_cast<TCPMoverConfigDialog*>(getConfigDialog())
+ ->proxyFinder->getSelectedProxyName()
+ .toStdString();
}
-
-
-
-void armarx::TCPMover::onInitComponent()
+void
+armarx::TCPMover::onInitComponent()
{
// kinematicUnitFile = getProperty<std::string>("RobotFileName", KINEMATIC_UNIT_FILE_DEFAULT).getValueOrDefault();
// kinematicUnitName = getProperty<std::string>("RobotNodeSetName",KINEMATIC_UNIT_NAME_DEFAULT).getValueOrDefault();
@@ -127,10 +127,10 @@ void armarx::TCPMover::onInitComponent()
// usingProxy(kinematicUnitName);
usingProxy(tcpMoverUnitName);
usingProxy("RobotStateComponent");
-
}
-void armarx::TCPMover::onConnectComponent()
+void
+armarx::TCPMover::onConnectComponent()
{
tcpMoverUnitPrx = getProxy<TCPControlUnitInterfacePrx>(tcpMoverUnitName);
@@ -175,109 +175,123 @@ void armarx::TCPMover::onConnectComponent()
ui.gridLayout->setEnabled(false);
}
-void TCPMover::onExitComponent()
+void
+TCPMover::onExitComponent()
{
// if(robotRequested)
// kinematicUnitPrx->release();
}
-void TCPMover::moveUp()
+void
+TCPMover::moveUp()
{
// moveRelative(-1,0,0);
tcpData[selectedTCP][0]++;
execMove();
}
-void TCPMover::moveDown()
+void
+TCPMover::moveDown()
{
// moveRelative(1,0,0);
tcpData[selectedTCP][0]--;
execMove();
}
-void TCPMover::moveZUp()
+void
+TCPMover::moveZUp()
{
tcpData[selectedTCP][2]++;
execMove();
}
-void TCPMover::moveZDown()
+void
+TCPMover::moveZDown()
{
tcpData[selectedTCP][2]--;
execMove();
}
-void TCPMover::moveLeft()
+void
+TCPMover::moveLeft()
{
// moveRelative(0,1,0);
- tcpData[selectedTCP][1] ++;
+ tcpData[selectedTCP][1]++;
execMove();
-
}
-void TCPMover::moveRight()
+void
+TCPMover::moveRight()
{
- tcpData[selectedTCP][1] --;
+ tcpData[selectedTCP][1]--;
execMove();
// moveRelative(0,-1,0);
}
-void TCPMover::increaseAlpha()
+void
+TCPMover::increaseAlpha()
{
tcpData[selectedTCP].at(3) += 2.f;
execMove();
}
-void TCPMover::decreaseAlpha()
+void
+TCPMover::decreaseAlpha()
{
tcpData[selectedTCP].at(3) -= 2.f;
execMove();
}
-void TCPMover::increaseBeta()
+void
+TCPMover::increaseBeta()
{
tcpData[selectedTCP].at(4) += 2.f;
execMove();
}
-void TCPMover::decreaseBeta()
+void
+TCPMover::decreaseBeta()
{
tcpData[selectedTCP].at(4) -= 2.f;
execMove();
}
-void TCPMover::increaseGamma()
+void
+TCPMover::increaseGamma()
{
tcpData[selectedTCP].at(5) += 2.f;
execMove();
}
-void TCPMover::decreaseGamma()
+void
+TCPMover::decreaseGamma()
{
tcpData[selectedTCP].at(5) -= 2.f;
execMove();
}
-void TCPMover::stopMoving()
+void
+TCPMover::stopMoving()
{
tcpData[selectedTCP][0] = tcpData[selectedTCP][1] = tcpData[selectedTCP][2] = 0.0f;
execMove();
}
-void TCPMover::reset()
+void
+TCPMover::reset()
{
tcpData[selectedTCP][0] = tcpData[selectedTCP][1] = tcpData[selectedTCP][2] = 0.0f;
tcpData[selectedTCP].at(3) = tcpData[selectedTCP].at(4) = tcpData[selectedTCP].at(5) = 0.0f;
execMove();
tcpMoverUnitPrx->release();
// tcpMoverUnitPrx->resetArmToHomePosition(eLeftHand);
-
}
-void TCPMover::robotControl(bool request)
+void
+TCPMover::robotControl(bool request)
{
tcpMoverUnitPrx->request();
// try{
@@ -292,15 +306,16 @@ void TCPMover::robotControl(bool request)
// ui.BtnRequestControl->setChecked(true);
// }
// robotRequested = ui.BtnRequestControl->isChecked();
-
}
-void TCPMover::selectHand(int index)
+void
+TCPMover::selectHand(int index)
{
selectedTCP = ui.cbselectedTCP->itemText(index).toStdString();
}
-void TCPMover::execMove()
+void
+TCPMover::execMove()
{
ARMARX_INFO << "Setting new velos and orientation";
ui.lblXValue->setText(QString::number(tcpData[selectedTCP][0]));
@@ -316,7 +331,8 @@ void TCPMover::execMove()
const auto agentName = robotPrx->getName();
FramedDirectionPtr tcpVel = new FramedDirection(vec, refFrame, agentName);
Eigen::Vector3f vecOri;
- vecOri << tcpData[selectedTCP].at(3) / 180.f * 3.145f, tcpData[selectedTCP].at(4) / 180.f * 3.145f, tcpData[selectedTCP].at(5) / 180.f * 3.145f;
+ vecOri << tcpData[selectedTCP].at(3) / 180.f * 3.145f,
+ tcpData[selectedTCP].at(4) / 180.f * 3.145f, tcpData[selectedTCP].at(5) / 180.f * 3.145f;
vecOri *= ui.sbFactor->value();
FramedDirectionPtr tcpOri = new FramedDirection(vecOri, refFrame, agentName);
@@ -330,13 +346,15 @@ void TCPMover::execMove()
tcpOri = NULL;
}
- tcpMoverUnitPrx->begin_setTCPVelocity(selectedTCP, ui.edtTCPName->text().toStdString(), tcpVel, tcpOri);
+ tcpMoverUnitPrx->begin_setTCPVelocity(
+ selectedTCP, ui.edtTCPName->text().toStdString(), tcpVel, tcpOri);
// tcpMoverUnitPrx->setCartesianTCPVelocity(selectedTCP, handData[selectedTCP][0], handData[selectedTCP][1], handData[selectedTCP][2], ui.sbFactor->value());
// tcpMoverUnitPrx->setTCPOrientation(selectedTCP, handData[selectedTCP].at(3)/180.f*3.145f, handData[selectedTCP].at(4)/180.f*3.145f, handData[selectedTCP].at(5)/180.f*3.145f);
}
-void TCPMover::moveRelative(float x, float y, float z)
+void
+TCPMover::moveRelative(float x, float y, float z)
{
// RobotNodePtr tcpNode = tcpNodeSet->getTCP();
// // calculate cartesian error
@@ -351,8 +369,6 @@ void TCPMover::moveRelative(float x, float y, float z)
//// errorCartVec.segment(0,3) = newPosRelative.block(0,3,3,1);
-
-
// Eigen::MatrixXf Ji = ik->getPseudoInverseJacobianMatrix(tcpNode, IKSolver::Position);
// // calculat joint error
// Eigen::VectorXf errorJoint(tcpNodeSet->getSize());
@@ -373,7 +389,6 @@ void TCPMover::moveRelative(float x, float y, float z)
// kinematicUnitPrx->setJointVelocities(targetAnglesMap);
}
-
/*void TCPMoverConfigDialog::setupUI(QWidget *parent)
{
this->setWindowTitle("Enter name of TCPMoverUnit");
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
index bcbfc2e079c91bd5db1f114bfa4902b3cd99b954..bf1a04d99cd008ae1e6eff4f14e69f594db318fe 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMover.h
@@ -23,24 +23,25 @@
#pragma once
/** ArmarX headers **/
-#include "TCPMoverConfigDialog.h"
#include <RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ui_TCPMover.h>
+#include "TCPMoverConfigDialog.h"
+
// ArmarX includes
-#include <RobotAPI/interface/units/TCPMoverUnitInterface.h>
#include <RobotAPI/interface/units/TCPControlUnit.h>
+#include <RobotAPI/interface/units/TCPMoverUnitInterface.h>
/** VirtualRobot headers **/
#include <VirtualRobot/VirtualRobot.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
-
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
// Qt includes
#include <QDialog>
-#define KINEMATIC_UNIT_FILE_DEFAULT armarx::ArmarXDataPath::getHomePath()+std::string("Armar4/ARMAR_IV_simox.xml")
+#define KINEMATIC_UNIT_FILE_DEFAULT \
+ armarx::ArmarXDataPath::getHomePath() + std::string("Armar4/ARMAR_IV_simox.xml")
#define KINEMATIC_UNIT_NAME_DEFAULT "RobotKinematicUnit"
@@ -74,8 +75,7 @@ namespace armarx
*
* \image html TCPMoverGUI.png
*/
- class TCPMover :
- public ArmarXComponentWidgetControllerTemplate<TCPMover>
+ class TCPMover : public ArmarXComponentWidgetControllerTemplate<TCPMover>
{
Q_OBJECT
@@ -85,18 +85,25 @@ namespace armarx
//inherited from ArmarXMdiWidget
void loadSettings(QSettings* settings) override;
void saveSettings(QSettings* settings) override;
- static QString GetWidgetName()
+
+ static QString
+ GetWidgetName()
{
return "RobotControl.TCPMover";
}
- static QIcon GetWidgetIcon()
+
+ static QIcon
+ GetWidgetIcon()
{
return QIcon("://icons/games.ico");
}
- static QIcon GetWidgetCategoryIcon()
+
+ static QIcon
+ GetWidgetCategoryIcon()
{
return QIcon("://icons/games.ico");
}
+
QPointer<QDialog> getConfigDialog(QWidget* parent = 0) override;
void configured() override;
@@ -129,7 +136,7 @@ namespace armarx
void execMove();
void moveRelative(float x, float y, float z);
- std::map<std::string, std::vector<float> > tcpData;
+ std::map<std::string, std::vector<float>> tcpData;
std::string selectedTCP;
std::string refFrame;
// float velocities[2][3];
@@ -149,4 +156,4 @@ namespace armarx
SharedRobotInterfacePrx robotPrx;
QPointer<TCPMoverConfigDialog> configDialog;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp
index 2f906a864348c1ea87b97f5b79a78d7a984c5252..33d8788a83ce113d7c0e4ee624d3609c5f5dd783 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.cpp
@@ -21,16 +21,14 @@
*/
#include "TCPMoverConfigDialog.h"
-#include <RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ui_TCPMoverConfigDialog.h>
#include <IceUtil/UUID.h>
+#include <RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ui_TCPMoverConfigDialog.h>
#include <RobotAPI/interface/units/TCPControlUnit.h>
armarx::TCPMoverConfigDialog::TCPMoverConfigDialog(QWidget* parent) :
- QDialog(parent),
- ui(new Ui::TCPMoverConfigDialog),
- uuid(IceUtil::generateUUID())
+ QDialog(parent), ui(new Ui::TCPMoverConfigDialog), uuid(IceUtil::generateUUID())
{
ui->setupUi(this);
setName(getDefaultName());
@@ -46,13 +44,13 @@ armarx::TCPMoverConfigDialog::~TCPMoverConfigDialog()
delete ui;
}
-
-
-void armarx::TCPMoverConfigDialog::onInitComponent()
+void
+armarx::TCPMoverConfigDialog::onInitComponent()
{
proxyFinder->setIceManager(getIceManager());
}
-void armarx::TCPMoverConfigDialog::onConnectComponent()
+void
+armarx::TCPMoverConfigDialog::onConnectComponent()
{
}
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h
index 010545fff9e39d192624d3f5fdb391aa89541c1c..c47a7a47fbf70becbe9a6ff7737763bc59d123bb 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXTCPMover/TCPMoverConfigDialog.h
@@ -36,9 +36,7 @@ namespace Ui
namespace armarx
{
- class TCPMoverConfigDialog :
- public QDialog,
- virtual public ManagedIceObject
+ class TCPMoverConfigDialog : public QDialog, virtual public ManagedIceObject
{
Q_OBJECT
@@ -48,10 +46,12 @@ namespace armarx
protected:
// ManagedIceObject interface
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "TCPMoverConfigDialog" + uuid;
}
+
void onInitComponent() override;
void onConnectComponent() override;
@@ -62,7 +62,5 @@ namespace armarx
std::string uuid;
friend class TCPMover;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.cpp
index 390da0d077fc9afadd7c9fc6f8ebd1d8d5827186..276c246ea15c6f926cecb3a6b7b557a528da9c4d 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.cpp
@@ -21,6 +21,7 @@
*/
#include "SensorActorWidgetsPlugin.h"
+
#include "ArmarXTCPMover/TCPMover.h"
namespace armarx
@@ -30,4 +31,4 @@ namespace armarx
{
addWidget<TCPMover>();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.h b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.h
index 8dc78bbe2bd49d4dd582db739466eaaeac35ddd9..db1615fe7c53b03f3c4fd0921f0a9d25c75b81b1 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.h
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/SensorActorWidgetsPlugin.h
@@ -22,10 +22,9 @@
#pragma once
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-
#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -35,8 +34,7 @@ namespace armarx
* @see ArmarXPlotter
* @see TCPMover
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT SensorActorPlugin :
- public ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT SensorActorPlugin : public ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -45,5 +43,4 @@ namespace armarx
SensorActorPlugin();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.cpp
index 05cd7a5cc08224561af8df64cf6be2347adb3e37..d1356c3e2320e728c90a1a6ea4fe98fee88ddcd0 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/ColorPalettes.cpp
@@ -9,6 +9,7 @@ namespace armarx::gui_color_palette
errorPalette.setColor(QPalette::Base, Qt::red);
return errorPalette;
}
+
QPalette
getNormalPalette()
{
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
index 52110b494a2bfc38a361d8354eb18efccc760617..15fc3236a80e5a13ac2aaf874fcc7078feed1463 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetController.cpp
@@ -6,7 +6,6 @@
#include "visitors/AronTreeWidgetConverter.h"
#include "visitors/AronTreeWidgetSetter.h"
-
namespace armarx
{
AronTreeWidgetController::AronTreeWidgetController(QTreeWidget* tree,
@@ -76,6 +75,7 @@ namespace armarx
aron::data::visit(v, data);
}
}
+
void
AronTreeWidgetController::ShowContextMenu(const QPoint& pos)
{
@@ -93,6 +93,7 @@ namespace armarx
tree->blockSignals(false);
}
+
void
AronTreeWidgetController::onTreeWidgetItemDoubleClicked(QTreeWidgetItem* item, int column)
{
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
index 29d895f7b0f9666ebf42acd2bf3046ac90d7f1db..b4544d2de6302990d608cecbdf759bab7a485208 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/AronTreeWidgetItem.cpp
@@ -86,7 +86,6 @@ namespace armarx
}
}
-
void
AronTreeWidgetItem::checkKeyValidityOfChildren()
{
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/Data.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/Data.h
index 3fea187d34f34cfcffa79a12ead1af3e5d97a55d..efdc78d8f6477e97b5e459d86c910d6bdfb5bf6f 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/Data.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/Data.h
@@ -10,4 +10,4 @@ namespace armarx::aron_tree_widget::constantes
const std::string ITEM_EMPTY_MESSAGE = "(double click to edit)";
const std::string NEW_ITEM_DEFAULT_MESSAGE = "(Please set via main GUI (not in modal))";
-}
+} // namespace armarx::aron_tree_widget::constantes
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.h
index c5d991ad2861aef5c32fc82c033c9090070f13b5..447f99ac78c469af244577bc7141176893eff5fb 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/ListDictHelper.h
@@ -1,6 +1,5 @@
#include <QString>
-
namespace armarx::misc
{
// helper that generates a string on how many items are available
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.cpp
index 8fbd9c5ba1abdb5e2d2175b1ff545579034c7376..e5e92afb816e20644f0855ceb266a1a37aa5c719 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.cpp
@@ -2,19 +2,21 @@
namespace armarx
{
- AronTreeWidgetModal::AronTreeWidgetModal(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
- QDialog(parent),
- item(item),
- label(label),
- parent(parent)
+ AronTreeWidgetModal::AronTreeWidgetModal(const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent) :
+ QDialog(parent), item(item), label(label), parent(parent)
{
init.aronType = item->aronType;
- init.setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME, item->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME));
- init.setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE, item->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
- init.setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE, item->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE));
+ init.setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME,
+ item->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME));
+ init.setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE,
+ item->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
+ init.setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE,
+ item->text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE));
for (int i = 0; i < item->childCount(); ++i)
{
init.addChild(item->child(i)->clone());
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.h
index cda9f7e728df6da713fb1e09001bc37e8502b9e6..058ef8efb8a5db376bbc20a618f3c1a6bf1303c9 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/AronTreeWidgetModal.h
@@ -1,34 +1,41 @@
#pragma once
#include <stack>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include "../AronTreeWidgetItem.h"
-#include "../Data.h"
+#include <QDialog>
+#include <QTreeWidget>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <QDialog>
-#include <QTreeWidget>
+#include "../AronTreeWidgetItem.h"
+#include "../Data.h"
namespace armarx
{
- class AronTreeWidgetModal :
- public QDialog
+ class AronTreeWidgetModal : public QDialog
{
Q_OBJECT
public:
- AronTreeWidgetModal(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent);
+ AronTreeWidgetModal(const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent);
protected slots:
- virtual void reset()
+
+ virtual void
+ reset()
{
item->aronType = init.aronType;
- item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME, init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME));
- item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE, init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
- item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE, init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE));
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME,
+ init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_NAME));
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE,
+ init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE,
+ init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_TYPE));
for (int i = 0; i < item->childCount(); ++i)
{
item->removeChild(item->child(i));
@@ -38,13 +45,14 @@ namespace armarx
item->addChild(init.child(i)->clone());
}
}
- virtual void submit()
+
+ virtual void
+ submit()
{
accept();
}
protected:
-
AronTreeWidgetItem init;
AronTreeWidgetItem* item;
@@ -54,4 +62,4 @@ namespace armarx
};
using AronTreeWidgetModalControllerPtr = std::shared_ptr<AronTreeWidgetModal>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.cpp
index 7bc282597909fef3dcd538724cb2bf52f05893d2..430872a0be958d0176142892afb162fd4fb0b87f 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.cpp
@@ -2,7 +2,10 @@
namespace armarx
{
- AronTreeWidgetBoolInputModalController::AronTreeWidgetBoolInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
+ AronTreeWidgetBoolInputModalController::AronTreeWidgetBoolInputModalController(
+ const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent) :
AronTreeWidgetModal(label, item, parent)
{
widget.setupUi(this);
@@ -10,18 +13,22 @@ namespace armarx
// TODO
}
- void AronTreeWidgetBoolInputModalController::submit()
+ void
+ AronTreeWidgetBoolInputModalController::submit()
{
- item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE, widget.textEditInput->toPlainText());
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE,
+ widget.textEditInput->toPlainText());
AronTreeWidgetModal::submit();
}
- void AronTreeWidgetBoolInputModalController::reset()
+ void
+ AronTreeWidgetBoolInputModalController::reset()
{
AronTreeWidgetModal::reset();
// reset to initial value
- widget.textEditInput->setPlainText(init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
+ widget.textEditInput->setPlainText(
+ init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.h
index 1a1c60a07665fae06869962bc9a66c84da4cef12..65d908c3e2d8487b1236fcced0f71a9e295c0cb2 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/bool/AronTreeWidgetBoolInputModalController.h
@@ -1,20 +1,20 @@
#pragma once
-#include "../AronTreeWidgetModal.h"
+#include <QDialog>
#include <RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/bool/ui_AronTreeWidgetBoolInputModal.h>
-#include <QDialog>
+#include "../AronTreeWidgetModal.h"
namespace armarx
{
- class AronTreeWidgetBoolInputModalController :
- public AronTreeWidgetModal
+ class AronTreeWidgetBoolInputModalController : public AronTreeWidgetModal
{
public:
-
- AronTreeWidgetBoolInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent);
+ AronTreeWidgetBoolInputModalController(const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent);
private slots:
@@ -24,4 +24,4 @@ namespace armarx
private:
Ui::AronTreeWidgetBoolInputModalWidget widget;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
index 71a88e54fcf49d77ea5de6804ef06a11e90e6ae8..c391bb079be9d501eeb592769512402c0ef667e4 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.cpp
@@ -6,7 +6,10 @@
namespace armarx
{
- AronTreeWidgetDictInputModalController::AronTreeWidgetDictInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
+ AronTreeWidgetDictInputModalController::AronTreeWidgetDictInputModalController(
+ const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent) :
AronTreeWidgetModal(label, item, parent)
{
widget.setupUi(this);
@@ -16,17 +19,23 @@ namespace armarx
reset();
// connect signals
- connect(widget.pushButtonAddElement, &QPushButton::clicked,
- this, &AronTreeWidgetDictInputModalController::addEmptyElement);
-
- connect(widget.pushButtonReset, &QPushButton::clicked,
- this, &AronTreeWidgetDictInputModalController::reset);
- connect(widget.pushButtonSubmit, &QPushButton::clicked,
- this, &AronTreeWidgetDictInputModalController::submit);
+ connect(widget.pushButtonAddElement,
+ &QPushButton::clicked,
+ this,
+ &AronTreeWidgetDictInputModalController::addEmptyElement);
+ connect(widget.pushButtonReset,
+ &QPushButton::clicked,
+ this,
+ &AronTreeWidgetDictInputModalController::reset);
+ connect(widget.pushButtonSubmit,
+ &QPushButton::clicked,
+ this,
+ &AronTreeWidgetDictInputModalController::submit);
}
- void AronTreeWidgetDictInputModalController::submit()
+ void
+ AronTreeWidgetDictInputModalController::submit()
{
for (const auto& added : addedItems)
{
@@ -36,7 +45,8 @@ namespace armarx
AronTreeWidgetModal::submit();
}
- void AronTreeWidgetDictInputModalController::reset()
+ void
+ AronTreeWidgetDictInputModalController::reset()
{
AronTreeWidgetModal::reset();
@@ -49,7 +59,8 @@ namespace armarx
}
}
- void AronTreeWidgetDictInputModalController::addEmptyElement()
+ void
+ AronTreeWidgetDictInputModalController::addEmptyElement()
{
QString s = widget.lineEditKey->text();
widget.lineEditKey->setText("Enter Key");
@@ -73,4 +84,4 @@ namespace armarx
}
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.h
index f5d94a04b46994220fb0ff9c81e1fff50a9c3091..a64b2ffb5202b9f51ea58c9dbf04730002762a1b 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/AronTreeWidgetDictInputModalController.h
@@ -1,26 +1,27 @@
#pragma once
#include <stack>
+
+#include <QDialog>
+
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include "../AronTreeWidgetModal.h"
-
#include <RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/dict/ui_AronTreeWidgetDictInputModal.h>
-#include <QDialog>
+#include "../AronTreeWidgetModal.h"
namespace armarx
{
- class AronTreeWidgetDictInputModalController :
- public AronTreeWidgetModal
+ class AronTreeWidgetDictInputModalController : public AronTreeWidgetModal
{
public:
-
- AronTreeWidgetDictInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent);
+ AronTreeWidgetDictInputModalController(const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent);
private slots:
@@ -33,4 +34,4 @@ namespace armarx
std::vector<AronTreeWidgetItem*> addedItems;
Ui::AronTreeWidgetDictInputModalWidget widget;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.cpp
index ae9cbe6aac34390785cdc410508681ff0b79a9f6..850d10af477c72b9be8e9cc300b4e4c8d10000aa 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.cpp
@@ -2,7 +2,10 @@
namespace armarx
{
- AronTreeWidgetFloatInputModalController::AronTreeWidgetFloatInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
+ AronTreeWidgetFloatInputModalController::AronTreeWidgetFloatInputModalController(
+ const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent) :
AronTreeWidgetModal(label, item, parent)
{
widget.setupUi(this);
@@ -10,18 +13,22 @@ namespace armarx
// TODO
}
- void AronTreeWidgetFloatInputModalController::submit()
+ void
+ AronTreeWidgetFloatInputModalController::submit()
{
- item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE, widget.textEditInput->toPlainText());
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE,
+ widget.textEditInput->toPlainText());
AronTreeWidgetModal::submit();
}
- void AronTreeWidgetFloatInputModalController::reset()
+ void
+ AronTreeWidgetFloatInputModalController::reset()
{
AronTreeWidgetModal::reset();
// reset to initial value
- widget.textEditInput->setPlainText(init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
+ widget.textEditInput->setPlainText(
+ init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.h
index debd50e9f04eae0d35c161d9124e7876d3eacc6c..f8d60deb0fed4f84a5b831c75b189b086ab8b3e9 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/float_double/AronTreeWidgetFloatInputModalController.h
@@ -1,20 +1,20 @@
#pragma once
-#include "../AronTreeWidgetModal.h"
+#include <QDialog>
#include <RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/float_double/ui_AronTreeWidgetFloatInputModal.h>
-#include <QDialog>
+#include "../AronTreeWidgetModal.h"
namespace armarx
{
- class AronTreeWidgetFloatInputModalController :
- public AronTreeWidgetModal
+ class AronTreeWidgetFloatInputModalController : public AronTreeWidgetModal
{
public:
-
- AronTreeWidgetFloatInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent);
+ AronTreeWidgetFloatInputModalController(const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent);
private slots:
@@ -24,4 +24,4 @@ namespace armarx
private:
Ui::AronTreeWidgetFloatInputModalWidget widget;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.cpp
index dd9b19e06494c8720f7cc2e172b0caecad3fc7d2..7997e9f0b2d6a6ea8e8a4e4c7c19a0a1dab06123 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.cpp
@@ -2,7 +2,10 @@
namespace armarx
{
- AronTreeWidgetIntInputModalController::AronTreeWidgetIntInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
+ AronTreeWidgetIntInputModalController::AronTreeWidgetIntInputModalController(
+ const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent) :
AronTreeWidgetModal(label, item, parent)
{
widget.setupUi(this);
@@ -10,18 +13,22 @@ namespace armarx
// TODO
}
- void AronTreeWidgetIntInputModalController::submit()
+ void
+ AronTreeWidgetIntInputModalController::submit()
{
- item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE, widget.textEditInput->toPlainText());
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE,
+ widget.textEditInput->toPlainText());
AronTreeWidgetModal::submit();
}
- void AronTreeWidgetIntInputModalController::reset()
+ void
+ AronTreeWidgetIntInputModalController::reset()
{
AronTreeWidgetModal::reset();
// reset to initial value
- widget.textEditInput->setPlainText(init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
+ widget.textEditInput->setPlainText(
+ init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.h
index 1de43a0870b2af8ff4edbc974c13f826d31e67ef..094efe785605bb3ef35850ffbbac79ca4eec87b5 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/int_long/AronTreeWidgetIntInputModalController.h
@@ -1,20 +1,20 @@
#pragma once
-#include "../AronTreeWidgetModal.h"
+#include <QDialog>
#include <RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/int_long/ui_AronTreeWidgetIntInputModal.h>
-#include <QDialog>
+#include "../AronTreeWidgetModal.h"
namespace armarx
{
- class AronTreeWidgetIntInputModalController :
- public AronTreeWidgetModal
+ class AronTreeWidgetIntInputModalController : public AronTreeWidgetModal
{
public:
-
- AronTreeWidgetIntInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent);
+ AronTreeWidgetIntInputModalController(const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent);
private slots:
@@ -24,4 +24,4 @@ namespace armarx
private:
Ui::AronTreeWidgetIntInputModalWidget widget;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp
index 972fe6ff98b20e2bfe0970455df1ed760a4c4203..3167fe08741d1afbca6a880dd24468b49870396d 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.cpp
@@ -2,7 +2,10 @@
namespace armarx
{
- AronTreeWidgetTextInputModalController::AronTreeWidgetTextInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
+ AronTreeWidgetTextInputModalController::AronTreeWidgetTextInputModalController(
+ const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent) :
AronTreeWidgetModal(label, item, parent)
{
widget.setupUi(this);
@@ -12,24 +15,32 @@ namespace armarx
reset();
// connect signals
- connect(widget.pushButtonReset, &QPushButton::clicked,
- this, &AronTreeWidgetTextInputModalController::reset);
- connect(widget.pushButtonSubmit, &QPushButton::clicked,
- this, &AronTreeWidgetTextInputModalController::submit);
+ connect(widget.pushButtonReset,
+ &QPushButton::clicked,
+ this,
+ &AronTreeWidgetTextInputModalController::reset);
+ connect(widget.pushButtonSubmit,
+ &QPushButton::clicked,
+ this,
+ &AronTreeWidgetTextInputModalController::submit);
}
- void AronTreeWidgetTextInputModalController::submit()
+ void
+ AronTreeWidgetTextInputModalController::submit()
{
- item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE, widget.textEditInput->toPlainText());
+ item->setText(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE,
+ widget.textEditInput->toPlainText());
AronTreeWidgetModal::submit();
}
- void AronTreeWidgetTextInputModalController::reset()
+ void
+ AronTreeWidgetTextInputModalController::reset()
{
AronTreeWidgetModal::reset();
// reset to initial value
- widget.textEditInput->setPlainText(init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
+ widget.textEditInput->setPlainText(
+ init.text(aron_tree_widget::constantes::TREE_WIDGET_ITEM_VALUE));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h
index c86cb5e8475d507b71495a3c28eca34f063a932e..39efb36e3a9f620ac811b6f75084f7f7fd9d3e0e 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/AronTreeWidgetTextInputModalController.h
@@ -1,20 +1,20 @@
#pragma once
-#include "../AronTreeWidgetModal.h"
+#include <QDialog>
#include <RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/modal/text/ui_AronTreeWidgetTextInputModal.h>
-#include <QDialog>
+#include "../AronTreeWidgetModal.h"
namespace armarx
{
- class AronTreeWidgetTextInputModalController :
- public AronTreeWidgetModal
+ class AronTreeWidgetTextInputModalController : public AronTreeWidgetModal
{
public:
-
- AronTreeWidgetTextInputModalController(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent);
+ AronTreeWidgetTextInputModalController(const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent);
private slots:
@@ -24,4 +24,4 @@ namespace armarx
private:
Ui::AronTreeWidgetTextInputModalWidget widget;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp
index 98e271cd81c8180af051e094169f63b58176b0e8..5fcbb59304154b072b6467b4e211418b7f265bf3 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetContextMenu.cpp
@@ -65,7 +65,6 @@ namespace armarx
}
}
-
void
AronTreeWidgetContextMenuVisitor::visitAronVariant(const aron::type::ObjectPtr&)
{
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
index ddd6ee11312de00f2b7451905ffc404ff5fd6322..9a9146d09f09860f8aea077a7e0917089b5324a2 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
@@ -281,16 +281,16 @@ namespace armarx
{
case armarx::aron::type::matrix::UINT8:
createdMatrix->setType("unsigned char");
- break;
+ break;
case armarx::aron::type::matrix::UINT16:
createdMatrix->setType("unsigned short");
- break;
+ break;
case armarx::aron::type::matrix::UINT32:
createdMatrix->setType("unsigned int");
break;
case armarx::aron::type::matrix::INT8:
createdMatrix->setType("char");
- break;
+ break;
case armarx::aron::type::matrix::INT16:
createdMatrix->setType("short");
break;
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
index c127ac5e49276e878221dc39a5db8a7b821ccec7..c61aa7c298f8c6a8d2d84e58717a26d2f4d8ea16 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.h
@@ -28,7 +28,6 @@
// forward declarations of qt
class QTreeWidgetItem;
-
namespace armarx
{
// Conversion from TreeView to aron data
@@ -40,9 +39,11 @@ namespace armarx
aron::data::VariantPtr createdAron = nullptr;
AronTreeWidgetConverterVisitor() = delete;
+
AronTreeWidgetConverterVisitor(QTreeWidgetItem* i, int x) : parentItem(i), index(x)
{
}
+
// if the conversion was successful after calling visit()
bool isConversionSuccessful();
// returns true if this type itself was sucessfully parsed, but some contained object failed.
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
index 6f83e52c44b785fa1e277138a3e0d47f9220a89e..b3267a64fd75c734adcdd114fb70f2da2b3c13be 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
@@ -231,7 +231,7 @@ namespace armarx
case armarx::aron::type::matrix::UINT32:
type = "<uint32>";
break;
- case armarx::aron::type::matrix::INT8:
+ case armarx::aron::type::matrix::INT8:
type = "<int8>";
break;
case armarx::aron::type::matrix::INT16:
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
index fc58634bb9bc02834d4e72663e3c16ffe36d6de6..1d4f6ab138ae039353c93d328dec88360de08342 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
@@ -20,10 +20,10 @@
* GNU General Public License
*/
-#include <string>
-
#include "AronTreeWidgetModalCreator.h"
+#include <string>
+
#include <SimoxUtility/algorithm/string.h>
// modals
@@ -40,12 +40,14 @@ namespace armarx
void
AronTreeWidgetModalCreatorVisitor::visitAronVariant(const aron::type::StringPtr& i)
{
- createdModal = std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
+ createdModal =
+ std::make_shared<AronTreeWidgetTextInputModalController>(label, item, parent);
}
- void AronTreeWidgetModalCreatorVisitor::visitUnknown(Input&)
+ void
+ AronTreeWidgetModalCreatorVisitor::visitUnknown(Input&)
{
- ARMARX_WARNING_S << "Received an unknown type when trying to create a tree view widget modal for a skill argument type.";
+ ARMARX_WARNING_S << "Received an unknown type when trying to create a tree view widget "
+ "modal for a skill argument type.";
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
index f4c8f0223a039c6b9be926d6bddf360bce32f629..50b17f2104b12a66ce0d44ab2e7a3d2a2d16f3d5 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.h
@@ -21,19 +21,18 @@
*/
#pragma once
-#include "../modal/AronTreeWidgetModal.h"
-
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/visitor/variant/VariantVisitor.h>
+#include "../modal/AronTreeWidgetModal.h"
+
namespace armarx
{
// Convert aron type to tree widget.
// (Widgets are only created for string types to enter longer texts.
// However, the visitor implementation allows modals for differnt types. Might be useful in the future..)
- class AronTreeWidgetModalCreatorVisitor :
- public armarx::aron::type::ConstVariantVisitor
+ class AronTreeWidgetModalCreatorVisitor : public armarx::aron::type::ConstVariantVisitor
{
public:
std::string label = "";
@@ -42,15 +41,15 @@ namespace armarx
AronTreeWidgetModalControllerPtr createdModal = nullptr;
AronTreeWidgetModalCreatorVisitor() = delete;
- AronTreeWidgetModalCreatorVisitor(const std::string& label, AronTreeWidgetItem* item, QTreeWidget* parent) :
- label(label),
- item(item),
- parent(parent)
- {}
+
+ AronTreeWidgetModalCreatorVisitor(const std::string& label,
+ AronTreeWidgetItem* item,
+ QTreeWidget* parent) :
+ label(label), item(item), parent(parent)
+ {
+ }
void visitAronVariant(const aron::type::StringPtr& i) final;
void visitUnknown(Input&) final;
};
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h
index 5090c12e82d28498efe8e1e63015e659f1e1abce..34bc633ad3defce259646882bdb195bb26a4f940 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetSetter.h
@@ -42,6 +42,7 @@ namespace armarx
AronTreeWidgetItem* qWidgetItem;
AronTreeWidgetSetterVisitor() = delete;
+
AronTreeWidgetSetterVisitor(QTreeWidgetItem* i, int x) : parentItem(i), index(x)
{
}
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp
index 2caddc1846edfb80005225c963c2bd93f7dfb73c..28b48686e13b84ab681d0e38f5c8151ee5c389c7 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.cpp
@@ -246,11 +246,13 @@ namespace armarx
{
return std::min(realRows, MAX_ROWS_DISPLAY);
}
+
long
EditMatrixWidget::getDisplayedCols() const
{
return std::min(realCols, MAX_COLS_DISPLAY);
}
+
void
EditMatrixWidget::matrixElementChanged()
{
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h
index 7ba61b702daed837495375e5afeeec47b695cda9..9122ead2e82ede436d24902ca49c33442807c086 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/EditMatrixWidget.h
@@ -6,6 +6,7 @@
#include <QObject>
#include <QTreeWidgetItem>
#include <QVBoxLayout>
+
#include <SimoxUtility/algorithm/string/string_conversion.h>
#include "RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h"
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp
index 768d907b7bb7e81576a5a50c9e9bfe5e353383e1..adb9ce76063e0729c5e648f473f06092970638a1 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/IntEnumWidget.cpp
@@ -1,6 +1,7 @@
#include "IntEnumWidget.h"
#include <QHBoxLayout>
+
#include <SimoxUtility/algorithm/string/string_conversion.h>
#include "RobotAPI/libraries/aron/core/type/variant/All.h"
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp
index 7ba00d9da9f38fa7975ff67cd2096e30b30cbbbc..f25ee08be89361c9dbf3ac0d07f601fc4d9e8ac9 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.cpp
@@ -3,11 +3,14 @@
#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
-#include <RobotAPI/interface/aron/Aron.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/interface/aron/Aron.h>
+
#include "../../ColorPalettes.h"
#include "NDArrayHelper.h"
+
namespace armarx
{
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h
index e1fca223ef52042b8174231d2d0eaf320efcf158..3c5bc5912be9bf2eededf36b59ca672286f2db03 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/widgets/QuaternionWidget.h
@@ -6,13 +6,14 @@
#include <QLineEdit>
#include <QObject>
#include <QVBoxLayout>
+
#include <SimoxUtility/algorithm/string/string_conversion.h>
#include <SimoxUtility/error/SimoxError.h>
+
#include <RobotAPI/interface/aron/Aron.h>
#include "CustomWidget.h"
-
namespace armarx
{
// Custom Widget which represents a Quaternion - This class does not normalize the inputs
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.cpp b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.cpp
index d221e2ff267cbcbf7768d2be6bdf5e65872c81e6..f47ed75c8f1c30917266f171462872aec245e7f2 100644
--- a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.cpp
@@ -1,18 +1,17 @@
#include "ViewSelectionConfigDialog.h"
-#include <RobotAPI/gui-plugins/ViewSelection/ui_ViewSelectionConfigDialog.h>
-
-#include <RobotAPI/interface/components/ViewSelectionInterface.h>
-#include <QPushButton>
#include <QMessageBox>
+#include <QPushButton>
+
#include <IceUtil/UUID.h>
+#include <RobotAPI/gui-plugins/ViewSelection/ui_ViewSelectionConfigDialog.h>
+#include <RobotAPI/interface/components/ViewSelectionInterface.h>
+
using namespace armarx;
ViewSelectionConfigDialog::ViewSelectionConfigDialog(QWidget* parent) :
- QDialog(parent),
- ui(new Ui::ViewSelectionConfigDialog),
- uuid(IceUtil::generateUUID())
+ QDialog(parent), ui(new Ui::ViewSelectionConfigDialog), uuid(IceUtil::generateUUID())
{
ui->setupUi(this);
ui->buttonBox->button(QDialogButtonBox::Ok)->setDefault(true);
@@ -28,27 +27,31 @@ ViewSelectionConfigDialog::~ViewSelectionConfigDialog()
delete ui;
}
-
-void ViewSelectionConfigDialog::onInitComponent()
+void
+ViewSelectionConfigDialog::onInitComponent()
{
}
-void ViewSelectionConfigDialog::onConnectComponent()
+void
+ViewSelectionConfigDialog::onConnectComponent()
{
viewSelectionProxyFinder->setIceManager(getIceManager());
}
-void ViewSelectionConfigDialog::onExitComponent()
+void
+ViewSelectionConfigDialog::onExitComponent()
{
QObject::disconnect();
}
-std::string ViewSelectionConfigDialog::getDefaultName() const
+std::string
+ViewSelectionConfigDialog::getDefaultName() const
{
return "ViewSelectionConfigDialog" + uuid;
}
-void ViewSelectionConfigDialog::verifyConfig()
+void
+ViewSelectionConfigDialog::verifyConfig()
{
if (!viewSelectionProxyFinder->getSelectedProxyName().trimmed().length())
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.h b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.h
index 97b06a077436640ae62a8b8018d121a40a3435c6..bb215132ad13c6d57a4a4dcb3808fba3a4067824 100644
--- a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionConfigDialog.h
@@ -5,7 +5,6 @@
#include <ArmarXCore/core/ManagedIceObject.h>
-
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
namespace Ui
@@ -13,8 +12,7 @@ namespace Ui
class ViewSelectionConfigDialog;
}
-class ViewSelectionConfigDialog : public QDialog,
- virtual public armarx::ManagedIceObject
+class ViewSelectionConfigDialog : public QDialog, virtual public armarx::ManagedIceObject
{
Q_OBJECT
@@ -22,7 +20,8 @@ public:
explicit ViewSelectionConfigDialog(QWidget* parent = 0);
~ViewSelectionConfigDialog() override;
- std::string getViewSelectionName()
+ std::string
+ getViewSelectionName()
{
return viewSelectionProxyFinder->getSelectedProxyName().toStdString();
}
@@ -40,12 +39,9 @@ protected:
std::string getDefaultName() const override;
private:
-
Ui::ViewSelectionConfigDialog* ui;
armarx::IceProxyFinderBase* viewSelectionProxyFinder;
std::string uuid;
-
};
-
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.cpp b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.cpp
index f72a0dded172bf20476241f932146a9d686c1a9e..b64ca54c4dbf8694d6a63b11d1faebd19574462d 100644
--- a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.cpp
@@ -28,5 +28,5 @@ using namespace armarx;
ViewSelectionGuiPlugin::ViewSelectionGuiPlugin()
{
- addWidget < ViewSelectionWidgetController > ();
+ addWidget<ViewSelectionWidgetController>();
}
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.h b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.h
index be569a604570ea6ecadcf2a6acb902a885727900..0c16ef78e9d35f15845244de813dd37039de239f 100644
--- a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionGuiPlugin.h
@@ -23,8 +23,9 @@
#pragma once
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
namespace armarx
{
@@ -35,8 +36,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT ViewSelectionGuiPlugin:
- public armarx::ArmarXGuiPlugin
+ class ARMARXCOMPONENT_IMPORT_EXPORT ViewSelectionGuiPlugin : public armarx::ArmarXGuiPlugin
{
Q_OBJECT
Q_INTERFACES(ArmarXGuiInterface)
@@ -48,5 +48,4 @@ namespace armarx
*/
ViewSelectionGuiPlugin();
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.cpp b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.cpp
index aa366549f925c7f97406fce61383272ffe3bcf3e..578672b995d0b7986c6be09b9623c65daa53de59 100644
--- a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.cpp
@@ -22,9 +22,10 @@
#include "ViewSelectionWidgetController.h"
-#include <QListWidgetItem>
#include <string>
+#include <QListWidgetItem>
+
using namespace armarx;
ViewSelectionWidgetController::ViewSelectionWidgetController()
@@ -38,18 +39,14 @@ ViewSelectionWidgetController::ViewSelectionWidgetController()
connect(widget.pushButton_3, SIGNAL(clicked()), this, SLOT(updateSaliencyMapNames()));
connect(widget.checkBox, SIGNAL(toggled(bool)), this, SLOT(toggleViewSelection(bool)));
-
}
-
ViewSelectionWidgetController::~ViewSelectionWidgetController()
{
-
}
-
-
-QPointer<QDialog> ViewSelectionWidgetController::getConfigDialog(QWidget* parent)
+QPointer<QDialog>
+ViewSelectionWidgetController::getConfigDialog(QWidget* parent)
{
if (!configDialog)
{
@@ -59,49 +56,55 @@ QPointer<QDialog> ViewSelectionWidgetController::getConfigDialog(QWidget* parent
return qobject_cast<ViewSelectionConfigDialog*>(configDialog);
}
-
-void ViewSelectionWidgetController::loadSettings(QSettings* settings)
+void
+ViewSelectionWidgetController::loadSettings(QSettings* settings)
{
viewSelectionName = settings->value("viewSelectionName", "").toString().toStdString();
}
-void ViewSelectionWidgetController::saveSettings(QSettings* settings)
+void
+ViewSelectionWidgetController::saveSettings(QSettings* settings)
{
- viewSelectionName = settings->value("viewSelectionName", QString::fromStdString(viewSelectionName)).toString().toStdString();
+ viewSelectionName =
+ settings->value("viewSelectionName", QString::fromStdString(viewSelectionName))
+ .toString()
+ .toStdString();
}
-
-void ViewSelectionWidgetController::onInitComponent()
+void
+ViewSelectionWidgetController::onInitComponent()
{
usingProxy(viewSelectionName);
usingTopic(viewSelectionName + "Observer");
-
}
-
-void ViewSelectionWidgetController::onConnectComponent()
+void
+ViewSelectionWidgetController::onConnectComponent()
{
viewSelection = getProxy<ViewSelectionInterfacePrx>(viewSelectionName);
widget.checkBox->setChecked(viewSelection->isEnabledAutomaticViewSelection());
}
-
-void armarx::ViewSelectionWidgetController::onActivateAutomaticViewSelection(const Ice::Current&)
+void
+armarx::ViewSelectionWidgetController::onActivateAutomaticViewSelection(const Ice::Current&)
{
widget.checkBox->setChecked(true);
}
-void armarx::ViewSelectionWidgetController::onDeactivateAutomaticViewSelection(const Ice::Current&)
+void
+armarx::ViewSelectionWidgetController::onDeactivateAutomaticViewSelection(const Ice::Current&)
{
widget.checkBox->setChecked(false);
}
-void armarx::ViewSelectionWidgetController::nextViewTarget(Ice::Long, const Ice::Current&)
+void
+armarx::ViewSelectionWidgetController::nextViewTarget(Ice::Long, const Ice::Current&)
{
}
-void ViewSelectionWidgetController::toggleViewSelection(bool isEnabled)
+void
+ViewSelectionWidgetController::toggleViewSelection(bool isEnabled)
{
ARMARX_LOG << "toggling view selection " << isEnabled;
@@ -115,7 +118,8 @@ void ViewSelectionWidgetController::toggleViewSelection(bool isEnabled)
}
}
-void ViewSelectionWidgetController::triggerSaliencyMapVisualization()
+void
+ViewSelectionWidgetController::triggerSaliencyMapVisualization()
{
std::vector<std::string> names;
@@ -127,13 +131,12 @@ void ViewSelectionWidgetController::triggerSaliencyMapVisualization()
{
names.push_back(item->text().toStdString());
}
-
}
viewSelection->drawSaliencySphere(names);
-
}
-void ViewSelectionWidgetController::updateSaliencyMapNames()
+void
+ViewSelectionWidgetController::updateSaliencyMapNames()
{
std::vector<std::string> names = viewSelection->getSaliencyMapNames();
@@ -141,19 +144,21 @@ void ViewSelectionWidgetController::updateSaliencyMapNames()
for (std::string name : names)
{
- QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(name), widget.listWidget);
+ QListWidgetItem* item =
+ new QListWidgetItem(QString::fromStdString(name), widget.listWidget);
item->setFlags(item->flags() | Qt::ItemIsUserCheckable);
item->setCheckState(Qt::Unchecked);
}
}
-
-void ViewSelectionWidgetController::clearSaliencyMapVisualization()
+void
+ViewSelectionWidgetController::clearSaliencyMapVisualization()
{
viewSelection->clearSaliencySphere();
}
-void armarx::ViewSelectionWidgetController::configured()
+void
+armarx::ViewSelectionWidgetController::configured()
{
viewSelectionName = configDialog->getViewSelectionName();
}
diff --git a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h
index d95095edc7862d2386ca1d188aa252a339398871..f0b7b09e85564729667fecfceaa747ef923662f6 100644
--- a/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ViewSelection/ViewSelectionWidgetController.h
@@ -22,13 +22,12 @@
#pragma once
-#include <RobotAPI/gui-plugins/ViewSelection/ui_ViewSelectionWidget.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
+#include <RobotAPI/gui-plugins/ViewSelection/ui_ViewSelectionWidget.h>
#include <RobotAPI/interface/components/ViewSelectionInterface.h>
#include "ViewSelectionConfigDialog.h"
@@ -57,8 +56,7 @@ namespace armarx
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- ViewSelectionWidgetController:
+ class ARMARXCOMPONENT_IMPORT_EXPORT ViewSelectionWidgetController :
public ArmarXComponentWidgetControllerTemplate<ViewSelectionWidgetController>,
public armarx::ViewSelectionObserver
{
@@ -95,7 +93,8 @@ namespace armarx
* Returns the Widget name displayed in the ArmarXGui to create an
* instance of this class.
*/
- static QString GetWidgetName()
+ static QString
+ GetWidgetName()
{
return "RobotControl.ViewSelection";
}
@@ -135,7 +134,5 @@ namespace armarx
ViewSelectionInterfacePrx viewSelection;
std::string viewSelectionName;
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/interface/ArViz.ice b/source/RobotAPI/interface/ArViz.ice
index bfbc8715305165f7d8d74ff39a463e3727727105..e27fd71c9e1978c3f29d232548e3a7e5b80d6a83 100644
--- a/source/RobotAPI/interface/ArViz.ice
+++ b/source/RobotAPI/interface/ArViz.ice
@@ -1,5 +1,4 @@
#pragma once
-#include <RobotAPI/interface/ArViz/Elements.ice>
#include <RobotAPI/interface/ArViz/Component.ice>
-
+#include <RobotAPI/interface/ArViz/Elements.ice>
diff --git a/source/RobotAPI/interface/ArViz/Component.ice b/source/RobotAPI/interface/ArViz/Component.ice
index 6ba528b4f87fe2dab27c6d9511612b3ae01055ed..0af7862b659cfab7071e313813c79dbdefda645b 100644
--- a/source/RobotAPI/interface/ArViz/Component.ice
+++ b/source/RobotAPI/interface/ArViz/Component.ice
@@ -1,149 +1,145 @@
#pragma once
-#include <RobotAPI/interface/ArViz/Elements.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-module armarx
-{
-module viz
-{
-module data
-{
-
-sequence <Element> ElementSeq;
-
-enum LayerAction
-{
- // Create a new layer or update an existing layer
- Layer_CREATE_OR_UPDATE,
-
- // Delete an existing layer
- Layer_DELETE,
-};
-
-struct LayerUpdate
-{
- string component;
- string name;
- LayerAction action = Layer_CREATE_OR_UPDATE;
-
- // Elements are only needed for Layer_CREATE_OR_UPDATE
- ElementSeq elements;
-};
-
-sequence <LayerUpdate> LayerUpdateSeq;
-
-struct TimestampedLayerUpdate
-{
- LayerUpdate update;
- long revision = 0;
- long timestampInMicroseconds = 0;
-};
-
-struct LayerUpdates
-{
- LayerUpdateSeq updates;
- long revision = 0;
-};
-
-struct CommitInput
-{
- LayerUpdateSeq updates;
-
- string interactionComponent;
- Ice::StringSeq interactionLayers;
-};
-
-struct CommitResult
-{
- // The revision number corresponding to the applied commit
- long revision = 0;
-
- // Interactions for the requested layers (see Layer_RECEIVE_INTERACTIONS)
- InteractionFeedbackSeq interactions;
-};
-
-struct RecordingHeader
-{
- string prefix;
- string comment;
- long firstTimestampInMicroSeconds = 0;
- long lastTimestampInMicroSeconds = 0;
- long firstRevision = 0;
- long lastRevision = 0;
-};
-
-struct RecordingBatchHeader
-{
- long index = 0;
- long firstTimestampInMicroSeconds = 0;
- long lastTimestampInMicroSeconds = 0;
- long firstRevision = 0;
- long lastRevision = 0;
-};
-
-sequence<RecordingBatchHeader> RecordingBatchHeaderSeq;
-
-struct Recording
-{
- string id;
- string comment;
- long firstTimestampInMicroSeconds = 0;
- long lastTimestampInMicroSeconds = 0;
- long firstRevision = 0;
- long lastRevision = 0;
- RecordingBatchHeaderSeq batchHeaders;
-};
-
-sequence<TimestampedLayerUpdate> TimestampedLayerUpdateSeq;
-
-struct RecordingBatch
-{
- RecordingBatchHeader header;
- TimestampedLayerUpdateSeq initialState; // Keyframe with complete state
- TimestampedLayerUpdateSeq updates; // Incremental updates on keyframe
-};
-
-sequence<Recording> RecordingSeq;
-
-struct RecordingsInfo
-{
- data::RecordingSeq recordings;
- // Equivalent to the property "HistoryPath" of ArVizStorage.
- string recordingsPath;
-};
-
-};
+#include <RobotAPI/interface/ArViz/Elements.ice>
-interface StorageInterface
+module armarx
{
- data::CommitResult commitAndReceiveInteractions(data::CommitInput input);
-
- data::LayerUpdates pullUpdatesSince(long revision);
-
- data::LayerUpdates pullUpdatesSinceAndSendInteractions(
+ module viz
+ {
+ module data
+ {
+
+ sequence<Element> ElementSeq;
+
+ enum LayerAction
+ {
+ // Create a new layer or update an existing layer
+ Layer_CREATE_OR_UPDATE,
+
+ // Delete an existing layer
+ Layer_DELETE,
+ };
+
+ struct LayerUpdate
+ {
+ string component;
+ string name;
+ LayerAction action = Layer_CREATE_OR_UPDATE;
+
+ // Elements are only needed for Layer_CREATE_OR_UPDATE
+ ElementSeq elements;
+ };
+
+ sequence<LayerUpdate> LayerUpdateSeq;
+
+ struct TimestampedLayerUpdate
+ {
+ LayerUpdate update;
+ long revision = 0;
+ long timestampInMicroseconds = 0;
+ };
+
+ struct LayerUpdates
+ {
+ LayerUpdateSeq updates;
+ long revision = 0;
+ };
+
+ struct CommitInput
+ {
+ LayerUpdateSeq updates;
+
+ string interactionComponent;
+ Ice::StringSeq interactionLayers;
+ };
+
+ struct CommitResult
+ {
+ // The revision number corresponding to the applied commit
+ long revision = 0;
+
+ // Interactions for the requested layers (see Layer_RECEIVE_INTERACTIONS)
+ InteractionFeedbackSeq interactions;
+ };
+
+ struct RecordingHeader
+ {
+ string prefix;
+ string comment;
+ long firstTimestampInMicroSeconds = 0;
+ long lastTimestampInMicroSeconds = 0;
+ long firstRevision = 0;
+ long lastRevision = 0;
+ };
+
+ struct RecordingBatchHeader
+ {
+ long index = 0;
+ long firstTimestampInMicroSeconds = 0;
+ long lastTimestampInMicroSeconds = 0;
+ long firstRevision = 0;
+ long lastRevision = 0;
+ };
+
+ sequence<RecordingBatchHeader> RecordingBatchHeaderSeq;
+
+ struct Recording
+ {
+ string id;
+ string comment;
+ long firstTimestampInMicroSeconds = 0;
+ long lastTimestampInMicroSeconds = 0;
+ long firstRevision = 0;
+ long lastRevision = 0;
+ RecordingBatchHeaderSeq batchHeaders;
+ };
+
+ sequence<TimestampedLayerUpdate> TimestampedLayerUpdateSeq;
+
+ struct RecordingBatch
+ {
+ RecordingBatchHeader header;
+ TimestampedLayerUpdateSeq initialState; // Keyframe with complete state
+ TimestampedLayerUpdateSeq updates; // Incremental updates on keyframe
+ };
+
+ sequence<Recording> RecordingSeq;
+
+ struct RecordingsInfo
+ {
+ data::RecordingSeq recordings;
+ // Equivalent to the property "HistoryPath" of ArVizStorage.
+ string recordingsPath;
+ };
+ };
+
+ interface StorageInterface
+ {
+ data::CommitResult commitAndReceiveInteractions(data::CommitInput input);
+
+ data::LayerUpdates pullUpdatesSince(long revision);
+
+ data::LayerUpdates pullUpdatesSinceAndSendInteractions(
long revision, data::InteractionFeedbackSeq interactions);
- string startRecording(string prefix);
+ string startRecording(string prefix);
- void stopRecording();
+ void stopRecording();
- data::RecordingsInfo getAllRecordings();
+ data::RecordingsInfo getAllRecordings();
- data::RecordingBatch getRecordingBatch(string recordingID, long batchIndex);
-};
+ data::RecordingBatch getRecordingBatch(string recordingID, long batchIndex);
+ };
-interface Topic
-{
- // FIXME: The old interface used this topic.
- // But this call has been superceeded by commitAndReceiveInteractions
- void updateLayers(data::LayerUpdateSeq updates);
-};
+ interface Topic
+ {
+ // FIXME: The old interface used this topic.
+ // But this call has been superceeded by commitAndReceiveInteractions
+ void updateLayers(data::LayerUpdateSeq updates);
+ };
-interface StorageAndTopicInterface extends StorageInterface, Topic
-{
-};
-
-
-};
+ interface StorageAndTopicInterface extends StorageInterface, Topic{};
+ };
};
diff --git a/source/RobotAPI/interface/ArViz/Elements.ice b/source/RobotAPI/interface/ArViz/Elements.ice
index 5833f259e501b2e47106bc1c9195158f30fccd83..007bc1278219ad78028a708ca95c4eae805df0e4 100644
--- a/source/RobotAPI/interface/ArViz/Elements.ice
+++ b/source/RobotAPI/interface/ArViz/Elements.ice
@@ -6,259 +6,258 @@
module armarx
{
-module viz
-{
-module data
-{
- struct GlobalPose
- {
- float x = 0.0f;
- float y = 0.0f;
- float z = 0.0f;
- float qw = 1.0f;
- float qx = 0.0f;
- float qy = 0.0f;
- float qz = 0.0f;
- };
-
- struct Color
- {
- byte a = 255;
- byte r = 100;
- byte g = 100;
- byte b = 100;
- };
-
- module InteractionEnableFlags
- {
- const int NONE = 0;
-
- // Make an object selectable
- const int SELECT = 1;
- // Enable the context menu for an object
- const int CONTEXT_MENU = 2;
-
- // Enable translation along the three axes
- const int TRANSLATION_X = 4;
- const int TRANSLATION_Y = 8;
- const int TRANSLATION_Z = 16;
- const int TRANSLATION_LOCAL = 32;
-
- // Enable rotation along the three axes
- const int ROTATION_X = 64;
- const int ROTATION_Y = 128;
- const int ROTATION_Z = 256;
- const int ROTATION_LOCAL = 512;
-
- // Enable scaling along the three axes
- const int SCALING_X = 1024;
- const int SCALING_Y = 2048;
- const int SCALING_Z = 4096;
- const int SCALING_LOCAL = 8192;
-
- // Hide the original object during the transformation
- const int TRANSFORM_HIDE = 16384;
- };
-
- struct InteractionDescription
- {
- int enableFlags = 0;
- Ice::StringSeq contextMenuOptions;
- };
-
- module InteractionFeedbackType
- {
- const int NONE = 0;
-
- const int SELECT = 1;
- const int DESELECT = 2;
-
- const int CONTEXT_MENU_CHOSEN = 3;
-
- const int TRANSFORM = 4;
-
- // Flag to indicate state of the transformation
- const int TRANSFORM_BEGIN_FLAG = 16;
- const int TRANSFORM_DURING_FLAG = 32;
- const int TRANSFORM_END_FLAG = 64;
- };
-
- struct InteractionFeedback
- {
- // The type of interaction that happened (in the lower 4 bits)
- // The higher bits are used for flags specifying more details of the interaction
- int type = 0;
-
- // The element with which the interaction took place
- string component;
- string layer;
- string element;
- // The revision in which the interaction took place
- long revision = 0;
-
- // Chosen context menu entry is only relevant for type == CONTEXT_MENU_CHOSEN
- int chosenContextMenuEntry = 0;
-
- // Applied transformation is only relevant for type == TRANSFORM
- GlobalPose transformation;
- Vector3f scale;
- };
-
- sequence<InteractionFeedback> InteractionFeedbackSeq;
-
- module ElementFlags
- {
- const int NONE = 0;
- const int OVERRIDE_MATERIAL = 1;
- const int HIDDEN = 2;
- };
-
- class Element
- {
- string id;
- InteractionDescription interaction;
-
- GlobalPose pose;
- Vector3f scale;
- Color color;
- int flags = 0;
- };
-
- class ElementPose extends Element
- {
- };
-
- class ElementLine extends Element
- {
- Vector3f from;
- Vector3f to;
-
- float lineWidth = 10.0f;
- };
-
- class ElementBox extends Element
- {
- Vector3f size;
- };
-
- class ElementSphere extends Element
- {
- float radius = 10.0f;
- };
-
- class ElementEllipsoid extends Element
- {
- Vector3f axisLengths;
- Vector3f curvature;
- };
-
- class ElementCylinder extends Element
+ module viz
{
- float height = 10.0f;
- float radius = 10.0f;
+ module data
+ {
+ struct GlobalPose
+ {
+ float x = 0.0f;
+ float y = 0.0f;
+ float z = 0.0f;
+ float qw = 1.0f;
+ float qx = 0.0f;
+ float qy = 0.0f;
+ float qz = 0.0f;
+ };
+
+ struct Color
+ {
+ byte a = 255;
+ byte r = 100;
+ byte g = 100;
+ byte b = 100;
+ };
+
+ module InteractionEnableFlags
+ {
+ const int NONE = 0;
+
+ // Make an object selectable
+ const int SELECT = 1;
+ // Enable the context menu for an object
+ const int CONTEXT_MENU = 2;
+
+ // Enable translation along the three axes
+ const int TRANSLATION_X = 4;
+ const int TRANSLATION_Y = 8;
+ const int TRANSLATION_Z = 16;
+ const int TRANSLATION_LOCAL = 32;
+
+ // Enable rotation along the three axes
+ const int ROTATION_X = 64;
+ const int ROTATION_Y = 128;
+ const int ROTATION_Z = 256;
+ const int ROTATION_LOCAL = 512;
+
+ // Enable scaling along the three axes
+ const int SCALING_X = 1024;
+ const int SCALING_Y = 2048;
+ const int SCALING_Z = 4096;
+ const int SCALING_LOCAL = 8192;
+
+ // Hide the original object during the transformation
+ const int TRANSFORM_HIDE = 16384;
+ };
+
+ struct InteractionDescription
+ {
+ int enableFlags = 0;
+ Ice::StringSeq contextMenuOptions;
+ };
+
+ module InteractionFeedbackType
+ {
+ const int NONE = 0;
+
+ const int SELECT = 1;
+ const int DESELECT = 2;
+
+ const int CONTEXT_MENU_CHOSEN = 3;
+
+ const int TRANSFORM = 4;
+
+ // Flag to indicate state of the transformation
+ const int TRANSFORM_BEGIN_FLAG = 16;
+ const int TRANSFORM_DURING_FLAG = 32;
+ const int TRANSFORM_END_FLAG = 64;
+ };
+
+ struct InteractionFeedback
+ {
+ // The type of interaction that happened (in the lower 4 bits)
+ // The higher bits are used for flags specifying more details of the interaction
+ int type = 0;
+
+ // The element with which the interaction took place
+ string component;
+ string layer;
+ string element;
+ // The revision in which the interaction took place
+ long revision = 0;
+
+ // Chosen context menu entry is only relevant for type == CONTEXT_MENU_CHOSEN
+ int chosenContextMenuEntry = 0;
+
+ // Applied transformation is only relevant for type == TRANSFORM
+ GlobalPose transformation;
+ Vector3f scale;
+ };
+
+ sequence<InteractionFeedback> InteractionFeedbackSeq;
+
+ module ElementFlags
+ {
+ const int NONE = 0;
+ const int OVERRIDE_MATERIAL = 1;
+ const int HIDDEN = 2;
+ };
+
+ class Element
+ {
+ string id;
+ InteractionDescription interaction;
+
+ GlobalPose pose;
+ Vector3f scale;
+ Color color;
+ int flags = 0;
+ };
+
+ class ElementPose extends Element
+ {
+ };
+
+ class ElementLine extends Element
+ {
+ Vector3f from;
+ Vector3f to;
+
+ float lineWidth = 10.0f;
+ };
+
+ class ElementBox extends Element
+ {
+ Vector3f size;
+ };
+
+ class ElementSphere extends Element
+ {
+ float radius = 10.0f;
+ };
+
+ class ElementEllipsoid extends Element
+ {
+ Vector3f axisLengths;
+ Vector3f curvature;
+ };
+
+ class ElementCylinder extends Element
+ {
+ float height = 10.0f;
+ float radius = 10.0f;
+ };
+
+ class ElementCylindroid extends Element
+ {
+ Vector2f axisLengths;
+ Vector2f curvature;
+ float height;
+ };
+
+ class ElementText extends Element
+ {
+ string text;
+ };
+
+ class ElementPolygon extends Element
+ {
+ Vector3fSeq points;
+
+ Color lineColor;
+ float lineWidth = 0.0f;
+ };
+
+ class ElementPath extends Element
+ {
+ Vector3fSeq points;
+
+ float lineWidth = 10.0f;
+ };
+
+ class ElementArrow extends Element
+ {
+ float length = 100.0f;
+ float width = 10.0f;
+ };
+
+ class ElementArrowCircle extends Element
+ {
+ float radius = 100.0f;
+ float completion = 1.0f;
+ float width = 10.0f;
+ };
+
+ struct ColoredPoint
+ {
+ float x;
+ float y;
+ float z;
+ Color color;
+ };
+
+ sequence<ColoredPoint> ColoredPointList;
+
+ class ElementPointCloud extends Element
+ {
+ Ice::ByteSeq points;
+ float transparency = 0.0f;
+ float pointSizeInPixels = 1.0f;
+ };
+
+ sequence<Color> ColorSeq;
+
+ struct Face
+ {
+ int v0 = 0;
+ int v1 = 0;
+ int v2 = 0;
+ int c0 = 0;
+ int c1 = 0;
+ int c2 = 0;
+ };
+
+ sequence<Face> FaceSeq;
+
+ class ElementMesh extends Element
+ {
+ Vector3fSeq vertices;
+ ColorSeq colors;
+ FaceSeq faces;
+ };
+
+ module ModelDrawStyle
+ {
+ const int ORIGINAL = 0;
+ const int COLLISION = 1;
+ const int OVERRIDE_COLOR = 2;
+ };
+
+ class ElementRobot extends Element
+ {
+ string project;
+ string filename;
+ int drawStyle = ModelDrawStyle::ORIGINAL;
+
+ StringFloatDictionary jointValues;
+ };
+
+ class ElementObject extends Element
+ {
+ string project;
+ string filename;
+ int drawStyle = ModelDrawStyle::ORIGINAL;
+ };
+ };
};
-
- class ElementCylindroid extends Element
- {
- Vector2f axisLengths;
- Vector2f curvature;
- float height;
- };
-
- class ElementText extends Element
- {
- string text;
- };
-
- class ElementPolygon extends Element
- {
- Vector3fSeq points;
-
- Color lineColor;
- float lineWidth = 0.0f;
- };
-
- class ElementPath extends Element
- {
- Vector3fSeq points;
-
- float lineWidth = 10.0f;
- };
-
- class ElementArrow extends Element
- {
- float length = 100.0f;
- float width = 10.0f;
- };
-
- class ElementArrowCircle extends Element
- {
- float radius = 100.0f;
- float completion = 1.0f;
- float width = 10.0f;
- };
-
- struct ColoredPoint
- {
- float x;
- float y;
- float z;
- Color color;
- };
-
- sequence<ColoredPoint> ColoredPointList;
-
- class ElementPointCloud extends Element
- {
- Ice::ByteSeq points;
- float transparency = 0.0f;
- float pointSizeInPixels = 1.0f;
- };
-
- sequence<Color> ColorSeq;
-
- struct Face
- {
- int v0 = 0;
- int v1 = 0;
- int v2 = 0;
- int c0 = 0;
- int c1 = 0;
- int c2 = 0;
- };
-
- sequence<Face> FaceSeq;
-
- class ElementMesh extends Element
- {
- Vector3fSeq vertices;
- ColorSeq colors;
- FaceSeq faces;
- };
-
- module ModelDrawStyle
- {
- const int ORIGINAL = 0;
- const int COLLISION = 1;
- const int OVERRIDE_COLOR = 2;
- };
-
- class ElementRobot extends Element
- {
- string project;
- string filename;
- int drawStyle = ModelDrawStyle::ORIGINAL;
-
- StringFloatDictionary jointValues;
- };
-
- class ElementObject extends Element
- {
- string project;
- string filename;
- int drawStyle = ModelDrawStyle::ORIGINAL;
- };
-
-};
-};
};
diff --git a/source/RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.ice b/source/RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.ice
index 779b25dd1902e49409fa9c23576192c7bc5f97ba..ffa1e8c6ea1a700d620d8aed08f4496610d5f106 100644
--- a/source/RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.ice
+++ b/source/RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.ice
@@ -36,7 +36,7 @@ module armarx
/// An optional instance name, chosen by the provider.
string instanceName;
};
+
sequence<ObjectID> ObjectIDSeq;
};
};
-
diff --git a/source/RobotAPI/interface/armem.ice b/source/RobotAPI/interface/armem.ice
index f7621099de52b5a2f15b3f0f6367ec5c19471fb3..a7a5a6a086a2a5d5e1c97e12c22a6736d13e87c0 100644
--- a/source/RobotAPI/interface/armem.ice
+++ b/source/RobotAPI/interface/armem.ice
@@ -11,8 +11,7 @@
module armarx
{
- module armem
- {
+ module armem{
};
};
diff --git a/source/RobotAPI/interface/armem/actions.ice b/source/RobotAPI/interface/armem/actions.ice
index 5bb06fc4c9ec815dba87fef49a31bc13498b66fc..e3882145832b783fb25227236027cfd04e7c92ed 100644
--- a/source/RobotAPI/interface/armem/actions.ice
+++ b/source/RobotAPI/interface/armem/actions.ice
@@ -18,6 +18,7 @@ module armarx
/// Human-readable text displayed to the user.
string text;
};
+
sequence<MenuEntry> MenuEntrySeq;
/**
@@ -35,7 +36,6 @@ module armarx
MenuEntrySeq entries;
};
-
class Menu
{
MenuEntrySeq entries;
@@ -47,12 +47,14 @@ module armarx
{
armem::data::MemoryID id;
};
+
sequence<GetActionsInput> GetActionsInputSeq;
struct GetActionsOutput
{
Menu menu;
};
+
sequence<GetActionsOutput> GetActionsOutputSeq;
struct ExecuteActionInput
@@ -60,6 +62,7 @@ module armarx
armem::data::MemoryID id;
ActionPath actionPath;
};
+
sequence<ExecuteActionInput> ExecuteActionInputSeq;
struct ExecuteActionOutput
@@ -67,6 +70,7 @@ module armarx
bool success = false;
string errorMessage;
};
+
sequence<ExecuteActionOutput> ExecuteActionOutputSeq;
}
}
diff --git a/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice b/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
index baad0e798bc8a3762ec28ac4394b1a3556e3c576..0d1389987e79b1d95b9cb44ea237d2d53039d088 100644
--- a/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
+++ b/source/RobotAPI/interface/armem/addon/LegacyRobotStateMemoryAdapterInterface.ice
@@ -1,8 +1,8 @@
#pragma once
+#include <RobotAPI/interface/core/RobotLocalization.ice>
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
-#include <RobotAPI/interface/core/RobotLocalization.ice>
module armarx
{
@@ -10,7 +10,8 @@ module armarx
{
module robot_state
{
- interface LegacyRobotStateMemoryAdapterInterface extends armarx::KinematicUnitListener, armarx::PlatformUnitListener, armarx::GlobalRobotPoseLocalizationListener
+ interface LegacyRobotStateMemoryAdapterInterface extends armarx::KinematicUnitListener,
+ armarx::PlatformUnitListener, armarx::GlobalRobotPoseLocalizationListener
{
}
}
diff --git a/source/RobotAPI/interface/armem/commit.ice b/source/RobotAPI/interface/armem/commit.ice
index 4a7008293fbe1bd2080ebf808f1bd28621792303..6471c9034262424219e35996cb1cfc12b76627ba 100644
--- a/source/RobotAPI/interface/armem/commit.ice
+++ b/source/RobotAPI/interface/armem/commit.ice
@@ -18,6 +18,7 @@ module armarx
bool clearWhenExists = false;
};
+
sequence<AddSegmentInput> AddSegmentsInput;
struct AddSegmentResult
@@ -27,8 +28,8 @@ module armarx
string errorMessage;
};
- sequence<AddSegmentResult> AddSegmentsResult;
+ sequence<AddSegmentResult> AddSegmentsResult;
struct EntityUpdate
{
@@ -39,6 +40,7 @@ module armarx
float confidence = 1.0;
armarx::core::time::dto::DateTime timeSent;
};
+
sequence<EntityUpdate> EntityUpdateList;
struct EntityUpdateResult
@@ -50,12 +52,14 @@ module armarx
string errorMessage;
};
+
sequence<EntityUpdateResult> EntityUpdateResultList;
struct Commit
{
EntityUpdateList updates;
};
+
struct CommitResult
{
EntityUpdateResultList results;
diff --git a/source/RobotAPI/interface/armem/memory.ice b/source/RobotAPI/interface/armem/memory.ice
index 63801dd4da540ae46fef76e993a862e4979409c7..01577673ef0c6bb0e25bc2f5ae8f573939f96dc7 100644
--- a/source/RobotAPI/interface/armem/memory.ice
+++ b/source/RobotAPI/interface/armem/memory.ice
@@ -7,95 +7,92 @@
module armarx
{
- module armem
+ module armem{module data{struct MemoryID{string memoryName = "";
+ string coreSegmentName = "";
+ string providerSegmentName = "";
+ string entityName = "";
+ armarx::core::time::dto::DateTime timestamp;
+ int instanceIndex = -1;
+}
+
+sequence<MemoryID> MemoryIDs;
+
+module detail
+{
+ class MemoryItem
{
- module data
- {
- struct MemoryID
- {
- string memoryName = "";
- string coreSegmentName = "";
- string providerSegmentName = "";
- string entityName = "";
- armarx::core::time::dto::DateTime timestamp;
- int instanceIndex = -1;
- }
-
- sequence<MemoryID> MemoryIDs;
-
- module detail
- {
- class MemoryItem
- {
- MemoryID id;
- };
- class TypedMemoryContainer extends MemoryItem
- {
- aron::type::dto::GenericType aronType;
- };
- }
-
- /// Ice Twin of `armarx::armem::EntityInstanceMetadata`.
- class EntityInstanceMetadata
- {
- armarx::core::time::dto::DateTime referencedTime;
- armarx::core::time::dto::DateTime sentTime;
- armarx::core::time::dto::DateTime arrivedTime;
- armarx::core::time::dto::DateTime lastAccessedTime;
- long accessed = 0;
-
- float confidence = 1.0;
- };
- /// Ice Twin of `armarx::armem::EntityInstance`.
- class EntityInstance extends detail::MemoryItem
- {
- aron::data::dto::Dict data;
-
- EntityInstanceMetadata metadata;
- };
- sequence<EntityInstance> EntityInstanceSeq;
-
-
- /// Ice Twin of `armarx::armem::EntitySnapshot`.
- class EntitySnapshot extends detail::MemoryItem
- {
- EntityInstanceSeq instances;
- };
- dictionary<armarx::core::time::dto::DateTime, EntitySnapshot> EntityHistory;
-
-
- /// Ice Twin of `armarx::armem::Entity`.
- class Entity extends detail::MemoryItem
- {
- EntityHistory history;
- };
- dictionary<string, Entity> EntityDict;
-
-
- /// Ice Twin of `armarx::armem::ProviderSegment`.
- class ProviderSegment extends detail::TypedMemoryContainer
- {
- EntityDict entities;
- };
- dictionary<string, ProviderSegment> ProviderSegmentDict;
-
-
- /// Ice Twin of `armarx::armem::CoreSegment`.
- class CoreSegment extends detail::TypedMemoryContainer
- {
- ProviderSegmentDict providerSegments;
- };
- dictionary<string, CoreSegment> CoreSegmentDict;
-
-
- /// Ice Twin of `armarx::armem::Memory`.
- class Memory extends detail::MemoryItem
- {
- CoreSegmentDict coreSegments;
- };
- dictionary<string, Memory> MemoryDict;
-
- }
+ MemoryID id;
+ };
+ class TypedMemoryContainer extends MemoryItem
+ {
+ aron::type::dto::GenericType aronType;
};
+}
+
+/// Ice Twin of `armarx::armem::EntityInstanceMetadata`.
+class EntityInstanceMetadata
+{
+ armarx::core::time::dto::DateTime referencedTime;
+ armarx::core::time::dto::DateTime sentTime;
+ armarx::core::time::dto::DateTime arrivedTime;
+ armarx::core::time::dto::DateTime lastAccessedTime;
+ long accessed = 0;
+
+ float confidence = 1.0;
};
+
+/// Ice Twin of `armarx::armem::EntityInstance`.
+class EntityInstance extends detail::MemoryItem
+{
+ aron::data::dto::Dict data;
+
+ EntityInstanceMetadata metadata;
+};
+
+sequence<EntityInstance> EntityInstanceSeq;
+
+/// Ice Twin of `armarx::armem::EntitySnapshot`.
+class EntitySnapshot extends detail::MemoryItem
+{
+ EntityInstanceSeq instances;
+};
+
+dictionary<armarx::core::time::dto::DateTime, EntitySnapshot> EntityHistory;
+
+/// Ice Twin of `armarx::armem::Entity`.
+class Entity extends detail::MemoryItem
+{
+ EntityHistory history;
+};
+
+dictionary<string, Entity> EntityDict;
+
+/// Ice Twin of `armarx::armem::ProviderSegment`.
+class ProviderSegment extends detail::TypedMemoryContainer
+{
+ EntityDict entities;
+};
+
+dictionary<string, ProviderSegment> ProviderSegmentDict;
+
+/// Ice Twin of `armarx::armem::CoreSegment`.
+class CoreSegment extends detail::TypedMemoryContainer
+{
+ ProviderSegmentDict providerSegments;
+};
+
+dictionary<string, CoreSegment> CoreSegmentDict;
+
+/// Ice Twin of `armarx::armem::Memory`.
+class Memory extends detail::MemoryItem
+{
+ CoreSegmentDict coreSegments;
+};
+
+dictionary<string, Memory> MemoryDict;
+}
+}
+;
+}
+;
diff --git a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
index 73d27e0788c89f7d31b0c4f21c546ff276420fd6..099f12730e6f9471c0f3f91f9225fa03a1397233 100644
--- a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
+++ b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
@@ -8,117 +8,113 @@
module armarx
{
- module armem
- {
- module mns
- {
- module dto
- {
- /**
+ module armem{module mns{
+ module dto{/**
* A memory server can implement the reading and/or writing
* memory interface. They should be handled individually.
* Additionally, it can implement prediction or actions interfaces,
* which is currently the case for all working memories.
*/
- struct MemoryServerInterfaces
- {
- server::ReadingMemoryInterface* reading;
- server::WritingMemoryInterface* writing;
- server::PredictingMemoryInterface* prediction;
- server::actions::ActionsInterface* actions;
- };
- dictionary<string, MemoryServerInterfaces> MemoryServerInterfacesMap;
-
+ struct MemoryServerInterfaces{server::ReadingMemoryInterface * reading;
+ server::WritingMemoryInterface* writing;
+ server::PredictingMemoryInterface* prediction;
+ server::actions::ActionsInterface* actions;
+};
+dictionary<string, MemoryServerInterfaces> MemoryServerInterfacesMap;
- /**
+/**
* @brief Register a memory server.
*/
- struct RegisterServerInput
- {
- string name;
- MemoryServerInterfaces server;
+struct RegisterServerInput
+{
+ string name;
+ MemoryServerInterfaces server;
- bool existOk = true;
- };
- struct RegisterServerResult
- {
- bool success;
- string errorMessage;
- };
+ bool existOk = true;
+};
+struct RegisterServerResult
+{
+ bool success;
+ string errorMessage;
+};
- /**
+/**
* @brief Remove a memory server.
*/
- struct RemoveServerInput
- {
- string name;
- bool notExistOk = true;
- };
- struct RemoveServerResult
- {
- bool success;
- string errorMessage;
- };
-
- /**
+struct RemoveServerInput
+{
+ string name;
+ bool notExistOk = true;
+};
+
+struct RemoveServerResult
+{
+ bool success;
+ string errorMessage;
+};
+
+/**
* @brief Resolve a memory name.
*/
- struct ResolveServerInput
- {
- string name;
- };
- struct ResolveServerResult
- {
- bool success;
- string errorMessage;
+struct ResolveServerInput
+{
+ string name;
+};
- MemoryServerInterfaces server;
- };
+struct ResolveServerResult
+{
+ bool success;
+ string errorMessage;
+ MemoryServerInterfaces server;
+};
- /**
+/**
* @brief Get all registered entries.
*/
- struct GetAllRegisteredServersResult
- {
- bool success;
- string errorMessage;
-
- MemoryServerInterfacesMap servers;
- };
+struct GetAllRegisteredServersResult
+{
+ bool success;
+ string errorMessage;
+ MemoryServerInterfacesMap servers;
+};
- /**
+/**
* @brief Wait until a server is registered.
*/
- struct WaitForServerInput
- {
- string name;
- };
- struct WaitForServerResult
- {
- bool success;
- string errorMessage;
+struct WaitForServerInput
+{
+ string name;
+};
- MemoryServerInterfaces server;
- };
- };
+struct WaitForServerResult
+{
+ bool success;
+ string errorMessage;
+ MemoryServerInterfaces server;
+};
+}
+;
- interface MemoryNameSystemInterface
- {
- dto::RegisterServerResult registerServer(dto::RegisterServerInput input);
- dto::RemoveServerResult removeServer(dto::RemoveServerInput input);
- dto::GetAllRegisteredServersResult getAllRegisteredServers();
+interface MemoryNameSystemInterface
+{
+ dto::RegisterServerResult registerServer(dto::RegisterServerInput input);
+ dto::RemoveServerResult removeServer(dto::RemoveServerInput input);
- dto::ResolveServerResult resolveServer(dto::ResolveServerInput input);
+ dto::GetAllRegisteredServersResult getAllRegisteredServers();
- ["amd"] // Asynchronous Method Dispatch
- dto::WaitForServerResult waitForServer(dto::WaitForServerInput input);
- };
- }
+ dto::ResolveServerResult resolveServer(dto::ResolveServerInput input);
- };
+ ["amd"] // Asynchronous Method Dispatch
+ dto::WaitForServerResult
+ waitForServer(dto::WaitForServerInput input);
};
+}
+}
+;
+}
+;
diff --git a/source/RobotAPI/interface/armem/prediction.ice b/source/RobotAPI/interface/armem/prediction.ice
index a2aae725e3ef5a399ccfacf819d07eb56693f8e4..2ede00d2b530dff200cec8a38cfa06ca6bd553d7 100644
--- a/source/RobotAPI/interface/armem/prediction.ice
+++ b/source/RobotAPI/interface/armem/prediction.ice
@@ -15,8 +15,8 @@ module armarx
struct PredictionEngine
{
string engineID;
- }
- sequence<PredictionEngine> PredictionEngineSeq;
+ } sequence<PredictionEngine> PredictionEngineSeq;
+
dictionary<armem::data::MemoryID, PredictionEngineSeq> EngineSupportMap;
// Settings to use for a given prediction (e.g. which engine to use)
@@ -31,8 +31,7 @@ module armarx
{
armem::data::MemoryID snapshotID;
PredictionSettings settings;
- }
- sequence<PredictionRequest> PredictionRequestSeq;
+ } sequence<PredictionRequest> PredictionRequestSeq;
// The result of a single prediction.
// If successful, it contains the predicted data.
@@ -43,8 +42,7 @@ module armarx
armem::data::MemoryID snapshotID;
aron::data::dto::Dict prediction;
- }
- sequence<PredictionResult> PredictionResultSeq;
+ } sequence<PredictionResult> PredictionResultSeq;
};
};
};
diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index c672e42ddf7b5444b37b4a78a7f813f67054f82a..20e6ff1064e14d179fe461899ab66c0876a74864 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/source/RobotAPI/interface/armem/query.ice
@@ -24,6 +24,7 @@ module armarx
class EntityQuery
{
};
+
sequence<EntityQuery> EntityQuerySeq;
module entity
@@ -32,17 +33,20 @@ module armarx
class All extends EntityQuery
{
};
+
/// Get a single snapshot (default for latest).
class Single extends EntityQuery
{
- armarx::core::time::dto::DateTime timestamp; // -1 for latest
+ armarx::core::time::dto::DateTime timestamp; // -1 for latest
};
+
/// Get snapshots based on a time range (default for all).
class TimeRange extends EntityQuery
{
- armarx::core::time::dto::DateTime minTimestamp; // -1 for oldest
- armarx::core::time::dto::DateTime maxTimestamp; // -1 for latest
+ armarx::core::time::dto::DateTime minTimestamp; // -1 for oldest
+ armarx::core::time::dto::DateTime maxTimestamp; // -1 for latest
};
+
/**
* @brief Get snapshots based on an index range (default for all).
*
@@ -55,11 +59,10 @@ module armarx
*/
class IndexRange extends EntityQuery
{
- long first = 0; ///< First index to get.
- long last = -1; ///< Last index to get (inclusive).
+ long first = 0; ///< First index to get.
+ long last = -1; ///< Last index to get (inclusive).
};
-
/**
* @brief Get snapshot(s) around timestamp.
*
@@ -104,41 +107,43 @@ module armarx
* - #maxEntries < 0 => all elements before timestamp
*/
class BeforeTime extends EntityQuery
+
{
armarx::core::time::dto::DateTime timestamp;
long maxEntries = 1;
}
-
}
-
/// Which entities to get from a provider segment?
class ProviderSegmentQuery
{
EntityQuerySeq entityQueries;
};
+
sequence<ProviderSegmentQuery> ProviderSegmentQuerySeq;
module provider
{
class All extends ProviderSegmentQuery
{
};
+
class Single extends ProviderSegmentQuery
{
string entityName;
};
+
class Regex extends ProviderSegmentQuery
{
string entityNameRegex;
};
}
-
/// Which provider segments to get from a core segment?
class CoreSegmentQuery
{
ProviderSegmentQuerySeq providerSegmentQueries;
};
+
sequence<CoreSegmentQuery> CoreSegmentQuerySeq;
module core
@@ -146,23 +151,25 @@ module armarx
class All extends CoreSegmentQuery
{
};
+
class Single extends CoreSegmentQuery
{
string providerSegmentName;
};
+
class Regex extends CoreSegmentQuery
{
string providerSegmentNameRegex;
};
}
-
/// Which core segments to get from a memory?
class MemoryQuery
{
CoreSegmentQuerySeq coreSegmentQueries;
QueryTarget::QueryTargetEnum target = QueryTarget::QueryTargetEnum::WM;
};
+
sequence<MemoryQuery> MemoryQuerySeq;
dictionary<string, MemoryQuerySeq> MemoryQueriesDict;
@@ -171,17 +178,18 @@ module armarx
class All extends MemoryQuery
{
};
+
class Single extends MemoryQuery
{
string coreSegmentName;
};
+
class Regex extends MemoryQuery
{
string coreSegmentNameRegex;
};
}
-
/// Which memories to get from the distributed memory system?
class DistributedMemoryQuery
{
@@ -196,6 +204,7 @@ module armarx
/// If false, no AronData is transferred.
bool withData = true;
};
+
struct Result
{
bool success = false;
diff --git a/source/RobotAPI/interface/armem/server.ice b/source/RobotAPI/interface/armem/server.ice
index 5c7a32c4734ede6124ba8c4e028ce3dcd98bb556..8d541ce6241a7651e65720ff4781e51f959f684a 100644
--- a/source/RobotAPI/interface/armem/server.ice
+++ b/source/RobotAPI/interface/armem/server.ice
@@ -1,6 +1,6 @@
#pragma once
#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
+#include <RobotAPI/interface/armem/server/PredictingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
-#include <RobotAPI/interface/armem/server/PredictingMemoryInterface.ice>
diff --git a/source/RobotAPI/interface/armem/server/ActionsInterface.ice b/source/RobotAPI/interface/armem/server/ActionsInterface.ice
index 8fa6754f7e1e0c961c4a5ff9c9bb317dc9fc3aef..3ba13f1d545d09c12dd22ca24738f8e7d3ea9716 100644
--- a/source/RobotAPI/interface/armem/server/ActionsInterface.ice
+++ b/source/RobotAPI/interface/armem/server/ActionsInterface.ice
@@ -12,8 +12,10 @@ module armarx
{
interface ActionsInterface
{
- actions::data::GetActionsOutputSeq getActions(actions::data::GetActionsInputSeq inputs);
- actions::data::ExecuteActionOutputSeq executeActions(actions::data::ExecuteActionInputSeq inputs);
+ actions::data::GetActionsOutputSeq getActions(
+ actions::data::GetActionsInputSeq inputs);
+ actions::data::ExecuteActionOutputSeq executeActions(
+ actions::data::ExecuteActionInputSeq inputs);
};
}
}
diff --git a/source/RobotAPI/interface/armem/server/MemoryInterface.ice b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
index ddfcb5b2bdbcdfe8685ce6d4380d7fdbe283396a..e40a0ade92eed16521b43a0c9edb6fb65e452c13 100644
--- a/source/RobotAPI/interface/armem/server/MemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
@@ -1,15 +1,12 @@
#pragma once
-#include <RobotAPI/interface/armem/server/ReplayingMemoryInterface.ice>
-#include <RobotAPI/interface/armem/server/RecordingMemoryInterface.ice>
-
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
+#include <RobotAPI/interface/armem/server/ActionsInterface.ice>
+#include <RobotAPI/interface/armem/server/PredictingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/server/RecordingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/server/ReplayingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
-#include <RobotAPI/interface/armem/server/PredictingMemoryInterface.ice>
-
-#include <RobotAPI/interface/armem/server/ActionsInterface.ice>
-
-#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
module armarx
@@ -18,22 +15,15 @@ module armarx
{
module server
{
- interface LongTermMemoryInterface extends ReplayingMemoryInterface, RecordingMemoryInterface
- {
- };
+ interface LongTermMemoryInterface extends ReplayingMemoryInterface,
+ RecordingMemoryInterface{};
- interface WorkingMemoryInterface extends ReadingMemoryInterface, WritingMemoryInterface
- {
- };
+ interface WorkingMemoryInterface extends ReadingMemoryInterface,
+ WritingMemoryInterface{};
- interface MemoryInterface extends
- WorkingMemoryInterface,
- LongTermMemoryInterface,
- PredictingMemoryInterface,
- actions::ActionsInterface,
- client::MemoryListenerInterface
- {
- };
+ interface MemoryInterface extends WorkingMemoryInterface, LongTermMemoryInterface,
+ PredictingMemoryInterface, actions::ActionsInterface,
+ client::MemoryListenerInterface{};
};
};
};
diff --git a/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice b/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
index fa9d74f160dc12e05fd6b1395d03b85b36384bea..fe5eb0ecacd875e3478689baaeb30b3178fba7ec 100644
--- a/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/ObjectMemoryInterface.ice
@@ -24,9 +24,8 @@
#pragma once
#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
-
-#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice>
#include <RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.ice>
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice>
module armarx
@@ -37,26 +36,21 @@ module armarx
{
interface ObjectInstanceSegmentInterface extends
- armarx::objpose::ObjectPoseStorageInterface
- {
+ armarx::objpose::ObjectPoseStorageInterface{
- };
+ };
interface FamiliarObjectInstanceSegmentInterface extends
- armarx::objpose::FamiliarObjectPoseStorageInterface
- {
+ armarx::objpose::FamiliarObjectPoseStorageInterface{
- };
+ };
- interface ObjectMemoryInterface extends
- MemoryInterface
- , ObjectInstanceSegmentInterface
- , FamiliarObjectInstanceSegmentInterface
+ interface ObjectMemoryInterface extends MemoryInterface, ObjectInstanceSegmentInterface,
+ FamiliarObjectInstanceSegmentInterface
{
void reloadKnownObjectClasses();
};
-
};
};
};
diff --git a/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice b/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice
index aeae3745854d595ba7799a399df43cf2e33922b7..a69676fa686bf82c31a1cf92c96bdfc8ed406813 100644
--- a/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/PredictingMemoryInterface.ice
@@ -5,24 +5,23 @@
module armarx
{
- module armem
- {
- module server
+ module armem
{
- interface PredictingMemoryInterface
- {
+ module server{interface PredictingMemoryInterface{
/* Request multiple predictions from the memory.
* The timestamp to requst a prediction for is encoded in the MemoryIDs.
*/
- prediction::data::PredictionResultSeq
- predict(prediction::data::PredictionRequestSeq requests);
+ prediction::data::PredictionResultSeq predict(
+ prediction::data::PredictionRequestSeq requests);
- /* Get a map from memory segments to the prediction engines they support.
+ /* Get a map from memory segments to the prediction engines they support.
* Best used with the container_map operations to easily retrieve
* the available engine selection for fully evaluated MemoryIDs.
*/
- prediction::data::EngineSupportMap getAvailableEngines();
- }
- };
- };
+ prediction::data::EngineSupportMap getAvailableEngines();
+ }
};
+}
+;
+}
+;
diff --git a/source/RobotAPI/interface/armem/server/ReadingMemoryInterface.ice b/source/RobotAPI/interface/armem/server/ReadingMemoryInterface.ice
index 8a0d185d6092ada17170d652850633d52a5eb89d..c580aeb45efafc5325ee8d07d12e3e03f0603ea4 100644
--- a/source/RobotAPI/interface/armem/server/ReadingMemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/ReadingMemoryInterface.ice
@@ -12,7 +12,6 @@ module armarx
{
module dto
{
-
}
interface ReadingMemoryInterface
diff --git a/source/RobotAPI/interface/armem/server/RecordingMemoryInterface.ice b/source/RobotAPI/interface/armem/server/RecordingMemoryInterface.ice
index bd831fe2294dd8476c6fb3b3a73fb776c2acf2d2..3217e3338a42d1224cc3b8b4ff8a945222bf2449 100644
--- a/source/RobotAPI/interface/armem/server/RecordingMemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/RecordingMemoryInterface.ice
@@ -5,7 +5,7 @@
module armarx
{
module armem
- {
+ {
module server
{
module dto
@@ -45,25 +45,30 @@ module armarx
RecordingMode::RecordingModeEnum mode;
float frequency;
};
- dictionary <armarx::armem::data::MemoryID, RecordingModeConfiguration> Configuration;
- struct StartRecordResult {
+ dictionary<armarx::armem::data::MemoryID, RecordingModeConfiguration> Configuration;
+
+ struct StartRecordResult
+ {
bool success;
string errorMessage;
};
- struct StopRecordResult {
+ struct StopRecordResult
+ {
bool success;
string errorMessage;
};
- struct RecordStatusResult {
+ struct RecordStatusResult
+ {
bool success;
string errorMessage;
RecordStatus status;
};
- struct StartRecordInput {
+ struct StartRecordInput
+ {
armarx::core::time::dto::DateTime executionTime;
string recordingID;
armarx::core::time::dto::DateTime startTime;
diff --git a/source/RobotAPI/interface/armem/server/ReplayingMemoryInterface.ice b/source/RobotAPI/interface/armem/server/ReplayingMemoryInterface.ice
index d0825d8385f07e35ba7aca6ac0c95a5a1a43a767..81e1204355f0c98bd71a0f4517d9707d35bda0a9 100644
--- a/source/RobotAPI/interface/armem/server/ReplayingMemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/ReplayingMemoryInterface.ice
@@ -6,11 +6,10 @@
module armarx
{
module armem
- {
+ {
module server
{
- interface ReplayingMemoryInterface
- {
+ interface ReplayingMemoryInterface{
};
};
diff --git a/source/RobotAPI/interface/armem/structure.ice b/source/RobotAPI/interface/armem/structure.ice
index 2331a9e9be618c4c7c0b24c55fd34340a3f71eab..adfc9ce9ccfe5ce612d00261d52d083e63d0e693 100644
--- a/source/RobotAPI/interface/armem/structure.ice
+++ b/source/RobotAPI/interface/armem/structure.ice
@@ -10,10 +10,12 @@ module armarx
{
module data
{
- struct GetServerStructureResult {
+ struct GetServerStructureResult
+ {
bool success;
string errorMessage;
- armarx::armem::data::Memory serverStructure; //structure of the Server without data (all Segments...)
+ armarx::armem::data::Memory
+ serverStructure; //structure of the Server without data (all Segments...)
};
};
};
diff --git a/source/RobotAPI/interface/aron.ice b/source/RobotAPI/interface/aron.ice
index 2bbcc667f3d5f065d7c10e3c94f7df32739fe8c4..704ed53fc6934c1dfda92ac0db34a7df88547e5c 100644
--- a/source/RobotAPI/interface/aron.ice
+++ b/source/RobotAPI/interface/aron.ice
@@ -4,8 +4,7 @@
module armarx
{
- module aron
- {
+ module aron{
};
};
diff --git a/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice b/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice
index 924f40c6d8664123c822eb9f7eb338d8d50b52d5..aad852c198ecf136ea0f0b100e1c63bf8c67b6b1 100644
--- a/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice
+++ b/source/RobotAPI/interface/aron/test/AronConversionTestInterface.ice
@@ -26,36 +26,41 @@
#include <RobotAPI/interface/aron/Aron.ice>
-module armarx { module aron { module test
+module armarx
{
+ module aron
+ {
+ module test
+ {
-module dto
-{
+ module dto
+ {
- struct TestAronConversionRequest
- {
- string aronClassName;
- ::armarx::aron::data::dto::Dict probe;
- };
- struct TestAronConversionResponse
- {
- bool success;
- string errorMessage;
+ struct TestAronConversionRequest
+ {
+ string aronClassName;
+ ::armarx::aron::data::dto::Dict probe;
+ };
- ::armarx::aron::data::dto::Dict probe;
- };
+ struct TestAronConversionResponse
+ {
+ bool success;
+ string errorMessage;
-};
+ ::armarx::aron::data::dto::Dict probe;
+ };
+ };
-module dti
-{
+ module dti
+ {
- interface AronConversionTestInterface
- {
- dto::TestAronConversionResponse testAronConversion(dto::TestAronConversionRequest req);
+ interface AronConversionTestInterface
+ {
+ dto::TestAronConversionResponse testAronConversion(
+ dto::TestAronConversionRequest req);
+ };
+ };
+ };
};
-
};
-
-};};};
diff --git a/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice b/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice
index 13741869c90df9117d6f326036ae92b6e6a24ce0..053159c440c59eb73b0626783d39ab114c5fba35 100644
--- a/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice
+++ b/source/RobotAPI/interface/components/CartesianPositionControlInterface.ice
@@ -37,6 +37,5 @@ module armarx
void resetFTOffset(string side);
int getFTTresholdExceededCounter(string side);
void emulateFTTresholdExceeded(string side);
-
};
};
diff --git a/source/RobotAPI/interface/components/FrameTrackingInterface.ice b/source/RobotAPI/interface/components/FrameTrackingInterface.ice
index ba04341101e0102c622216b9a35f0a77028ca333..5cf9f29f88888f9fbacf26d7da58593e7af23441 100644
--- a/source/RobotAPI/interface/components/FrameTrackingInterface.ice
+++ b/source/RobotAPI/interface/components/FrameTrackingInterface.ice
@@ -30,16 +30,15 @@ module armarx
{
void enableTracking(bool enable);
void setFrame(string frameName);
- ["cpp:const"]
- idempotent
- string getFrame();
+ ["cpp:const"] idempotent string getFrame();
void lookAtFrame(string frameName);
void lookAtPointInGlobalFrame(Vector3f point);
bool isLookingAtPointInGlobalFrame(Vector3f point, float max_diff);
void lookAtPointInRobotFrame(Vector3f point);
- void scanInConfigurationSpace(float yawFrom, float yawTo, ::Ice::FloatSeq pitchValues, float velocity);
+ void scanInConfigurationSpace(
+ float yawFrom, float yawTo, ::Ice::FloatSeq pitchValues, float velocity);
void scanInWorkspace(::armarx::Vector3fSeq points, float maxVelocity, float acceleration);
void moveJointsTo(float yaw, float pitch);
};
diff --git a/source/RobotAPI/interface/components/NaturalIKInterface.ice b/source/RobotAPI/interface/components/NaturalIKInterface.ice
index 40a8d2fc22ccf48e92d9d43f056413d42d50ea03..57eddcc68b7c5566c1539610591947c42f064b54 100644
--- a/source/RobotAPI/interface/components/NaturalIKInterface.ice
+++ b/source/RobotAPI/interface/components/NaturalIKInterface.ice
@@ -24,6 +24,7 @@
#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
#include <ArmarXCore/interface/serialization/Eigen.ice>
+
#include <RobotAPI/interface/aron.ice>
module armarx
@@ -36,6 +37,7 @@ module armarx
interface NaturalIKInterface
{
- NaturalIKResult solveIK(string side, Eigen::Matrix4f target, bool setOrientation, aron::data::dto::Dict args);
+ NaturalIKResult solveIK(
+ string side, Eigen::Matrix4f target, bool setOrientation, aron::data::dto::Dict args);
};
};
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice
index 42ed92db3ba81fe2a0255bab1e9a98c9f386991a..743747c2b5fc0b443c8f07e5d81f53feeb2068b4 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DSObstacleAvoidanceInterface.ice
@@ -48,19 +48,15 @@ module armarx
double weight_power;
double agent_safety_margin;
};
-
};
- interface DSObstacleAvoidanceInterface extends
- ObstacleAvoidanceInterface, ObstacleDetectionInterface
+ interface DSObstacleAvoidanceInterface extends ObstacleAvoidanceInterface,
+ ObstacleDetectionInterface
{
- dsobstacleavoidance::Config getConfig();
-
- void
- writeDebugPlots(string filename);
+ dsobstacleavoidance::Config getConfig();
+ void writeDebugPlots(string filename);
};
-
};
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
index df0f5596b78a783c94e6ae21fc1a0cfdf0e5a16e..10299e3e4cdf1c7cf7673bcde661f5bb313181fb 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -44,27 +44,22 @@ module armarx
interface DynamicObstacleManagerInterface
{
- void
- add_decayable_obstacle(Eigen::Vector2f e_origin, float e_rx, float e_ry, float e_yaw);
+ void add_decayable_obstacle(Eigen::Vector2f e_origin, float e_rx, float e_ry, float e_yaw);
- void
- add_decayable_line_segment(Eigen::Vector2f line_start, Eigen::Vector2f line_end);
+ void add_decayable_line_segment(Eigen::Vector2f line_start, Eigen::Vector2f line_end);
- void
- add_decayable_line_segments(dynamicobstaclemanager::LineSegments lines);
+ void add_decayable_line_segments(dynamicobstaclemanager::LineSegments lines);
- void
- remove_all_decayable_obstacles();
+ void remove_all_decayable_obstacles();
- void
- directly_update_obstacle(string name, Eigen::Vector2f e_origin, float e_rx, float e_ry, float e_yaw);
+ void directly_update_obstacle(
+ string name, Eigen::Vector2f e_origin, float e_rx, float e_ry, float e_yaw);
- void
- remove_obstacle(string name);
+ void remove_obstacle(string name);
void wait_unitl_obstacles_are_ready();
- float distanceToObstacle(Eigen::Vector2f agentPosition, float safetyRadius, Eigen::Vector2f goal);
+ float distanceToObstacle(
+ Eigen::Vector2f agentPosition, float safetyRadius, Eigen::Vector2f goal);
};
};
-
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.ice
index 529f4201fb92327ca9b26262e335b06036044441..872c812cf312c9c29df9112861bb713900794485 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleAvoidanceInterface.ice
@@ -41,16 +41,12 @@ module armarx
Eigen::Vector3f goal;
float safety_margin;
};
-
};
interface ObstacleAvoidanceInterface
{
- Eigen::Vector3f
- modulateVelocity(obstacleavoidance::Agent agent);
-
+ Eigen::Vector3f modulateVelocity(obstacleavoidance::Agent agent);
};
-
};
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.ice
index 0a0435b203c00767deeb7e52fc4af93244c3a080..0d53f21002358e210d6702306cb82d91c8bdb528 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/ObstacleDetectionInterface.ice
@@ -32,48 +32,41 @@ module armarx
{
module obstacledetection
- {
-
- struct Obstacle
- {
- string name;
- double posX = 0;
- double posY = 0;
- double posZ = 0;
- double yaw = 0;
- double axisLengthX = 0;
- double axisLengthY = 0;
- double axisLengthZ = 0;
- double refPosX = 0;
- double refPosY = 0;
- double refPosZ = 0;
- double safetyMarginX = 0;
- double safetyMarginY = 0;
- double safetyMarginZ = 0;
- double curvatureX = 1;
- double curvatureY = 1;
- double curvatureZ = 1;
- };
+ {
+
+ struct Obstacle
+ {
+ string name;
+ double posX = 0;
+ double posY = 0;
+ double posZ = 0;
+ double yaw = 0;
+ double axisLengthX = 0;
+ double axisLengthY = 0;
+ double axisLengthZ = 0;
+ double refPosX = 0;
+ double refPosY = 0;
+ double refPosZ = 0;
+ double safetyMarginX = 0;
+ double safetyMarginY = 0;
+ double safetyMarginZ = 0;
+ double curvatureX = 1;
+ double curvatureY = 1;
+ double curvatureZ = 1;
+ };
sequence<obstacledetection::Obstacle> ObstacleList;
+ };
- };
-
-
- interface ObstacleDetectionInterface
- {
- void
- updateObstacle(obstacledetection::Obstacle obstacle);
-
- void
- removeObstacle(string obstacle_name);
- obstacledetection::ObstacleList
- getObstacles();
+ interface ObstacleDetectionInterface
+ {
+ void updateObstacle(obstacledetection::Obstacle obstacle);
- void
- updateEnvironment();
+ void removeObstacle(string obstacle_name);
- };
+ obstacledetection::ObstacleList getObstacles();
+ void updateEnvironment();
+ };
};
diff --git a/source/RobotAPI/interface/components/RobotHealthInterface.ice b/source/RobotAPI/interface/components/RobotHealthInterface.ice
index 0832146d934a45cf35dd2d42591a4b4949f8d15d..45c5660ae3f4c6082a522f52251e64a7cd1bad41 100644
--- a/source/RobotAPI/interface/components/RobotHealthInterface.ice
+++ b/source/RobotAPI/interface/components/RobotHealthInterface.ice
@@ -30,7 +30,9 @@ module armarx
enum RobotHealthState
{
- HealthOK, HealthWarning, HealthError
+ HealthOK,
+ HealthWarning,
+ HealthError
};
struct RobotHealthHeartbeatArgs
@@ -54,7 +56,7 @@ module armarx
void resetRequiredTags();
void heartbeat(string identifier, armarx::core::time::dto::DateTime referenceTime);
-
+
string getTopicName();
};
@@ -75,7 +77,7 @@ module armarx
armarx::core::time::dto::DateTime lastReferenceTimestamp;
- //< Time delta to now() when arrived at heart beat component
+ //< Time delta to now() when arrived at heart beat component
armarx::core::time::dto::Duration timeSinceLastArrival;
//< Time delta to reference timestamp sent by component
@@ -83,7 +85,7 @@ module armarx
armarx::core::time::dto::Duration timeSyncDelayAndIce;
- bool required; //
+ bool required; //
bool enabled;
armarx::core::time::dto::Duration maximumCycleTimeWarning;
@@ -100,7 +102,6 @@ module armarx
RobotHealthEntries entries;
};
-
interface RobotHealthComponentInterface extends RobotHealthInterface
{
RobotHealthInfo getSummary();
diff --git a/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice b/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
index 7807f84419960a8219f5a809feccf66e08d6f76a..e87849a93da2369ea5713121c8d50d2a5b4177dc 100644
--- a/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
+++ b/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
@@ -22,10 +22,11 @@
#pragma once
-#include <RobotAPI/interface/core/Trajectory.ice>
-#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.ice>
#include <ArmarXCore/interface/serialization/Eigen.ice>
+
#include <RobotAPI/interface/aron.ice>
+#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.ice>
module armarx
{
diff --git a/source/RobotAPI/interface/components/ViewSelectionInterface.ice b/source/RobotAPI/interface/components/ViewSelectionInterface.ice
old mode 100755
new mode 100644
index 9ba19073af9c83d0a63fc9ea419afbf192cf4777..606bd193c5f32b66887d549aa62b3b98eded02e3
--- a/source/RobotAPI/interface/components/ViewSelectionInterface.ice
+++ b/source/RobotAPI/interface/components/ViewSelectionInterface.ice
@@ -23,19 +23,18 @@
#pragma once
-#include <RobotAPI/interface/core/PoseBase.ice>
-#include <RobotAPI/interface/core/FramedPoseBase.ice>
-
-#include <ArmarXCore/interface/observers/Timestamp.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+#include <RobotAPI/interface/core/PoseBase.ice>
module armarx
{
const int DEFAULT_VIEWTARGET_PRIORITY = 50;
- ["cpp:virtual"]
- class ViewTargetBase
+ ["cpp:virtual"] class ViewTargetBase
{
armarx::FramedPositionBase target;
@@ -50,8 +49,7 @@ module armarx
string source;
};
- ["cpp:virtual"]
- class SaliencyMapBase
+ ["cpp:virtual"] class SaliencyMapBase
{
string name;
@@ -83,16 +81,10 @@ module armarx
};
-
interface ViewSelectionObserver
{
void onActivateAutomaticViewSelection();
void onDeactivateAutomaticViewSelection();
void nextViewTarget(long timestamp);
};
-
};
-
-
-
-
diff --git a/source/RobotAPI/interface/core/BlackWhitelist.ice b/source/RobotAPI/interface/core/BlackWhitelist.ice
index dc93760a9530067a1254b35770783e87559b5ff6..74f7df313de2e7146c123836ce3e56850c4c3ed0 100644
--- a/source/RobotAPI/interface/core/BlackWhitelist.ice
+++ b/source/RobotAPI/interface/core/BlackWhitelist.ice
@@ -41,7 +41,6 @@ module armarx
StringListUpdate whitelist;
};
-
interface BlackWhitelistInterface
{
void updateBlackWhitelist(BlackWhitelistUpdate update);
@@ -52,4 +51,3 @@ module armarx
void updateBlackWhitelist(BlackWhitelistUpdate update);
}
};
-
diff --git a/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice b/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
index a876489d1f7f4042817f61c8b2fe751b97f06690..9dfd12d610a6fc6e89758d8ca4c53fd650bb0312 100644
--- a/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
+++ b/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
@@ -29,30 +29,28 @@ module armarx
{
struct CartesianNaturalPositionControllerConfig
{
- float maxPositionAcceleration = 500;
- float maxOrientationAcceleration = 1;
- float maxNullspaceAcceleration = 2;
+ float maxPositionAcceleration = 500;
+ float maxOrientationAcceleration = 1;
+ float maxNullspaceAcceleration = 2;
Ice::FloatSeq maxJointVelocity;
Ice::FloatSeq maxNullspaceVelocity;
Ice::FloatSeq jointCosts;
- float jointLimitAvoidanceKp = 0.1;
- float elbowKp = 1;
- float nullspaceTargetKp = 1;
+ float jointLimitAvoidanceKp = 0.1;
+ float elbowKp = 1;
+ float nullspaceTargetKp = 1;
- float thresholdOrientationNear = 0.1;
- float thresholdOrientationReached = 0.05;
- float thresholdPositionNear = 50;
- float thresholdPositionReached = 5;
+ float thresholdOrientationNear = 0.1;
+ float thresholdOrientationReached = 0.05;
+ float thresholdPositionNear = 50;
+ float thresholdPositionReached = 5;
float maxTcpPosVel = 80;
float maxTcpOriVel = 1;
float maxElbPosVel = 80;
- float KpOri = 1;
- float KpPos = 1;
+ float KpOri = 1;
+ float KpPos = 1;
};
};
-
-
diff --git a/source/RobotAPI/interface/core/CartesianPositionControllerConfig.ice b/source/RobotAPI/interface/core/CartesianPositionControllerConfig.ice
index 0c1418a257091bead62d86a0629967ba8bfde77b..f28cd8108ce083b5c35f2861c72e45bd1986155d 100644
--- a/source/RobotAPI/interface/core/CartesianPositionControllerConfig.ice
+++ b/source/RobotAPI/interface/core/CartesianPositionControllerConfig.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <ArmarXCore/interface/observers/VariantBase.ice>
#include <ArmarXCore/interface/observers/Filters.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
module armarx
{
@@ -42,8 +42,4 @@ module armarx
float thresholdPositionNear = -1;
float thresholdPositionReached = -1;
};
-
-
-
};
-
diff --git a/source/RobotAPI/interface/core/CartesianSelection.ice b/source/RobotAPI/interface/core/CartesianSelection.ice
index 6fc3d9b0897a8195766ba58aca46fb2b781b6daa..444ca24f869d773654be1501226b87b167bab11b 100644
--- a/source/RobotAPI/interface/core/CartesianSelection.ice
+++ b/source/RobotAPI/interface/core/CartesianSelection.ice
@@ -33,6 +33,4 @@ module armarx
eAll = 15
};
};
-
};
-
diff --git a/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice b/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice
index 5a509ea7bb2b6cf16c5e71ab4928116ef278dbca..411649209d0ffd46623934bfa61f029e7c0fdd03 100644
--- a/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice
+++ b/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice
@@ -26,23 +26,21 @@ module armarx
{
struct CartesianWaypointControllerConfig
{
- float maxPositionAcceleration = 500;
- float maxOrientationAcceleration = 1;
- float maxNullspaceAcceleration = 2;
+ float maxPositionAcceleration = 500;
+ float maxOrientationAcceleration = 1;
+ float maxNullspaceAcceleration = 2;
- float kpJointLimitAvoidance = 1;
- float jointLimitAvoidanceScale = 2;
+ float kpJointLimitAvoidance = 1;
+ float jointLimitAvoidanceScale = 2;
- float thresholdOrientationNear = 0.1;
- float thresholdOrientationReached = 0.05;
- float thresholdPositionNear = 50;
- float thresholdPositionReached = 5;
+ float thresholdOrientationNear = 0.1;
+ float thresholdOrientationReached = 0.05;
+ float thresholdPositionNear = 50;
+ float thresholdPositionReached = 5;
float maxOriVel = 1;
float maxPosVel = 80;
- float kpOri = 1;
- float kpPos = 1;
+ float kpOri = 1;
+ float kpPos = 1;
};
};
-
-
diff --git a/source/RobotAPI/interface/core/FTSensorValue.ice b/source/RobotAPI/interface/core/FTSensorValue.ice
index ed93a5259aff4a2e22d801387a88032ae22e4eb7..d2d912c1254f7f6098209d6bb9a35017078134e0 100644
--- a/source/RobotAPI/interface/core/FTSensorValue.ice
+++ b/source/RobotAPI/interface/core/FTSensorValue.ice
@@ -33,5 +33,4 @@ module armarx
Eigen::Vector3f force;
Eigen::Vector3f torque;
};
-
};
diff --git a/source/RobotAPI/interface/core/FramedPoseBase.ice b/source/RobotAPI/interface/core/FramedPoseBase.ice
index b4947429a1e360c808a896b6cf38bd8008259f87..5bea2ebc15a94a2c1ef6bd4a26f8c6f076a3f3e2 100644
--- a/source/RobotAPI/interface/core/FramedPoseBase.ice
+++ b/source/RobotAPI/interface/core/FramedPoseBase.ice
@@ -22,64 +22,61 @@
#pragma once
-#include <ArmarXCore/interface/observers/VariantBase.ice>
#include <ArmarXCore/interface/observers/Filters.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+
#include <RobotAPI/interface/core/PoseBase.ice>
module armarx
{
- /**
+ /**
* [FramedDirectionBase] defines a vector with regard to an agent and a frame within the agent.
* If agent is empty and frame is set to armarx::GlobalFrame, vector is defined in global frame.
* @param frame Name of frame.
* @param frame Name of agent.
*/
- ["cpp:virtual"]
- class FramedDirectionBase extends Vector3Base
+ ["cpp:virtual"] class FramedDirectionBase extends Vector3Base
{
string frame;
string agent;
};
- /**
+ /**
* [FramedPoseBase] defines a pose with regard to an agent and a frame within the agent.
* If agent is empty and frame is set to armarx::GlobalFrame, pose is defined in global frame.
* @param frame Name of frame.
* @param frame Name of agent.
*/
- ["cpp:virtual"]
- class FramedPoseBase extends PoseBase
+ ["cpp:virtual"] class FramedPoseBase extends PoseBase
{
string frame;
string agent;
};
-
- /**
+
+ /**
* [FramedPositionBase] defines a position with regard to an agent and a frame within the agent.
* If agent is empty and frame is set to armarx::GlobalFrame, position is defined in global frame.
* @param frame Name of frame.
* @param frame Name of agent.
*/
- ["cpp:virtual"]
- class FramedPositionBase extends Vector3Base
+ ["cpp:virtual"] class FramedPositionBase extends Vector3Base
{
string frame;
string agent;
};
- /**
+ /**
* [FramedOrientationBase] defines an orientation in quaternion with regard to an agent and a frame within the agent.
* If agent is empty and frame is set to armarx::GlobalFrame, orientation in quaternion is defined in global frame.
* @param frame Name of frame.
* @param frame Name of agent.
*/
- ["cpp:virtual"]
- class FramedOrientationBase extends QuaternionBase
+ ["cpp:virtual"] class FramedOrientationBase extends QuaternionBase
{
string frame;
string agent;
};
- /**
+ /**
* FramedDirectionMap is a map of FramedDirectionBases and corresponding agent nodes identified by name.
* @see FramedDirectionBase
*/
@@ -89,41 +86,30 @@ module armarx
* @see FramedPositionBase
*/
dictionary<string, FramedPositionBase> FramedPositionMap;
-
- /**
+
+ /**
* PoseMedianFilterBase filters poses with a median filter.
*/
- ["cpp:virtual"]
- class PoseMedianFilterBase extends MedianFilterBase
- {
+ ["cpp:virtual"] class PoseMedianFilterBase extends MedianFilterBase {
};
/**
* PoseMedianOffsetFilterBase filters poses with median filter and subsequent offset filter.
*/
- ["cpp:virtual"]
- class PoseMedianOffsetFilterBase extends PoseMedianFilterBase
- {
+ ["cpp:virtual"] class PoseMedianOffsetFilterBase extends PoseMedianFilterBase {
};
-
-
- ["cpp:virtual"]
- class MedianDerivativeFilterV3Base extends PoseMedianFilterBase
- {
- int offsetWindowSize;
- };
+
+
+ ["cpp:virtual"] class MedianDerivativeFilterV3Base extends PoseMedianFilterBase
+ { int offsetWindowSize; };
/**
* PoseAverageFilterBase filters poses with an average filter.
*/
- ["cpp:virtual"]
- class PoseAverageFilterBase extends AverageFilterBase
- {
+ ["cpp:virtual"] class PoseAverageFilterBase extends AverageFilterBase {
};
-
};
-
diff --git a/source/RobotAPI/interface/core/GeometryBase.ice b/source/RobotAPI/interface/core/GeometryBase.ice
index 3a3208019ebb02754b3cc2061ea9aea1e92fe9f5..b2faa5f18f60381669be3c81cf6f8db4889edf96 100644
--- a/source/RobotAPI/interface/core/GeometryBase.ice
+++ b/source/RobotAPI/interface/core/GeometryBase.ice
@@ -1,46 +1,47 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/serialization/Eigen.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
+
/**
* Messages that can also be found in ROS package 'geometry_msgs'
* http://docs.ros.org/en/jade/api/geometry_msgs/html/index-msg.html
*/
module armarx
-{
-
+{
+
struct FrameHeader
- {
- string parentFrame; // base frame
- string frame; // child frame
+ {
+ string parentFrame; // base frame
+ string frame; // child frame
- string agent; // the robot
+ string agent; // the robot
long timestampInMicroSeconds = 0;
- }
-
- struct TransformStamped
- {
- FrameHeader header;
- Eigen::Matrix4f transform; // [mm]
- }
-
- struct Twist
- {
- // Linear (x,y,z) and angular (roll, pitch, yaw) velocities
- Eigen::Vector3f linear; // [mm/s]
- Eigen::Vector3f angular; // [rad/s]
- }
-
- struct TwistStamped
- {
- FrameHeader header;
- Twist twist;
- }
+ }
+
+ struct TransformStamped
+
+ {
+ FrameHeader header;
+ Eigen::Matrix4f transform; // [mm]
+ }
+
+ struct Twist
+ {
+ // Linear (x,y,z) and angular (roll, pitch, yaw) velocities
+ Eigen::Vector3f linear; // [mm/s]
+ Eigen::Vector3f angular; // [rad/s]
+ }
+
+ struct TwistStamped
+ {
+ FrameHeader header;
+ Twist twist;
+ }
} // module armarx
\ No newline at end of file
diff --git a/source/RobotAPI/interface/core/LinkedPoseBase.ice b/source/RobotAPI/interface/core/LinkedPoseBase.ice
index 2aaa1b30c2f78337a74852141273f3e400bb89aa..985bceac1b1e7fc6104e8d83a3ed93e32c9509ec 100644
--- a/source/RobotAPI/interface/core/LinkedPoseBase.ice
+++ b/source/RobotAPI/interface/core/LinkedPoseBase.ice
@@ -27,11 +27,10 @@
module armarx
{
- /**
+ /**
* [LinkedPoseBase]
*/
- ["cpp:virtual"]
- class LinkedPoseBase extends FramedPoseBase
+ ["cpp:virtual"] class LinkedPoseBase extends FramedPoseBase
{
SharedRobotInterface* referenceRobot;
/**
@@ -40,12 +39,11 @@ module armarx
*/
void changeFrame(string newFrame);
};
-
- /**
+
+ /**
* [LinkedDirectionBase]
*/
- ["cpp:virtual"]
- class LinkedDirectionBase extends FramedDirectionBase
+ ["cpp:virtual"] class LinkedDirectionBase extends FramedDirectionBase
{
SharedRobotInterface* referenceRobot;
/**
@@ -55,4 +53,3 @@ module armarx
void changeFrame(string newFrame);
};
};
-
diff --git a/source/RobotAPI/interface/core/OrientedPoint.ice b/source/RobotAPI/interface/core/OrientedPoint.ice
index 088d4deda1a6bcdb94b25cf37a947c6c3a6bd2b7..f965fadc5baa6be03e402d6dc0ef2f75696bb664 100644
--- a/source/RobotAPI/interface/core/OrientedPoint.ice
+++ b/source/RobotAPI/interface/core/OrientedPoint.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <ArmarXCore/interface/observers/VariantBase.ice>
#include <ArmarXCore/interface/observers/Filters.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
module armarx
{
@@ -47,13 +47,9 @@ module armarx
* @param frame Name of frame.
* @param frame Name of agent.
*/
- ["cpp:virtual"]
- class FramedOrientedPointBase extends OrientedPointBase
+ ["cpp:virtual"] class FramedOrientedPointBase extends OrientedPointBase
{
string frame;
string agent;
};
-
-
};
-
diff --git a/source/RobotAPI/interface/core/PoseBase.ice b/source/RobotAPI/interface/core/PoseBase.ice
index 81e24b0da8d56f11c0c1fcdcfd2ad016b7d50391..cfe93764d30141bf6224bd56da14fcb9be600056 100644
--- a/source/RobotAPI/interface/core/PoseBase.ice
+++ b/source/RobotAPI/interface/core/PoseBase.ice
@@ -22,12 +22,12 @@
#pragma once
-#include <ArmarXCore/interface/observers/VariantBase.ice>
#include <ArmarXCore/interface/observers/Filters.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
module armarx
{
- /**
+ /**
* [Vector2Base] defines a 2D vector with x and y.
*/
class Vector2Base extends VariantDataClass
@@ -35,6 +35,7 @@ module armarx
float x = zeroInt;
float y = zeroInt;
};
+
/**
* List of [Vector3Base].
*/
@@ -49,12 +50,13 @@ module armarx
float y = zeroInt;
float z = zeroInt;
};
- /**
+
+ /**
* List of [Vector3Base].
*/
sequence<Vector3Base> Vector3BaseList;
- /**
+ /**
* [QuaternionBase] defines a quaternion with
* @param qw Rotation angle.
* @param qx x-coordinate of normalized rotation axis.
@@ -69,21 +71,18 @@ module armarx
float qz = zeroInt;
};
- /**
+ /**
* [PoseBase] defines a pose with position in [Vector3Base] and orientation in [QuaternionBase].
* @param position Position as 3D vector in x, y, and z [Vector3Base].
* @param orientation Orientation as quaternion [QuaternionBase].
*/
- ["cpp:virtual"]
- class PoseBase extends VariantDataClass
+ ["cpp:virtual"] class PoseBase extends VariantDataClass
{
Vector3Base position;
QuaternionBase orientation;
};
- /**
+ /**
* List of [PoseBase].
*/
sequence<PoseBase> PoseBaseList;
-
};
-
diff --git a/source/RobotAPI/interface/core/PoseBaseStdOverloads.h b/source/RobotAPI/interface/core/PoseBaseStdOverloads.h
index 890c603eaa561fd3fc7bb2f48651a242338a8fe1..5df1ce53b257eba5e653154c305839b2f133d0bc 100644
--- a/source/RobotAPI/interface/core/PoseBaseStdOverloads.h
+++ b/source/RobotAPI/interface/core/PoseBaseStdOverloads.h
@@ -30,21 +30,28 @@
namespace std
{
- inline bool isfinite(const armarx::Vector3BasePtr& v)
+ inline bool
+ isfinite(const armarx::Vector3BasePtr& v)
{
return v && isfinite(v->x) && isfinite(v->y) & isfinite(v->z);
}
- inline bool isfinite(const armarx::Vector2BasePtr& v)
+
+ inline bool
+ isfinite(const armarx::Vector2BasePtr& v)
{
return v && isfinite(v->x) && isfinite(v->y);
}
- inline bool isfinite(const armarx::QuaternionBasePtr& q)
+
+ inline bool
+ isfinite(const armarx::QuaternionBasePtr& q)
{
return q && isfinite(q->qw) && isfinite(q->qx) && isfinite(q->qy) && isfinite(q->qz);
}
- inline bool isfinite(const armarx::PoseBasePtr p)
+
+ inline bool
+ isfinite(const armarx::PoseBasePtr p)
{
- return p && p->position && isfinite(p->position) && p->orientation && isfinite(p->orientation);
+ return p && p->position && isfinite(p->position) && p->orientation &&
+ isfinite(p->orientation);
}
-}
-
+} // namespace std
diff --git a/source/RobotAPI/interface/core/RobotLocalization.ice b/source/RobotAPI/interface/core/RobotLocalization.ice
index 1d65d3a1102b1197713cf0d0d40834cfbd64b0e8..cefbd9d2c3560198b3166f21500a0c905b341e9a 100644
--- a/source/RobotAPI/interface/core/RobotLocalization.ice
+++ b/source/RobotAPI/interface/core/RobotLocalization.ice
@@ -28,8 +28,9 @@
#include <RobotAPI/interface/core/GeometryBase.ice>
#include <RobotAPI/interface/core/PoseBase.ice>
-module armarx{
- interface GlobalRobotPoseProvider extends armem::server::MemoryInterface
+module armarx
+{
+ interface GlobalRobotPoseProvider extends armem::server::MemoryInterface
{
// timestamp in microseconds
PoseBase getGlobalRobotPose(long timestamp, string robotName);
@@ -44,7 +45,7 @@ module armarx{
{
void reportGlobalRobotPoseCorrection(TransformStamped global_T_odom);
}
-
+
interface OdometryListener
{
void reportOdometryVelocity(TwistStamped platformVelocity);
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 6b6ad3bd501a57603a8da4226cd92e7f59ba1796..d826cd08f3dc759b3a9b18b137b833d013df8bb2 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -23,16 +23,16 @@
*/
#pragma once
+#include <Ice/BuiltinSequences.ice>
+
+#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/events/SimulatorResetEvent.ice>
#include <ArmarXCore/interface/observers/Timestamp.ice>
-#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/serialization/Eigen.ice>
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/core/FramedPoseBase.ice>
#include <RobotAPI/interface/core/RobotLocalization.ice>
-
-#include <Ice/BuiltinSequences.ice>
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
module armarx
{
@@ -84,52 +84,33 @@ module armarx
interface SharedRobotNodeInterface extends SharedObjectInterface
{
- ["cpp:const"] idempotent
- float getJointValue();
- ["cpp:const"] idempotent
- string getName();
-
- ["cpp:const"] idempotent
- PoseBase getLocalTransformation();
+ ["cpp:const"] idempotent float getJointValue();
+ ["cpp:const"] idempotent string getName();
- ["cpp:const"] idempotent
- FramedPoseBase getGlobalPose();
+ ["cpp:const"] idempotent PoseBase getLocalTransformation();
- ["cpp:const"] idempotent
- FramedPoseBase getPoseInRootFrame();
+ ["cpp:const"] idempotent FramedPoseBase getGlobalPose();
- ["cpp:const"] idempotent
- JointType getType();
- ["cpp:const"] idempotent
- Vector3Base getJointTranslationDirection();
- ["cpp:const"] idempotent
- Vector3Base getJointRotationAxis();
+ ["cpp:const"] idempotent FramedPoseBase getPoseInRootFrame();
- ["cpp:const"] idempotent
- bool hasChild(string name, bool recursive);
- ["cpp:const"] idempotent
- string getParent();
- ["cpp:const"] idempotent
- NameList getChildren();
- ["cpp:const"] idempotent
- NameList getAllParents(string name);
+ ["cpp:const"] idempotent JointType getType();
+ ["cpp:const"] idempotent Vector3Base getJointTranslationDirection();
+ ["cpp:const"] idempotent Vector3Base getJointRotationAxis();
- ["cpp:const"] idempotent
- float getJointValueOffest();
- ["cpp:const"] idempotent
- float getJointLimitHigh();
- ["cpp:const"] idempotent
- float getJointLimitLow();
+ ["cpp:const"] idempotent bool hasChild(string name, bool recursive);
+ ["cpp:const"] idempotent string getParent();
+ ["cpp:const"] idempotent NameList getChildren();
+ ["cpp:const"] idempotent NameList getAllParents(string name);
- ["cpp:const"] idempotent
- Vector3Base getCoM();
+ ["cpp:const"] idempotent float getJointValueOffest();
+ ["cpp:const"] idempotent float getJointLimitHigh();
+ ["cpp:const"] idempotent float getJointLimitLow();
- ["cpp:const"] idempotent
- Ice::FloatSeq getInertia();
+ ["cpp:const"] idempotent Vector3Base getCoM();
- ["cpp:const"] idempotent
- float getMass();
+ ["cpp:const"] idempotent Ice::FloatSeq getInertia();
+ ["cpp:const"] idempotent float getMass();
};
@@ -144,8 +125,7 @@ module armarx
/**
* @return returns the RobotStateComponent this robot belongs to
*/
- ["cpp:const"] idempotent
- RobotStateComponentInterface* getRobotStateComponent();
+ ["cpp:const"] idempotent RobotStateComponentInterface* getRobotStateComponent();
SharedRobotNodeInterface* getRobotNode(string name);
SharedRobotNodeInterface* getRootNode();
@@ -154,8 +134,7 @@ module armarx
/**
* @return The timestamp of the last joint value update (not the global pose update).
*/
- ["cpp:const"] idempotent
- TimestampBase getTimestamp();
+ ["cpp:const"] idempotent TimestampBase getTimestamp();
RobotNodeSetInfo getRobotNodeSet(string name);
NameList getRobotNodeSets();
@@ -173,6 +152,7 @@ module armarx
class RobotInfoNode;
sequence<RobotInfoNode> RobotInfoNodeList;
+
class RobotInfoNode
{
string name;
@@ -181,23 +161,19 @@ module armarx
RobotInfoNodeList children;
};
-
/**
* The interface used by the RobotStateComponent which allows creating
* snapshots of a robot configuration or retrieving a proxy to a constantly
* updating synchronized robot.
*/
- interface RobotStateComponentInterface extends
- KinematicUnitListener,
- GlobalRobotPoseLocalizationListener,
- SimulatorResetEvent
+ interface RobotStateComponentInterface extends KinematicUnitListener,
+ GlobalRobotPoseLocalizationListener, SimulatorResetEvent
{
/**
* @return proxy to the shared robot which constantly updates all joint values
*/
- ["cpp:const"]
- idempotent
- SharedRobotInterface* getSynchronizedRobot() throws NotInitializedException;
+ ["cpp:const"] idempotent SharedRobotInterface* getSynchronizedRobot()
+ throws NotInitializedException;
/**
* @return proxy to a copy of the shared robot with non updating joint values
@@ -209,69 +185,58 @@ module armarx
* @return Snapshot
*
* */
- SharedRobotInterface* getRobotSnapshotAtTimestamp(double time) throws NotInitializedException;
+ SharedRobotInterface* getRobotSnapshotAtTimestamp(double time)
+ throws NotInitializedException;
/**
* Return the joint values from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
* @return Jointname-jointvalue map
*
* */
- ["cpp:const"]
- idempotent
- NameValueMap getJointConfigAtTimestamp(double time) throws NotInitializedException;
+ ["cpp:const"] idempotent NameValueMap getJointConfigAtTimestamp(double time)
+ throws NotInitializedException;
/**
* Return the Robot configuration, including joint values and global pose of the root node, from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
* @return Robot configuration containing jointvalue map and globalpose
*
* */
- ["cpp:const"]
- idempotent
- RobotStateConfig getRobotStateAtTimestamp(double time) throws NotInitializedException;
+ ["cpp:const"] idempotent RobotStateConfig getRobotStateAtTimestamp(double time)
+ throws NotInitializedException;
/**
* @return the robot xml filename as specified in the configuration
*/
- ["cpp:const"]
- idempotent
- string getRobotFilename();
+ ["cpp:const"] idempotent string getRobotFilename();
/**
* @return All dependent packages, which might contain a robot file.
*/
- ["cpp:const"]
- idempotent
- Ice::StringSeq getArmarXPackages();
+ ["cpp:const"] idempotent Ice::StringSeq getArmarXPackages();
/**
* @return The name of the robot represented by this component. Same as
* getSynchronizedRobot()->getName()
*
*/
- ["cpp:const"]
- idempotent string getRobotName() throws NotInitializedException;
+ ["cpp:const"] idempotent string getRobotName() throws NotInitializedException;
- ["cpp:const"]
- idempotent string getRobotStateTopicName() throws NotInitializedException;
+ ["cpp:const"] idempotent string getRobotStateTopicName() throws NotInitializedException;
- /**
+ /**
* @return The scaling of the robot model represented by this component.
*
*/
- ["cpp:const"]
- idempotent float getScaling();
+ ["cpp:const"] idempotent float getScaling();
- /**
+ /**
* @return The name of the robot node set that is represented by this component.
*
*/
- ["cpp:const"]
- idempotent string getRobotNodeSetName() throws NotInitializedException;
-
- ["cpp:const"]
- idempotent RobotInfoNode getRobotInfo();
+ ["cpp:const"] idempotent string getRobotNodeSetName() throws NotInitializedException;
+ ["cpp:const"] idempotent RobotInfoNode getRobotInfo();
};
interface RobotStateListenerInterface
@@ -279,5 +244,4 @@ module armarx
void reportGlobalRobotRootPose(FramedPoseBase globalPose, long timestamp, bool poseChanged);
void reportJointValues(NameValueMap jointAngles, long timestamp, bool aValueChanged);
};
-
};
diff --git a/source/RobotAPI/interface/core/RobotStateObserverInterface.ice b/source/RobotAPI/interface/core/RobotStateObserverInterface.ice
index 1a6115fbe83d4c534876f27d54113156ba2c8d8f..6b7c3ec01286835131a4c535c246cb199c7ef864 100644
--- a/source/RobotAPI/interface/core/RobotStateObserverInterface.ice
+++ b/source/RobotAPI/interface/core/RobotStateObserverInterface.ice
@@ -23,16 +23,12 @@
#pragma once
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
-
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+
module armarx
{
["amd"] interface RobotStateObserverInterface extends ObserverInterface
- {
- ["cpp:const"]
- idempotent DatafieldRefBase getPoseDatafield(string nodeName);
- };
+ { ["cpp:const"] idempotent DatafieldRefBase getPoseDatafield(string nodeName); };
};
-
diff --git a/source/RobotAPI/interface/core/TopicTimingTest.ice b/source/RobotAPI/interface/core/TopicTimingTest.ice
index 3afe98c177d5736060810a4be03bf0111e04bca4..9f2d3acf908d390e58171f1f30082407d2d45309 100644
--- a/source/RobotAPI/interface/core/TopicTimingTest.ice
+++ b/source/RobotAPI/interface/core/TopicTimingTest.ice
@@ -4,28 +4,27 @@
module armarx
{
-module topic_timing
-{
- struct SmallData
+ module topic_timing
{
- long sentTimestamp = 0;
- };
+ struct SmallData
+ {
+ long sentTimestamp = 0;
+ };
- sequence <float> FloatSeq;
+ sequence<float> FloatSeq;
- struct BigData
- {
- long sentTimestamp = 0;
+ struct BigData
+ {
+ long sentTimestamp = 0;
- Ice::FloatSeq data;
- };
+ Ice::FloatSeq data;
+ };
- interface Topic
- {
- void reportSmall(SmallData data);
+ interface Topic
+ {
+ void reportSmall(SmallData data);
- void reportBig(BigData data);
+ void reportBig(BigData data);
+ };
};
-
-};
};
diff --git a/source/RobotAPI/interface/core/Trajectory.ice b/source/RobotAPI/interface/core/Trajectory.ice
index 3b0bc1cb621b4859604ae701ca3e88be727b020a..724da0698f6bcc0e597eb02b8a0af2c32c531756 100644
--- a/source/RobotAPI/interface/core/Trajectory.ice
+++ b/source/RobotAPI/interface/core/Trajectory.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <ArmarXCore/interface/observers/VariantBase.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
module armarx
{
@@ -38,15 +38,13 @@ module armarx
sequence<LimitlessState> LimitlessStateSeq;
- /**
+ /**
* [TrajectoryBase] defines a n-dimension trajectory with n deriviations.
*
*/
- ["cpp:virtual"]
- class TrajectoryBase extends VariantDataClass
+ ["cpp:virtual"] class TrajectoryBase extends VariantDataClass
{
- ["protected"]
- Ice::StringSeq dimensionNames;
+ ["protected"] Ice::StringSeq dimensionNames;
/**
* first vector dimension is the frame of the trajectory
* second vector dimension is dimension of trajectory (e.g. which joint).
@@ -58,7 +56,4 @@ module armarx
["protected"] LimitlessStateSeq limitless;
};
-
-
};
-
diff --git a/source/RobotAPI/interface/mdb/MotionDatabase.ice b/source/RobotAPI/interface/mdb/MotionDatabase.ice
index 2ca77b0b92f86b2945925949fd239678e35a78ba..de0a0427e3a9627a5361cf3df88d1d130f751007 100644
--- a/source/RobotAPI/interface/mdb/MotionDatabase.ice
+++ b/source/RobotAPI/interface/mdb/MotionDatabase.ice
@@ -1,146 +1,187 @@
#include <Glacier2/Session.ice>
-module MotionDatabase {
- exception InternalErrorException {
- string errorMessage;
- };
-
- exception InvalidParameterException {
- string parameterName;
- };
-
- exception NotAuthorizedException {};
- exception TooManyOpenFilesException {};
-
- class Institution;
- class MotionDescriptionTreeNode;
- class File;
- class Motion;
- class Project;
- class Subject;
- class MoCapObject;
-
- sequence<byte> ByteSequence;
- sequence<long> LongSequence;
- sequence<string> StringSequence;
- sequence<Institution> InstitutionList;
- sequence<MotionDescriptionTreeNode> MotionDescriptionTreeNodeList;
- sequence<File> FileList;
- sequence<Motion> MotionList;
- sequence<Project> ProjectList;
- sequence<Subject> SubjectList;
- sequence<MoCapObject> MoCapObjectList;
-
- dictionary<string, short> StringShortDictionary;
-
- enum VisibilityLevel { Public, Protected, Internal };
-
- class Institution {
- long id;
- string acronym;
- string name;
- };
-
- class MotionDescriptionTreeNode {
- long id;
- string label;
- MotionDescriptionTreeNodeList children;
- };
-
- class DatabaseObject {
- long id;
- long createdDate;
- string createdUser;
- long modifiedDate;
- string modifiedUser;
- StringSequence writeGroups;
- StringSequence readProtectedGroups;
- StringShortDictionary fileTypeCounts;
- };
-
- class File {
- long id;
- long createdDate;
- string createdUser;
- string fileName;
- string fileType;
- long attachedToId;
- string description;
- VisibilityLevel visibility;
- File originatedFrom;
- };
-
- class Motion extends DatabaseObject {
- Institution associatedInstitution;
- MotionDescriptionTreeNodeList motionDescriptions;
- Project associatedProject;
- SubjectList associatedSubjects;
- MoCapObjectList associatedObjects;
- string date;
- string comment;
- };
-
- class Project extends DatabaseObject {
- string name;
- string comment;
- };
-
- class Subject extends DatabaseObject {
- string firstName;
- string lastName;
- string comment;
- byte gender;
- short age;
- short weight;
- short height;
- StringShortDictionary anthropometricsTable;
- };
-
- class MoCapObject extends DatabaseObject {
- string label;
- string comment;
- string modelSettingsJSON;
- };
-
- interface FileReader {
- void destroy();
-
- idempotent long getSize() throws InternalErrorException;
- ByteSequence readChunk(long length) throws InternalErrorException, InvalidParameterException;
- idempotent void seek(long pos) throws InternalErrorException, InvalidParameterException;
- };
-
- interface FileWriter {
- void destroy();
-
- void writeChunk(ByteSequence data) throws InternalErrorException;
- };
-
- interface MotionDatabaseSession extends Glacier2::Session {
- idempotent string pingServer(string echoString);
-
- idempotent InstitutionList listInstitutions() throws InternalErrorException;
- idempotent MotionDescriptionTreeNodeList getMotionDescriptionTree() throws InternalErrorException;
-
- idempotent Motion getMotion(long motionId) throws InternalErrorException, InvalidParameterException;
- idempotent long countMotions(LongSequence filterMotionDescription, LongSequence filterProject, LongSequence filterInstitution,
- LongSequence filterSubject, LongSequence filterObject, string motionDescriptionSearchTerm) throws InternalErrorException,
- InvalidParameterException;
- idempotent MotionList listMotions(LongSequence filterMotionDescription, LongSequence filterProject, LongSequence filterInstitution,
- LongSequence filterSubject, LongSequence filterObject, string motionDescriptionSearchTerm, string sortField, long limit,
- long offset) throws InternalErrorException, InvalidParameterException;
- idempotent ProjectList listProjects() throws InternalErrorException;
- idempotent SubjectList listSubjects() throws InternalErrorException;
- idempotent MoCapObjectList listObjects() throws InternalErrorException;
-
- idempotent File getFile(long fileId) throws InternalErrorException, InvalidParameterException;
- idempotent FileList listFiles(long databaseObjectId) throws InternalErrorException, InvalidParameterException;
- idempotent FileReader* getFileReader(long fileId) throws InternalErrorException, InvalidParameterException, NotAuthorizedException,
- TooManyOpenFilesException;
-
- FileWriter* getFileWriter(long databaseObjectId, string fileName, string fileType, string description, VisibilityLevel visibility,
- optional(1) long originatedFromId) throws InternalErrorException, InvalidParameterException, NotAuthorizedException,
- TooManyOpenFilesException;
- void deleteFile(long fileId) throws InternalErrorException, InvalidParameterException, NotAuthorizedException;
- };
+module MotionDatabase
+{
+ exception InternalErrorException
+ {
+ string errorMessage;
+ };
+
+ exception InvalidParameterException
+ {
+ string parameterName;
+ };
+
+ exception NotAuthorizedException{};
+ exception TooManyOpenFilesException{};
+
+ class Institution;
+ class MotionDescriptionTreeNode;
+ class File;
+ class Motion;
+ class Project;
+ class Subject;
+ class MoCapObject;
+
+ sequence<byte> ByteSequence;
+ sequence<long> LongSequence;
+ sequence<string> StringSequence;
+ sequence<Institution> InstitutionList;
+ sequence<MotionDescriptionTreeNode> MotionDescriptionTreeNodeList;
+ sequence<File> FileList;
+ sequence<Motion> MotionList;
+ sequence<Project> ProjectList;
+ sequence<Subject> SubjectList;
+ sequence<MoCapObject> MoCapObjectList;
+
+ dictionary<string, short> StringShortDictionary;
+
+ enum VisibilityLevel
+ {
+ Public,
+ Protected,
+ Internal
+ };
+
+ class Institution
+ {
+ long id;
+ string acronym;
+ string name;
+ };
+
+ class MotionDescriptionTreeNode
+ {
+ long id;
+ string label;
+ MotionDescriptionTreeNodeList children;
+ };
+
+ class DatabaseObject
+ {
+ long id;
+ long createdDate;
+ string createdUser;
+ long modifiedDate;
+ string modifiedUser;
+ StringSequence writeGroups;
+ StringSequence readProtectedGroups;
+ StringShortDictionary fileTypeCounts;
+ };
+
+ class File
+ {
+ long id;
+ long createdDate;
+ string createdUser;
+ string fileName;
+ string fileType;
+ long attachedToId;
+ string description;
+ VisibilityLevel visibility;
+ File originatedFrom;
+ };
+
+ class Motion extends DatabaseObject
+ {
+ Institution associatedInstitution;
+ MotionDescriptionTreeNodeList motionDescriptions;
+ Project associatedProject;
+ SubjectList associatedSubjects;
+ MoCapObjectList associatedObjects;
+ string date;
+ string comment;
+ };
+
+ class Project extends DatabaseObject
+ {
+ string name;
+ string comment;
+ };
+
+ class Subject extends DatabaseObject
+ {
+ string firstName;
+ string lastName;
+ string comment;
+ byte gender;
+ short age;
+ short weight;
+ short height;
+ StringShortDictionary anthropometricsTable;
+ };
+
+ class MoCapObject extends DatabaseObject
+ {
+ string label;
+ string comment;
+ string modelSettingsJSON;
+ };
+
+ interface FileReader
+ {
+ void destroy();
+
+ idempotent long getSize() throws InternalErrorException;
+ ByteSequence readChunk(long length) throws InternalErrorException,
+ InvalidParameterException;
+ idempotent void seek(long pos) throws InternalErrorException, InvalidParameterException;
+ };
+
+ interface FileWriter
+ {
+ void destroy();
+
+ void writeChunk(ByteSequence data) throws InternalErrorException;
+ };
+
+ interface MotionDatabaseSession extends Glacier2::Session
+ {
+ idempotent string pingServer(string echoString);
+
+ idempotent InstitutionList listInstitutions() throws InternalErrorException;
+ idempotent MotionDescriptionTreeNodeList getMotionDescriptionTree()
+ throws InternalErrorException;
+
+ idempotent Motion getMotion(long motionId) throws InternalErrorException,
+ InvalidParameterException;
+ idempotent long countMotions(LongSequence filterMotionDescription,
+ LongSequence filterProject,
+ LongSequence filterInstitution,
+ LongSequence filterSubject,
+ LongSequence filterObject,
+ string motionDescriptionSearchTerm)
+ throws InternalErrorException,
+ InvalidParameterException;
+ idempotent MotionList listMotions(LongSequence filterMotionDescription,
+ LongSequence filterProject,
+ LongSequence filterInstitution,
+ LongSequence filterSubject,
+ LongSequence filterObject,
+ string motionDescriptionSearchTerm,
+ string sortField,
+ long limit,
+ long offset) throws InternalErrorException,
+ InvalidParameterException;
+ idempotent ProjectList listProjects() throws InternalErrorException;
+ idempotent SubjectList listSubjects() throws InternalErrorException;
+ idempotent MoCapObjectList listObjects() throws InternalErrorException;
+
+ idempotent File getFile(long fileId) throws InternalErrorException,
+ InvalidParameterException;
+ idempotent FileList listFiles(long databaseObjectId) throws InternalErrorException,
+ InvalidParameterException;
+ idempotent FileReader *getFileReader(long fileId) throws InternalErrorException,
+ InvalidParameterException, NotAuthorizedException, TooManyOpenFilesException;
+
+ FileWriter *getFileWriter(long databaseObjectId,
+ string fileName,
+ string fileType,
+ string description,
+ VisibilityLevel visibility,
+ optional(1) long originatedFromId) throws InternalErrorException,
+ InvalidParameterException, NotAuthorizedException, TooManyOpenFilesException;
+ void deleteFile(long fileId) throws InternalErrorException, InvalidParameterException,
+ NotAuthorizedException;
+ };
};
diff --git a/source/RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.ice b/source/RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.ice
index b7c819ec05d886d40f51723451411070f3930022..4c05c1bd09d9959375be0a95c6bde0c57f6b726b 100644
--- a/source/RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.ice
+++ b/source/RobotAPI/interface/objectpose/FamiliarObjectPoseStorageInterface.ice
@@ -27,17 +27,13 @@
#include <RobotAPI/interface/aron/Aron.ice>
-module armarx
-{
- module objpose
- {
- sequence<::armarx::aron::data::dto::Dict> ProvidedFamiliarObjectPoseSeq;
-
- interface FamiliarObjectPoseStorageInterface
- {
- void reportFamiliarObjectPoses(string providerName, ProvidedFamiliarObjectPoseSeq data);
- };
-
- }
+module armarx{
+ module objpose{sequence<::armarx::aron::data::dto::Dict> ProvidedFamiliarObjectPoseSeq;
+interface FamiliarObjectPoseStorageInterface
+{
+ void reportFamiliarObjectPoses(string providerName, ProvidedFamiliarObjectPoseSeq data);
};
+}
+}
+;
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
index 7eef5984e466173433e340cf45711be4fd187c95..2016d076bc29a049ad37ca4fe95eb1481bde89bf 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseProvider.ice
@@ -34,12 +34,14 @@ module armarx
module objpose
{
interface ObjectPoseProvider;
+
struct ProviderInfo
{
ObjectPoseProvider* proxy;
ObjectType objectType = AnyObject;
armarx::data::ObjectIDSeq supportedObjects;
};
+
dictionary<string, ProviderInfo> ProviderInfoMap;
@@ -57,7 +59,9 @@ module armarx
{
bool success;
};
+
dictionary<armarx::data::ObjectID, ObjectRequestResult> ObjectRequestResultMap;
+
struct RequestObjectsOutput
{
ObjectRequestResultMap results;
@@ -85,6 +89,4 @@ module armarx
void reportObjectPoses(string providerName, data::ProvidedObjectPoseSeq candidates);
};
};
-
};
-
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice b/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
index a069da9078a11beeb140d5477dd847beb85cf2c3..063ff205910ed11e9d5046606d140ea80bdd5490 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseStorageInterface.ice
@@ -28,9 +28,8 @@
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
-
-#include <RobotAPI/interface/objectpose/object_pose_types.ice>
#include <RobotAPI/interface/objectpose/ObjectPoseProvider.ice>
+#include <RobotAPI/interface/objectpose/object_pose_types.ice>
module armarx
@@ -46,6 +45,7 @@ module armarx
string provider;
provider::RequestObjectsInput request;
};
+
struct ObjectRequestResult
{
/// Name of the provider who was requested.
@@ -53,7 +53,9 @@ module armarx
string providerName;
provider::ObjectRequestResult result;
};
+
dictionary<armarx::data::ObjectID, ObjectRequestResult> ObjectRequestResultMap;
+
struct RequestObjectsOutput
{
/// The results per objectID.
@@ -79,6 +81,7 @@ module armarx
*/
PoseBase poseInFrame;
};
+
struct AttachObjectToRobotNodeOutput
{
bool success = false;
@@ -96,6 +99,7 @@ module armarx
*/
bool commitAttachedPose = true;
};
+
struct DetachObjectFromRobotNodeOutput
{
/// Whether the object was attached before.
@@ -109,8 +113,8 @@ module armarx
* detaching until they are provided again.
*/
bool commitAttachedPose = true;
- }
- struct DetachAllObjectsFromRobotNodesOutput
+ } struct DetachAllObjectsFromRobotNodesOutput
+
{
/// Number of objects that have been detached.
int numDetached = 0;
@@ -121,8 +125,8 @@ module armarx
string agent;
Ice::StringSeq frames;
};
- sequence<AgentFrames> AgentFramesSeq;
+ sequence<AgentFrames> AgentFramesSeq;
enum HeadMovementAction
{
@@ -131,6 +135,7 @@ module armarx
/// Head movement is stopping.
HeadMovementAction_Stopping
};
+
struct SignalHeadMovementInput
{
HeadMovementAction action;
@@ -146,6 +151,7 @@ module armarx
*/
long discardUpdatesIntervalMilliSeconds = -1;
};
+
struct SignalHeadMovementOutput
{
/// The time until new updates will be discarded.
@@ -159,7 +165,9 @@ module armarx
armarx::core::time::dto::Duration timeWindow;
armem::prediction::data::PredictionSettings settings;
};
+
sequence<ObjectPosePredictionRequest> ObjectPosePredictionRequestSeq;
+
struct ObjectPosePredictionResult
{
bool success = false;
@@ -167,10 +175,10 @@ module armarx
data::ObjectPose prediction;
};
+
sequence<ObjectPosePredictionResult> ObjectPosePredictionResultSeq;
- interface ObjectPoseStorageInterface extends
- ObjectPoseTopic
+ interface ObjectPoseStorageInterface extends ObjectPoseTopic
{
// Object poses
@@ -191,11 +199,14 @@ module armarx
// Attaching
/// Attach an object to a robot node.
- AttachObjectToRobotNodeOutput attachObjectToRobotNode(AttachObjectToRobotNodeInput input);
+ AttachObjectToRobotNodeOutput attachObjectToRobotNode(
+ AttachObjectToRobotNodeInput input);
/// Detach an attached object from a robot node.
- DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(DetachObjectFromRobotNodeInput input);
+ DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(
+ DetachObjectFromRobotNodeInput input);
/// Detach all objects from robot nodes.
- DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(DetachAllObjectsFromRobotNodesInput input);
+ DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(
+ DetachAllObjectsFromRobotNodesInput input);
AgentFramesSeq getAttachableFrames();
@@ -211,14 +222,11 @@ module armarx
// Predicting
// Request predictions for the poses of the given objects at the given timestamps.
- ObjectPosePredictionResultSeq
- predictObjectPoses(ObjectPosePredictionRequestSeq requests);
+ ObjectPosePredictionResultSeq predictObjectPoses(
+ ObjectPosePredictionRequestSeq requests);
// Get the prediction engines available for use with this storage interface.
armem::prediction::data::PredictionEngineSeq getAvailableObjectPoseEngines();
-
};
};
-
};
-
diff --git a/source/RobotAPI/interface/objectpose/object_pose_types.ice b/source/RobotAPI/interface/objectpose/object_pose_types.ice
index 6f6d5cfc83adab6023212dc3996bb5745efd89e2..c39d283ce3c90394f68aa00f9e5cdb8f16235c1d 100644
--- a/source/RobotAPI/interface/objectpose/object_pose_types.ice
+++ b/source/RobotAPI/interface/objectpose/object_pose_types.ice
@@ -26,9 +26,9 @@
#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
#include <ArmarXCore/interface/core/time.ice>
-#include <RobotAPI/interface/core/PoseBase.ice>
-#include <RobotAPI/interface/core/NameValueMap.ice>
#include <RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.ice>
+#include <RobotAPI/interface/core/NameValueMap.ice>
+#include <RobotAPI/interface/core/PoseBase.ice>
module armarx
@@ -39,11 +39,11 @@ module armarx
// ObjectTypeEnum is now renamed to ObjectType
enum ObjectType
{
- AnyObject, KnownObject, UnknownObject
+ AnyObject,
+ KnownObject,
+ UnknownObject
};
-
-
class AABB
{
Vector3Base center;
@@ -61,7 +61,6 @@ module armarx
Vector3Base extents;
};
-
module data
{
/**
@@ -69,13 +68,12 @@ module armarx
*
* @see `armarx::objpose::PoseManifoldGaussian`
*/
- class PoseManifoldGaussian // class => optional
+ class PoseManifoldGaussian // class => optional
{
PoseBase mean;
Ice::FloatSeq covariance6x6;
};
-
/// An object pose provided by an ObjectPoseProvider.
struct ProvidedObjectPose
{
@@ -110,6 +108,7 @@ module armarx
/// [Optional] Object bounding box in object's local coordinate frame.
Box localOOBB;
};
+
sequence<ProvidedObjectPose> ProvidedObjectPoseSeq;
@@ -156,8 +155,8 @@ module armarx
/// Object bounding box in object's local coordinate frame. May be null.
Box localOOBB;
};
- sequence<ObjectPose> ObjectPoseSeq;
+ sequence<ObjectPose> ObjectPoseSeq;
class ObjectAttachmentInfo
{
@@ -167,7 +166,5 @@ module armarx
PoseBase poseInFrame;
};
}
-
};
};
-
diff --git a/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice b/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
index b60aa19c3db3bd5d7f1a68937758e401745f3ef0..d44be89966a647439b27715ae02f97a2c35179c5 100644
--- a/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
+++ b/source/RobotAPI/interface/observers/GraspCandidateObserverInterface.ice
@@ -25,6 +25,7 @@
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.ice>
module armarx
@@ -38,13 +39,13 @@ module armarx
armarx::objpose::ObjectType objectType = AnyObject;
ApertureType preshape = AnyAperture;
ApproachType approach = AnyApproach;
-
};
sequence<string> StringSeq;
dictionary<string, int> IntMap;
- interface GraspCandidateObserverInterface extends ObserverInterface, GraspCandidatesTopicInterface
+ interface GraspCandidateObserverInterface extends ObserverInterface,
+ GraspCandidatesTopicInterface
{
InfoMap getAvailableProvidersWithInfo();
StringSeq getAvailableProviderNames();
@@ -68,9 +69,6 @@ module armarx
BimanualGraspCandidateSeq getSelectedBimanualCandidates();
void clearCandidatesByProvider(string providerName);
-
};
};
-
};
-
diff --git a/source/RobotAPI/interface/observers/KinematicUnitObserverInterface.ice b/source/RobotAPI/interface/observers/KinematicUnitObserverInterface.ice
index 71f5598e0ed0bed5df5099579b4d08738d436091..c737524a0b828653b8e011cc692ffb0d16fc5a7a 100644
--- a/source/RobotAPI/interface/observers/KinematicUnitObserverInterface.ice
+++ b/source/RobotAPI/interface/observers/KinematicUnitObserverInterface.ice
@@ -1,4 +1,4 @@
- /*
+/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
@@ -22,14 +22,11 @@
#pragma once
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
-
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+
module armarx
{
- interface KinematicUnitObserverInterface extends ObserverInterface, KinematicUnitListener
- {
- };
+ interface KinematicUnitObserverInterface extends ObserverInterface, KinematicUnitListener{};
};
-
diff --git a/source/RobotAPI/interface/observers/ObserverFilters.ice b/source/RobotAPI/interface/observers/ObserverFilters.ice
index ad2df8e05f197f8e773b168d40bff82acc014d8c..ec636f0de9e6b3c9497d7bfb273c69600ac85e04 100644
--- a/source/RobotAPI/interface/observers/ObserverFilters.ice
+++ b/source/RobotAPI/interface/observers/ObserverFilters.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <ArmarXCore/interface/observers/VariantBase.ice>
#include <ArmarXCore/interface/observers/Filters.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
module armarx
{
@@ -34,73 +34,56 @@ module armarx
const string magnitudeinrange = "magnitudeinrange";
};
- /**
+ /**
* OffsetFilterBase implements a filter which stores the measurement in the first run as an offset.
* This offset is subtracted from subsequent measurements.
*/
- ["cpp:virtual"]
- class OffsetFilterBase extends DatafieldFilterBase
- {
+ ["cpp:virtual"] class OffsetFilterBase extends DatafieldFilterBase {
};
- /**
+ /**
* MatrixMaxFilterBase implements a filter which returns the maximum coefficient of a matrix.
*/
- ["cpp:virtual"]
- class MatrixMaxFilterBase extends DatafieldFilterBase
- {
+ ["cpp:virtual"] class MatrixMaxFilterBase extends DatafieldFilterBase {
};
- /**
+ /**
* MatrixMinFilterBase implements a filter which returns the minimum coefficient of a matrix.
*/
- ["cpp:virtual"]
- class MatrixMinFilterBase extends DatafieldFilterBase
- {
+ ["cpp:virtual"] class MatrixMinFilterBase extends DatafieldFilterBase {
};
-
- /**
+
+ /**
* MatrixMinFilterBase implements a filter which returns the mean value of all coefficients of a matrix.
*/
- ["cpp:virtual"]
- class MatrixAvgFilterBase extends DatafieldFilterBase
- {
+ ["cpp:virtual"] class MatrixAvgFilterBase extends DatafieldFilterBase {
};
- /**
+ /**
* MatrixPercentileFilterBase implements a filter which returns a subset of the smallest coefficients of a matrix.
* The matrix is transformed int a vector which is sorted in a ascending order. The subset consists of the first entries
* of the sorted vector. The size of the subset is defined by parameter percentile which ranges from 0.0 to 1.0.
*/
- ["cpp:virtual"]
- class MatrixPercentileFilterBase extends DatafieldFilterBase
- {
- float percentile;
- };
+ ["cpp:virtual"] class MatrixPercentileFilterBase extends DatafieldFilterBase
+ { float percentile; };
- /**
+ /**
* MatrixPercentilesFilterBase implements a filter which returns a subset of the smallest coefficients of a matrix.
* The matrix is transformed int a vector which is sorted in a ascending order. The subset consists of the first entries
* of the sorted vector. The size of the subset is defined by parameter percentiles.
*/
- ["cpp:virtual"]
- class MatrixPercentilesFilterBase extends DatafieldFilterBase
- {
- int percentiles;
- };
-
- /**
+ ["cpp:virtual"] class MatrixPercentilesFilterBase extends DatafieldFilterBase
+ { int percentiles; };
+
+ /**
* MatrixCumulativeFrequencyFilterBase implements a filter which returns a matrix filtered by Cumulative Frequency filter.
* The entries of the filtered matrix range between the provided min and max values.
*/
- ["cpp:virtual"]
- class MatrixCumulativeFrequencyFilterBase extends DatafieldFilterBase
+ ["cpp:virtual"] class MatrixCumulativeFrequencyFilterBase extends DatafieldFilterBase
{
float min;
float max;
int bins;
};
-
};
-
diff --git a/source/RobotAPI/interface/observers/PlatformUnitObserverInterface.ice b/source/RobotAPI/interface/observers/PlatformUnitObserverInterface.ice
index 20709f1fedeb65bb3766d2008607ee750a74965a..780704954d6a6f77f5bc8ed7a56a939fe1b992b4 100644
--- a/source/RobotAPI/interface/observers/PlatformUnitObserverInterface.ice
+++ b/source/RobotAPI/interface/observers/PlatformUnitObserverInterface.ice
@@ -1,4 +1,4 @@
- /*
+/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
@@ -22,15 +22,13 @@
#pragma once
-#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
-
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
#include <RobotAPI/interface/core/RobotLocalization.ice>
+#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
module armarx
{
- interface PlatformUnitObserverInterface extends ObserverInterface, PlatformUnitListener, GlobalRobotPoseLocalizationListener
- {
- };
+ interface PlatformUnitObserverInterface extends ObserverInterface, PlatformUnitListener,
+ GlobalRobotPoseLocalizationListener{};
};
diff --git a/source/RobotAPI/interface/observers/SpeechObserverInterface.ice b/source/RobotAPI/interface/observers/SpeechObserverInterface.ice
index 33d6c1758ac04b0235812e8bbf5e6537eda9e830..81e123a465bc32b54870d9ad868a779aefe74426 100644
--- a/source/RobotAPI/interface/observers/SpeechObserverInterface.ice
+++ b/source/RobotAPI/interface/observers/SpeechObserverInterface.ice
@@ -24,14 +24,12 @@
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+
#include <RobotAPI/interface/speech/SpeechInterface.ice>
module armarx
{
- interface SpeechObserverInterface extends ObserverInterface, TextToSpeechStateInterface, TextListenerInterface
- {
- };
-
+ interface SpeechObserverInterface extends ObserverInterface, TextToSpeechStateInterface,
+ TextListenerInterface{};
};
-
diff --git a/source/RobotAPI/interface/selflocalisation/SelfLocalisationProcess.ice b/source/RobotAPI/interface/selflocalisation/SelfLocalisationProcess.ice
index 0a352add1aee10b6fa2642e70b479513cb2f32d9..9bdd6b17e60707ff0e5084537929b88f4dce2e7a 100644
--- a/source/RobotAPI/interface/selflocalisation/SelfLocalisationProcess.ice
+++ b/source/RobotAPI/interface/selflocalisation/SelfLocalisationProcess.ice
@@ -32,4 +32,3 @@ module armarx
void stopSelfLocalisationProcess();
};
};
-
diff --git a/source/RobotAPI/interface/skills/SkillManagerInterface.ice b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
index f22738bda8b71cac09faf71620e226135ac93532..04ed44b5411d5268760165e85e0091940b42e35e 100644
--- a/source/RobotAPI/interface/skills/SkillManagerInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillManagerInterface.ice
@@ -107,15 +107,15 @@ module armarx
dto::SkillDescriptionMap
getSkillDescriptions(); // get all skilldescriptions from all providers
- optional(2) dto::SkillStatusUpdate
- getSkillExecutionStatus(dto::SkillExecutionID executionId);
+ optional(2) dto::SkillStatusUpdate getSkillExecutionStatus(
+ dto::SkillExecutionID executionId);
dto::SkillStatusUpdateMap
getSkillExecutionStatuses(); // returns the current executions from all providers
- dto::SkillStatusUpdate
- executeSkill(dto::SkillExecutionRequest
- skillExecutionRequest); // blocks until skill is finished.
+ dto::SkillStatusUpdate executeSkill(
+ dto::SkillExecutionRequest
+ skillExecutionRequest); // blocks until skill is finished.
dto::SkillExecutionID executeSkillAsync(
dto::SkillExecutionRequest
@@ -127,8 +127,8 @@ module armarx
// notify a skill to stop ASAP.
provider::dto::AbortSkillResult abortSkill(dto::SkillExecutionID executionId);
- provider::dto::AbortSkillResult
- abortSkillAsync(dto::SkillExecutionID executionId);
+ provider::dto::AbortSkillResult abortSkillAsync(
+ dto::SkillExecutionID executionId);
};
}
}
diff --git a/source/RobotAPI/interface/skills/SkillProviderInterface.ice b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
index 3066c962e45326fecfdf8f08c6985bb89bf6c3d7..5f327376c7c59196be8129a2bd4a7c0a3baaf0bf 100644
--- a/source/RobotAPI/interface/skills/SkillProviderInterface.ice
+++ b/source/RobotAPI/interface/skills/SkillProviderInterface.ice
@@ -173,8 +173,8 @@ module armarx
dto::SkillDescriptionMap getSkillDescriptions();
- optional(2) dto::SkillStatusUpdate
- getSkillExecutionStatus(dto::SkillExecutionID executionId);
+ optional(2) dto::SkillStatusUpdate getSkillExecutionStatus(
+ dto::SkillExecutionID executionId);
dto::SkillStatusUpdateMap
getSkillExecutionStatuses(); // returns all current skill executions
@@ -184,12 +184,12 @@ module armarx
// This method returns a status update where the status is ALWAYS one of the terminating values
dto::SkillStatusUpdate executeSkill(dto::SkillExecutionRequest executionInfo);
- dto::SkillExecutionID
- executeSkillAsync(dto::SkillExecutionRequest executionInfo);
+ dto::SkillExecutionID executeSkillAsync(
+ dto::SkillExecutionRequest executionInfo);
- dto::ParameterUpdateResult
- updateSkillParameters(dto::SkillExecutionID executionId,
- aron::data::dto::Dict params); // add params to a skill
+ dto::ParameterUpdateResult updateSkillParameters(
+ dto::SkillExecutionID executionId,
+ aron::data::dto::Dict params); // add params to a skill
// try to kill a skill as soon as possible. When the skill is stopped depends on the implementation.
dto::AbortSkillResult abortSkill(dto::SkillExecutionID skill);
diff --git a/source/RobotAPI/interface/skills/StatechartListenerInterface.ice b/source/RobotAPI/interface/skills/StatechartListenerInterface.ice
index 084633cdcdc1772c7f8f960b3119707dc612373f..2c53130fd7d62d47b11540fe696eb22b2e2d860d 100644
--- a/source/RobotAPI/interface/skills/StatechartListenerInterface.ice
+++ b/source/RobotAPI/interface/skills/StatechartListenerInterface.ice
@@ -28,8 +28,6 @@ module armarx
{
module dti
{
- interface StatechartListenerInterface extends armarx::ProfilerListener
- {
- };
+ interface StatechartListenerInterface extends armarx::ProfilerListener{};
}
}
diff --git a/source/RobotAPI/interface/speech/SpeechInterface.ice b/source/RobotAPI/interface/speech/SpeechInterface.ice
index 8356ea1652d93a496cc0f811bdf4c9bb22496f0f..b8bc3e14761262d630d4e4d3bc02ef4b5367dc3f 100644
--- a/source/RobotAPI/interface/speech/SpeechInterface.ice
+++ b/source/RobotAPI/interface/speech/SpeechInterface.ice
@@ -32,7 +32,7 @@ module armarx
*/
enum AudioEncoding
{
- PCM /*!< digitized PCM audio data (uncompressed) */
+ PCM /*!< digitized PCM audio data (uncompressed) */
};
sequence<byte> AudioChunk;
@@ -81,8 +81,7 @@ module armarx
* \param string vector params.
*/
- void reportTextWithParams(string text,Ice::StringSeq params);
-
+ void reportTextWithParams(string text, Ice::StringSeq params);
};
enum TextToSpeechStateType
@@ -107,6 +106,4 @@ module armarx
{
void publishFeedback(FeedbackType type, bool sign);
};
-
};
-
diff --git a/source/RobotAPI/interface/units/ATINetFTUnit.ice b/source/RobotAPI/interface/units/ATINetFTUnit.ice
index 8339b68f57dc76ea7847068dff53ea88f2c2c350..40a7d036017ae87f104be012daa053cc7aa8e766 100644
--- a/source/RobotAPI/interface/units/ATINetFTUnit.ice
+++ b/source/RobotAPI/interface/units/ATINetFTUnit.ice
@@ -24,9 +24,10 @@
#pragma once
+#include <ArmarXCore/interface/core/UserException.ice>
+
#include <RobotAPI/interface/core/PoseBase.ice>
#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <ArmarXCore/interface/core/UserException.ice>
module armarx
{
@@ -54,4 +55,3 @@ module armarx
bool isComponentOnline();
};
};
-
diff --git a/source/RobotAPI/interface/units/CyberGloveInterface.ice b/source/RobotAPI/interface/units/CyberGloveInterface.ice
index de433915cb5ed25bf064c0c2408718c0914fd458..166ee83da203e90df47a4fa70e8d385e38f187de 100644
--- a/source/RobotAPI/interface/units/CyberGloveInterface.ice
+++ b/source/RobotAPI/interface/units/CyberGloveInterface.ice
@@ -60,7 +60,6 @@ module armarx
void reportGloveValues(CyberGloveValues gloveValues);
/*void reportMotorValues(string name, float position1, float pwm1, float position2, float pwm2);*/
};
-
};
#endif
diff --git a/source/RobotAPI/interface/units/CyberGloveObserverInterface.ice b/source/RobotAPI/interface/units/CyberGloveObserverInterface.ice
index f2482bec8564029ae9775909f9e681d428e39b1d..b89bd9fe1091ef2c98a07a481902c1a1dcb7215c 100644
--- a/source/RobotAPI/interface/units/CyberGloveObserverInterface.ice
+++ b/source/RobotAPI/interface/units/CyberGloveObserverInterface.ice
@@ -25,15 +25,14 @@
#pragma once
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+
#include <RobotAPI/interface/units/CyberGloveInterface.ice>
module armarx
-{
+{
interface CyberGloveObserverInterface extends ObserverInterface, CyberGloveListenerInterface
{
CyberGloveValues getLatestValues();
};
};
-
-
diff --git a/source/RobotAPI/interface/units/ForceTorqueUnit.ice b/source/RobotAPI/interface/units/ForceTorqueUnit.ice
index 8d0c2985c273e46a7343ea1ff77a344c2bd200de..5737239bcea41235ba6300ddf280cd90feb3fcfd 100644
--- a/source/RobotAPI/interface/units/ForceTorqueUnit.ice
+++ b/source/RobotAPI/interface/units/ForceTorqueUnit.ice
@@ -24,52 +24,53 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/core/RobotState.ice>
-#include <RobotAPI/interface/core/FramedPoseBase.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
-{
- /**
+{
+ /**
* Implements an interface to a ForceTorqueUnit.
**/
interface ForceTorqueUnitInterface extends armarx::SensorActorUnitInterface
{
- /**
+ /**
* setOffset allows to define offset forces and torques which are subtracted from the measured values.
* @param forceOffsets Offset forces in x, y, and z.
* @param torqueOffsets Offset torques in x, y, and z.
**/
void setOffset(FramedDirectionBase forceOffsets, FramedDirectionBase torqueOffsets);
- /**
+ /**
* setToNull allows to reset the ForceTorqueUnit by setting the measured values to zero.
**/
void setToNull();
};
- /**
+ /**
* Implements an interface to a ForceTorqueUnitListener
**/
interface ForceTorqueUnitListener
{
- /**
+ /**
* reportSensorValues reports measured forces and torques from the ForceTorqueUnit to ForceTorqueObserver.
* @param sensorNodeName Name of force/torque sensor.
* @param forces Measured forces.
* @param torques Measured torques.
**/
- void reportSensorValues(string sensorNodeName, FramedDirectionBase forces, FramedDirectionBase torques);
+ void reportSensorValues(
+ string sensorNodeName, FramedDirectionBase forces, FramedDirectionBase torques);
};
- /**
+ /**
* Implements an interface to a ForceTorqueUnitObserver. Provides topics on measured forces and torques which can be subscribed.
**/
interface ForceTorqueUnitObserverInterface extends ObserverInterface, ForceTorqueUnitListener
{
- /**
+ /**
* getForceDatafield returns a reference on the force topic.
* @param nodeName Name of force sensor topic.
**/
@@ -79,11 +80,10 @@ module armarx
* @param nodeName Name of torque sensor topic.
**/
DatafieldRefBase getTorqueDatafield(string nodeName) throws UserException;
- /**
+ /**
* createNulledDatafield resets the force/torque topic.
* @param forceTorqueDatafieldRef Reference of torque/torque sensor topic.
**/
DatafieldRefBase createNulledDatafield(DatafieldRefBase forceTorqueDatafieldRef);
};
};
-
diff --git a/source/RobotAPI/interface/units/GamepadUnit.ice b/source/RobotAPI/interface/units/GamepadUnit.ice
index c63ddcfdad36942cfcca6c65155c8e96c6847f79..ab61426270944c830a6f3f9fe0f2a0b71e6963f8 100644
--- a/source/RobotAPI/interface/units/GamepadUnit.ice
+++ b/source/RobotAPI/interface/units/GamepadUnit.ice
@@ -25,21 +25,21 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/core/RobotState.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/Matrix.ice>
-#include <ArmarXCore/interface/observers/Timestamp.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
{
- struct GamepadData {
+ struct GamepadData
+ {
float leftStickX;
float leftStickY;
float rightStickX;
@@ -60,22 +60,20 @@ module armarx
bool theMiddleButton;
bool leftStickButton;
bool rightStickButton;
-
};
+
interface GamepadUnitInterface // extends armarx::SensorActorUnitInterface
{
void vibrate();
};
interface GamepadUnitListener
- {
- void reportGamepadState(string device, string name, GamepadData values, TimestampBase timestamp);
+ {
+ void reportGamepadState(
+ string device, string name, GamepadData values, TimestampBase timestamp);
};
- /**
+ /**
* Implements an interface to an GamepadUnitObserver.
**/
- interface GamepadUnitObserverInterface extends ObserverInterface, GamepadUnitListener
- {
- };
-
+ interface GamepadUnitObserverInterface extends ObserverInterface, GamepadUnitListener{};
};
diff --git a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
index 6ae5638174ec0293a4a3ab73fea68e869a397cf9..12504b02be49535c2d99f4fce438066be3414626 100644
--- a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
+++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
@@ -24,9 +24,10 @@
#pragma once
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <RobotAPI/interface/core/FramedPoseBase.ice>
-#include <ArmarXCore/interface/observers/VariantBase.ice>
#include <ArmarXCore/interface/observers/RequestableService.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
#include <RobotAPI/interface/objectpose/object_pose_types.ice>
module armarx
@@ -38,11 +39,18 @@ module armarx
AnyObject, KnownObject, UnknownObject
};*/
- enum ApproachType {
- AnyApproach, TopApproach, SideApproach
+ enum ApproachType
+ {
+ AnyApproach,
+ TopApproach,
+ SideApproach
};
- enum ApertureType {
- AnyAperture, OpenAperture, PreshapedAperture
+
+ enum ApertureType
+ {
+ AnyAperture,
+ OpenAperture,
+ PreshapedAperture
};
class BoundingBox
@@ -60,6 +68,7 @@ module armarx
int segmentationLabelID = -1;
BoundingBox bbox;
};
+
class GraspCandidateExecutionHints
{
ApertureType preshape = AnyAperture;
@@ -67,6 +76,7 @@ module armarx
string graspTrajectoryName;
//string graspTrajectoryPackage;
};
+
class GraspCandidateReachabilityInfo
{
bool reachable = false;
@@ -76,8 +86,6 @@ module armarx
float maxOriError = 0;
};
-
-
class GraspCandidate
{
PoseBase graspPose;
@@ -85,14 +93,15 @@ module armarx
PoseBase tcpPoseInHandRoot;
Vector3Base approachVector;
- string sourceFrame; // frame where graspPose is located
- string targetFrame; // frame which should be moved to graspPose
+ string sourceFrame; // frame where graspPose is located
+ string targetFrame; // frame which should be moved to graspPose
string side;
float graspSuccessProbability;
armarx::objpose::ObjectType objectType = AnyObject;
- int groupNr = -1; // used to match candidates that belog together, e.g. from the same object or point cloud segment
+ int groupNr =
+ -1; // used to match candidates that belog together, e.g. from the same object or point cloud segment
string providerName;
GraspCandidateSourceInfo sourceInfo; // (optional)
@@ -114,13 +123,14 @@ module armarx
Vector3Base inwardsVectorRight;
Vector3Base inwardsVectorLeft;
- string sourceFrame; // frame where graspPose is located
- string targetFrame; // frame which should be moved to graspPose
+ string sourceFrame; // frame where graspPose is located
+ string targetFrame; // frame which should be moved to graspPose
//float graspSuccessProbability;
armarx::objpose::ObjectType objectType = AnyObject;
- int groupNr = -1; // used to match candidates that belog together, e.g. from the same object or point cloud segment
+ int groupNr =
+ -1; // used to match candidates that belog together, e.g. from the same object or point cloud segment
string providerName;
GraspCandidateSourceInfo sourceInfo; // (optional)
@@ -137,7 +147,6 @@ module armarx
dictionary<string, GraspCandidate> GraspCandidateDict;
sequence<BimanualGraspCandidate> BimanualGraspCandidateSeq;
-
class ProviderInfo
{
armarx::objpose::ObjectType objectType = AnyObject;
@@ -151,14 +160,10 @@ module armarx
{
void reportProviderInfo(string providerName, ProviderInfo info);
void reportGraspCandidates(string providerName, GraspCandidateSeq candidates);
- void reportBimanualGraspCandidates(string providerName, BimanualGraspCandidateSeq candidates);
-
+ void reportBimanualGraspCandidates(string providerName,
+ BimanualGraspCandidateSeq candidates);
};
- interface GraspCandidateProviderInterface extends RequestableServiceListenerInterface
- {
- };
+ interface GraspCandidateProviderInterface extends RequestableServiceListenerInterface{};
};
-
};
-
diff --git a/source/RobotAPI/interface/units/HapticUnit.ice b/source/RobotAPI/interface/units/HapticUnit.ice
index dcaec0c5c534e1455bb28692837f93e30276f654..3d9b8771c773ab08d491b1f06691662e48532d76 100644
--- a/source/RobotAPI/interface/units/HapticUnit.ice
+++ b/source/RobotAPI/interface/units/HapticUnit.ice
@@ -24,43 +24,38 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/core/RobotState.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/Matrix.ice>
-#include <ArmarXCore/interface/observers/Timestamp.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+
+#include <RobotAPI/interface/core/RobotState.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
{
- /**
+ /**
* Implements an interface to a HapitcUnit.
*/
- interface HapticUnitInterface extends armarx::SensorActorUnitInterface
- {
- };
- /**
+ interface HapticUnitInterface extends armarx::SensorActorUnitInterface{};
+ /**
* Implements an interface to a HapitcUnitListener.
*/
interface HapticUnitListener
{
- /**
+ /**
* reportSensorValues reports the tactile sensor values from a given sensor device.
* @param device Name of tactile sensor device.
* @param values Matrix of taxels.
* @param timestamp Timestamp of the measurement.
**/
- void reportSensorValues(string device, string name, MatrixFloatBase values, TimestampBase timestamp);
+ void reportSensorValues(
+ string device, string name, MatrixFloatBase values, TimestampBase timestamp);
};
- /**
+ /**
* Implements an interface to a HapitcUnitObserver.
*/
- interface HapticUnitObserverInterface extends ObserverInterface, HapticUnitListener
- {
- };
-
+ interface HapticUnitObserverInterface extends ObserverInterface, HapticUnitListener{};
};
-
diff --git a/source/RobotAPI/interface/units/HeadIKUnit.ice b/source/RobotAPI/interface/units/HeadIKUnit.ice
index d053a371f82b53fadc12c47e543dec8acd23d244..604f2574489e01e993558cb663ebc338eceb5114 100644
--- a/source/RobotAPI/interface/units/HeadIKUnit.ice
+++ b/source/RobotAPI/interface/units/HeadIKUnit.ice
@@ -24,23 +24,23 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/core/RobotState.ice>
-#include <RobotAPI/interface/core/FramedPoseBase.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
{
- /**
+ /**
* Implements an interface to a HeadIKUnit.
**/
interface HeadIKUnitInterface extends armarx::SensorActorUnitInterface
{
- /**
+ /**
* setCycleTime allows to set the cycle time with which the head IK is solved and the head is controlled within a periodic task.
* @param milliseconds Cycle time in milliseconds.
**/
@@ -51,15 +51,12 @@ module armarx
* @param targetPosition in x, y, and z.
**/
void setHeadTarget(string robotNodeSetName, FramedPositionBase targetPosition);
-
};
-
interface HeadIKUnitListener
{
- void reportHeadTargetChanged(NameValueMap targetJointAngles, FramedPositionBase targetPosition);
+ void reportHeadTargetChanged(NameValueMap targetJointAngles,
+ FramedPositionBase targetPosition);
};
-
};
-
diff --git a/source/RobotAPI/interface/units/InertialMeasurementUnit.ice b/source/RobotAPI/interface/units/InertialMeasurementUnit.ice
index 9acedc051830e4389af38d9e63a951b772a5d271..510a0509ea8ea15f4744ba77fefc8db5367b78e5 100644
--- a/source/RobotAPI/interface/units/InertialMeasurementUnit.ice
+++ b/source/RobotAPI/interface/units/InertialMeasurementUnit.ice
@@ -25,58 +25,55 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/core/RobotState.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/Matrix.ice>
-#include <ArmarXCore/interface/observers/Timestamp.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
{
- /**
+ /**
* Struct IMUData with which IMU sensor data is represented. It incorporates following entries:
* @param orientationQuaternion Orientation in quaternion.
* @param magneticRotation Magnetic rotation.
* @param gyroscopeRotation Rotation of gyroscope.
* @param acceleration Acceleration of IMU sensor device.
**/
- struct IMUData {
+ struct IMUData
+ {
Ice::FloatSeq orientationQuaternion;
Ice::FloatSeq magneticRotation;
- Ice::FloatSeq gyroscopeRotation;
+ Ice::FloatSeq gyroscopeRotation;
Ice::FloatSeq acceleration;
};
- /**
+
+ /**
* Implements an interface to an InertialMeasurementUnit.
**/
- interface InertialMeasurementUnitInterface extends armarx::SensorActorUnitInterface
- {
- };
- /**
+ interface InertialMeasurementUnitInterface extends armarx::SensorActorUnitInterface{};
+ /**
* Implements an interface to an InertialMeasurementUnitListener.
**/
interface InertialMeasurementUnitListener
- {
- /**
+ {
+ /**
* reportSensorValues reports the IMU sensor values from a given sensor device.
* @param device Name of IMU sensor device.
* @param values IMU sensor data.
* @param timestamp Timestamp of the measurement.
**/
- void reportSensorValues(string device, string name, IMUData values, TimestampBase timestamp);
+ void reportSensorValues(
+ string device, string name, IMUData values, TimestampBase timestamp);
};
- /**
+ /**
* Implements an interface to an InertialMeasurementUnitObserver.
**/
- interface InertialMeasurementUnitObserverInterface extends ObserverInterface, InertialMeasurementUnitListener
- {
- };
-
+ interface InertialMeasurementUnitObserverInterface extends ObserverInterface,
+ InertialMeasurementUnitListener{};
};
-
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterface.ice b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
index 3cc99f54cb8aa8785c3bfc7de09df8f5d7cf885e..be470e01ed1bad33aca7f0c56d42688bd12fde8f 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterface.ice
+++ b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
@@ -24,21 +24,20 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <RobotAPI/interface/core/NameValueMap.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
{
- /**
+ /**
* Struct RangeViolation for checking whether a given value is in given bounds:
**/
struct RangeViolation
{
- /**
+ /**
* @param rangeFrom Lower bound.
* @param rangeTo Upper bound.
* @param actualValue Actual value to be checked.
@@ -48,18 +47,19 @@ module armarx
float actualValue;
};
- /**
+ /**
* @param RangeViolationSequence Sequence of values and corresponding bounds to be checked.
**/
sequence<RangeViolation> RangeViolationSequence;
- /**
+ /**
* @throws OutOfRangeException Raised if value is violating the bounds.
**/
exception OutOfRangeException extends UserException
{
RangeViolationSequence violation;
};
- /**
+
+ /**
* [ControlMode] defines the different modes which a KinematicUnit can be controlled.
**/
enum ControlMode
@@ -71,7 +71,8 @@ module armarx
eTorqueControl,
ePositionVelocityControl
};
- /**
+
+ /**
* [OperationStatus] defines whether a KinematicUnit is online, offline, or initialized.
**/
enum OperationStatus
@@ -80,6 +81,7 @@ module armarx
eOnline,
eInitialized
};
+
/**
* [ErrorStatus] defines the error status of a KinematicUnit.
**/
@@ -89,6 +91,7 @@ module armarx
eWarning,
eError
};
+
/**
* JointStatus combines OperationStatus and ErrorStatus to describe the status of a joint.
* @see OperationStatus
@@ -98,11 +101,12 @@ module armarx
{
OperationStatus operation;
ErrorStatus error;
-
+
bool enabled;
bool emergencyStop;
};
- /**
+
+ /**
* ControlModeNotSupportedException Raised if a mode is requested which is not supported
* @see ControlMode
**/
@@ -110,14 +114,14 @@ module armarx
{
ControlMode mode;
};
- /**
+ /**
* KinematicUnitUnavailable Raised if the resource KinematicUnit is not available.
**/
exception KinematicUnitUnavailable extends ResourceUnavailableException
{
StringStringDictionary nodeOwners;
};
- /**
+ /**
* KinematicUnitNotOwnedException Raised if the resource KinematicUnit is not owned.
**/
exception KinematicUnitNotOwnedException extends ResourceNotOwnedException
@@ -137,7 +141,8 @@ module armarx
* [NameStatusMap] defined. This data container is mostly used to a status to e.g. a joint which is identified by name.
**/
dictionary<string, JointStatus> NameStatusMap;
- /**
+
+ /**
* Implements an interface to an KinematicUnit.
**/
@@ -172,14 +177,15 @@ module armarx
* After usage joints should be released so that another component can get access to these joints.
*/
void releaseJoints(Ice::StringSeq joints) throws KinematicUnitNotOwnedException;
-
+
/**
*
* switchControlMode allows switching control modes of joints specified in targetJointModes.
* @param targetJointModes defines target control modes and corresponding joints.
*/
- void switchControlMode(NameControlModeMap targetJointModes) throws ControlModeNotSupportedException;
-
+ void switchControlMode(NameControlModeMap targetJointModes)
+ throws ControlModeNotSupportedException;
+
/*
* Depending on the chosen control mode, one or more of the following functions need to be called
* to set all necessary parameters.
@@ -189,13 +195,15 @@ module armarx
void setJointAngles(NameValueMap targetJointAngles) throws OutOfRangeException;
void setJointVelocities(NameValueMap targetJointVelocities) throws OutOfRangeException;
void setJointTorques(NameValueMap targetJointTorques) throws OutOfRangeException;
- void setJointAccelerations(NameValueMap targetJointAccelerations) throws OutOfRangeException;
- void setJointDecelerations(NameValueMap targetJointDecelerations) throws OutOfRangeException;
-
- ["cpp:const"] NameValueMap getJointAngles();
- ["cpp:const"] NameValueMap getJointVelocities();
+ void setJointAccelerations(NameValueMap targetJointAccelerations)
+ throws OutOfRangeException;
+ void setJointDecelerations(NameValueMap targetJointDecelerations)
+ throws OutOfRangeException;
+
+ ["cpp:const"] NameValueMap getJointAngles();
+ ["cpp:const"] NameValueMap getJointVelocities();
["cpp:const"] Ice::StringSeq getJoints();
-
+
/*!
* Returns the current control mode setup of all joints.
* @return List with current control modes
@@ -205,39 +213,32 @@ module armarx
/**
* @return the robot xml filename as specified in the configuration
*/
- ["cpp:const"]
- idempotent
- string getRobotFilename();
+ ["cpp:const"] idempotent string getRobotFilename();
/**
* @return All dependent packages, which might contain a robot file.
*/
- ["cpp:const"]
- idempotent
- Ice::StringSeq getArmarXPackages();
+ ["cpp:const"] idempotent Ice::StringSeq getArmarXPackages();
/**
* @return The name of the robot used by this component.
*
*/
- ["cpp:const"]
- idempotent string getRobotName() throws NotInitializedException;
+ ["cpp:const"] idempotent string getRobotName() throws NotInitializedException;
/**
* @return The name of the robot node set that is used by this component.
*
*/
- ["cpp:const"]
- idempotent string getRobotNodeSetName() throws NotInitializedException;
+ ["cpp:const"] idempotent string getRobotNodeSetName() throws NotInitializedException;
/**
* @return The name of the report topic that is offered by this unit.
*
*/
- ["cpp:const"]
- idempotent string getReportTopicName() throws NotInitializedException;
+ ["cpp:const"] idempotent string getReportTopicName() throws NotInitializedException;
- /*
+ /*
* NYI
*/
//void set Trajectory(...);
@@ -251,74 +252,78 @@ module armarx
* @return
*
*/
- ["cpp:const"]
- DebugInfo getDebugInfo();
+ ["cpp:const"] DebugInfo getDebugInfo();
};
-
-
- /**
+ /**
* Implements an interface to an KinematicUnitListener.
**/
interface KinematicUnitListener
{
- /**
+ /**
* reportControlModeChanged reports if a ControlMode has changed.
* @param actualJointModes Map of control modes and corresponding joint names.
* @param aValueChanged Is set to true if a mode has changed.
**/
- void reportControlModeChanged(NameControlModeMap actualJointModes, long timestamp, bool aValueChanged);
- /**
+ void reportControlModeChanged(
+ NameControlModeMap actualJointModes, long timestamp, bool aValueChanged);
+ /**
* reportJointAngles reports joint angle values.
* @param actualJointAngles Map of joint angle values and corresponding joint names.
* @param aValueChanged Is set to true if a joint angle value has changed.
**/
void reportJointAngles(NameValueMap actualJointAngles, long timestamp, bool aValueChanged);
-
- /**
+
+ /**
* reportJointVelocities reports joint angle velocities.
* @param actualJointVelocities Map of joint angle velocities and corresponding joint names.
* @param aValueChanged Is set to true if a joint angle velocity has changed.
**/
- void reportJointVelocities(NameValueMap actualJointVelocities, long timestamp, bool aValueChanged);
-
- /**
+ void reportJointVelocities(
+ NameValueMap actualJointVelocities, long timestamp, bool aValueChanged);
+
+ /**
* reportJointTorques reports joint torques.
* @param actualJointTorques Map of joint torques and corresponding joint names.
* @param aValueChanged Is set to true if a joint torque has changed.
**/
- void reportJointTorques(NameValueMap actualJointTorques, long timestamp, bool aValueChanged);
-
- /**
+ void reportJointTorques(
+ NameValueMap actualJointTorques, long timestamp, bool aValueChanged);
+
+ /**
* reportJointAccelerations reports joint accelerations.
* @param actualJointAccelerations Map of joint accelerations and corresponding joint names.
* @param aValueChanged Is set to true if a joint acceleration has changed.
**/
- void reportJointAccelerations(NameValueMap actualJointAccelerations, long timestamp, bool aValueChanged);
-
- /**
+ void reportJointAccelerations(
+ NameValueMap actualJointAccelerations, long timestamp, bool aValueChanged);
+
+ /**
* reportJointCurrents reports joint currents.
* @param actualJointCurrents Map of joint currents and corresponding joint names.
* @param aValueChanged Is set to true if a joint current has changed.
**/
- void reportJointCurrents(NameValueMap actualJointCurrents, long timestamp, bool aValueChanged);
-
- /**
+ void reportJointCurrents(
+ NameValueMap actualJointCurrents, long timestamp, bool aValueChanged);
+
+ /**
* reportJointMotorTemperatures reports joint motor temperatures.
* @param actualJointMotorTemperatures Map of joint motor temperatures and corresponding joint names.
* @param aValueChanged Is set to true if a joint motor temperature has changed.
**/
- void reportJointMotorTemperatures(NameValueMap actualJointMotorTemperatures, long timestamp, bool aValueChanged);
-
- /**
+ void reportJointMotorTemperatures(
+ NameValueMap actualJointMotorTemperatures, long timestamp, bool aValueChanged);
+
+ /**
* reportJointStatuses reports current joint statuses.
* @param actualJointStatuses Map of joint statuses and corresponding joint names.
* @param aValueChanged Is set to true if a joint status has changed.
**/
- void reportJointStatuses(NameStatusMap actualJointStatuses, long timestamp, bool aValueChanged);
+ void reportJointStatuses(
+ NameStatusMap actualJointStatuses, long timestamp, bool aValueChanged);
};
-
+
/*interface KinematicUnitHandInterface extends KinematicUnitInterface
{
void open();
@@ -335,5 +340,4 @@ module armarx
void reportTactileSensorRawData(NameValueMap actualTactileSensorRawData);
void reportTactileSensorContactData(NameValueMap actualTactileSensorContactData);
};*/
-
};
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.cpp b/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.cpp
index da6b2dfc7d14310315e8f716e303657c0bdda2d5..4028eec3e902931492cf3c168342a47d4bdad228 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.cpp
+++ b/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.cpp
@@ -25,7 +25,8 @@
namespace armarx
{
- std::string to_string(const ErrorStatus& status)
+ std::string
+ to_string(const ErrorStatus& status)
{
switch (status)
{
@@ -43,7 +44,8 @@ namespace armarx
}
}
- std::string to_string(const OperationStatus& status)
+ std::string
+ to_string(const OperationStatus& status)
{
switch (status)
{
@@ -60,4 +62,4 @@ namespace armarx
return "?";
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h b/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h
index 44494ec0e2377e379ec3ccb466e299e595c6781d..3a5fbb529d47aefb4a7d892de0895d0901576794 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h
+++ b/source/RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h
@@ -31,4 +31,4 @@ namespace armarx
{
std::string to_string(const ErrorStatus& status);
std::string to_string(const OperationStatus& status);
-}
+} // namespace armarx
diff --git a/source/RobotAPI/interface/units/LaserScannerUnit.ice b/source/RobotAPI/interface/units/LaserScannerUnit.ice
index cf3a48562253065449dcdceb30030573f1a85e87..98477fc262a32fac283a863459378f8ee0db7d5e 100644
--- a/source/RobotAPI/interface/units/LaserScannerUnit.ice
+++ b/source/RobotAPI/interface/units/LaserScannerUnit.ice
@@ -25,21 +25,20 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/core/RobotState.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/Matrix.ice>
-#include <ArmarXCore/interface/observers/Timestamp.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
{
- /**
+ /**
* Struct LaserScanStep with which a single scan step is represented. It incorporates following entries:
* @param angle Angle in which direction a distance was measured [rad].
* @param distance The measured distance [mm].
@@ -65,20 +64,17 @@ module armarx
interface LaserScannerUnitInterface extends armarx::SensorActorUnitInterface
{
- ["cpp:const"]
- idempotent string getReportTopicName() throws NotInitializedException;
+ ["cpp:const"] idempotent string getReportTopicName() throws NotInitializedException;
- ["cpp:const"]
- LaserScannerInfoSeq getConnectedDevices();
+ ["cpp:const"] LaserScannerInfoSeq getConnectedDevices();
};
interface LaserScannerUnitListener
{
- void reportSensorValues(string device, string name, LaserScan values, TimestampBase timestamp);
- };
-
- interface LaserScannerUnitObserverInterface extends ObserverInterface, LaserScannerUnitListener
- {
+ void reportSensorValues(
+ string device, string name, LaserScan values, TimestampBase timestamp);
};
+ interface LaserScannerUnitObserverInterface extends ObserverInterface,
+ LaserScannerUnitListener{};
};
diff --git a/source/RobotAPI/interface/units/LocalizationUnitInterface.ice b/source/RobotAPI/interface/units/LocalizationUnitInterface.ice
index 8799ee4f0e75822c2c6d59dfb77212213ec1f7bf..bbb188591db956d71b96c9ead006a83f5c4e9b6e 100644
--- a/source/RobotAPI/interface/units/LocalizationUnitInterface.ice
+++ b/source/RobotAPI/interface/units/LocalizationUnitInterface.ice
@@ -24,30 +24,26 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <RobotAPI/interface/core/RobotLocalization.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <RobotAPI/interface/core/GeometryBase.ice>
+#include <RobotAPI/interface/core/RobotLocalization.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
-{
-
+{
+
interface LocalizationUnitInterface extends SensorActorUnitInterface
{
void reportGlobalRobotPoseCorrection(TransformStamped global_T_odom);
};
- interface LocalizationUnitListener
- {
-
+ interface LocalizationUnitListener{
+
};
- interface LocalizationSubUnitInterface extends LocalizationUnitInterface
- {
+ interface LocalizationSubUnitInterface extends LocalizationUnitInterface{
};
-
};
diff --git a/source/RobotAPI/interface/units/MetaWearIMU.ice b/source/RobotAPI/interface/units/MetaWearIMU.ice
index 7cd97f8e43df797e4915242fbd8e3db59f09bec7..2701487c516ebac76c7e4dc3cb752979d0212993 100644
--- a/source/RobotAPI/interface/units/MetaWearIMU.ice
+++ b/source/RobotAPI/interface/units/MetaWearIMU.ice
@@ -24,14 +24,16 @@
#pragma once
-#include <RobotAPI/interface/core/PoseBase.ice>
-#include <RobotAPI/interface/units/UnitInterface.ice>
#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
+
module armarx
{
- struct MetaWearIMUData {
+ struct MetaWearIMUData
+ {
Ice::FloatSeq orientationQuaternion;
Ice::FloatSeq magnetic;
Ice::FloatSeq gyro;
@@ -42,8 +44,5 @@ module armarx
{
void reportIMUValues(string name, MetaWearIMUData data, TimestampBase timestamp);
};
- interface MetaWearIMUObserverInterface extends ObserverInterface, MetaWearIMUListener
- {
- };
+ interface MetaWearIMUObserverInterface extends ObserverInterface, MetaWearIMUListener{};
};
-
diff --git a/source/RobotAPI/interface/units/MetaWearIMUInterface.ice b/source/RobotAPI/interface/units/MetaWearIMUInterface.ice
index c360a1e100cc6a124ed93a376af76c2e7b46a457..13f79c71378539e39ddd6a67e68b188e9c13148d 100644
--- a/source/RobotAPI/interface/units/MetaWearIMUInterface.ice
+++ b/source/RobotAPI/interface/units/MetaWearIMUInterface.ice
@@ -29,7 +29,8 @@ module armarx
{
sequence<float> FloatSeq;
- struct MetaWearIMUDataExt {
+ struct MetaWearIMUDataExt
+ {
FloatSeq orientationQuaternion;
FloatSeq magnetic;
FloatSeq gyro;
@@ -41,4 +42,3 @@ module armarx
void reportIMUValues(string name, MetaWearIMUDataExt data);
};
};
-
diff --git a/source/RobotAPI/interface/units/MultiHandUnitInterface.ice b/source/RobotAPI/interface/units/MultiHandUnitInterface.ice
index 4f107495c73a0fab746b6ef5d3607f0e6c430b5b..1308c97206b46a15b7b7ec6e2aaf955041bc2489 100644
--- a/source/RobotAPI/interface/units/MultiHandUnitInterface.ice
+++ b/source/RobotAPI/interface/units/MultiHandUnitInterface.ice
@@ -37,6 +37,7 @@ module armarx
string handName;
//Ice::StringSeq jointNames;
};
+
sequence<HandInfo> HandInfoSeq;
interface MultiHandUnitInterface
@@ -46,4 +47,3 @@ module armarx
NameValueMap getJointValues(string handName);
};
};
-
diff --git a/source/RobotAPI/interface/units/OptoForceUnit.ice b/source/RobotAPI/interface/units/OptoForceUnit.ice
index 8b893b11bb1fe323ee69f5498af434584d993cfc..acd5a4f1d3232785c9714f776c945e33173d62e0 100644
--- a/source/RobotAPI/interface/units/OptoForceUnit.ice
+++ b/source/RobotAPI/interface/units/OptoForceUnit.ice
@@ -25,21 +25,20 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/core/RobotState.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/Matrix.ice>
-#include <ArmarXCore/interface/observers/Timestamp.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
{
- /**
+ /**
* Implements an interface to an OptoForceUnit.
**/
interface OptoForceUnitInterface //extends armarx::SensorActorUnitInterface
@@ -47,25 +46,22 @@ module armarx
void startRecording(string filepath);
void stopRecording();
};
- /**
+ /**
* Implements an interface to an OptoForceUnitListener.
**/
interface OptoForceUnitListener
- {
- /**
+ {
+ /**
* reportSensorValues reports the IMU sensor values from a given sensor device.
* @param device Name of IMU sensor device.
* @param values IMU sensor data.
* @param timestamp Timestamp of the measurement.
**/
- void reportSensorValues(string device, string name, float fx, float fy, float fz, TimestampBase timestamp);
+ void reportSensorValues(
+ string device, string name, float fx, float fy, float fz, TimestampBase timestamp);
};
- /**
+ /**
* Implements an interface to an OptoForceUnitObserver.
**/
- interface OptoForceUnitObserverInterface extends ObserverInterface, OptoForceUnitListener
- {
- };
-
+ interface OptoForceUnitObserverInterface extends ObserverInterface, OptoForceUnitListener{};
};
-
diff --git a/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice b/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
index 7d14330d295e1aab314d66842ba2f5cedefba8e6..3ec6fcde73fe339238f81d65447c39fa8e83c87d 100644
--- a/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
+++ b/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
@@ -22,10 +22,11 @@
#pragma once
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/observers/Timestamp.ice>
+
#include <RobotAPI/interface/core/PoseBase.ice>
#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <ArmarXCore/interface/observers/Timestamp.ice>
-#include <ArmarXCore/interface/core/UserException.ice>
module armarx
{
@@ -38,20 +39,30 @@ module armarx
};
- interface OrientedTactileSensorUnitInterface extends armarx::SensorActorUnitInterface
- {
+ interface OrientedTactileSensorUnitInterface extends armarx::SensorActorUnitInterface{
//bool loadCalibration(int accel_offset_x, int accel_offset_y, int accel_offset_z, int gyro_offset_x, int gyro_offset_y, int gyro_offset_z, int mag_offset_x, int mag_offset_y, int mag_offset_z, int accel_radius, int mag_radius );
};
- interface OrientedTactileSensorUnitListener
+ interface OrientedTactileSensorUnitListener
{
- void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float pressureRate, float rotationRate, float accelerationRate, float accelx, float accely, float accelz, TimestampBase timestamp);
+ void reportSensorValues(int id,
+ float pressure,
+ float qw,
+ float qx,
+ float qy,
+ float qz,
+ float pressureRate,
+ float rotationRate,
+ float accelerationRate,
+ float accelx,
+ float accely,
+ float accelz,
+ TimestampBase timestamp);
};
- interface OrientedTactileSensorUnitObserverInterface extends ObserverInterface, OrientedTactileSensorUnitListener
- {
+ interface OrientedTactileSensorUnitObserverInterface extends ObserverInterface,
+ OrientedTactileSensorUnitListener{
- };
+ };
};
-
diff --git a/source/RobotAPI/interface/units/PlatformUnitInterface.ice b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
index f83ad97cf10f83c8975a8c79f1b44deecd594d4e..88bfe4ebf08d261bbd5326c8d365577ff7333604 100644
--- a/source/RobotAPI/interface/units/PlatformUnitInterface.ice
+++ b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
@@ -115,7 +115,5 @@ module armarx
reportPlatformOdometryPose(float x, float y, float angle);
};
- interface PlatformSubUnitInterface extends PlatformUnitInterface
- {
- };
+ interface PlatformSubUnitInterface extends PlatformUnitInterface{};
};
diff --git a/source/RobotAPI/interface/units/RobotPoseUnitInterface.ice b/source/RobotAPI/interface/units/RobotPoseUnitInterface.ice
index 7864ce45e391d73cc63b1e9a8875fc8527bd2b18..b98482721d34f3c7a7fbe109ee7ca517353096e1 100644
--- a/source/RobotAPI/interface/units/RobotPoseUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotPoseUnitInterface.ice
@@ -24,11 +24,11 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/core/PoseBase.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
+
+#include <RobotAPI/interface/core/PoseBase.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
{
@@ -57,7 +57,8 @@ module armarx
* @param postionalAccuracy Platform stops translating if distance to target position gets lower than this threshhold.
* @param orientationalAccuracy Platform stops rotating if distance from current to target orientation gets lower than this threshhold.
**/
- void moveRelative(PoseBase relativeTarget, float positionalAccuracy, float orientationalAccuracy);
+ void moveRelative(
+ PoseBase relativeTarget, float positionalAccuracy, float orientationalAccuracy);
/**
* setMaxVelocities allows to specify max velocities in translation and orientation.
* @param positionalVelocity Max translation velocity.
@@ -90,6 +91,4 @@ module armarx
**/
void reportRobotVelocity(PoseBase currentRobotvelocity);
};
-
};
-
diff --git a/source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice b/source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice
index 4b68726211fb66a76d68a5253f7b667d6cd39691..e3f9912aa1ddace166a1538469c71cbd221db673 100644
--- a/source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/DSControllerBase.ice
@@ -55,11 +55,9 @@ module armarx
float positionErrorTolerance;
float dsAdaptorEpsilon;
-
};
- interface DSControllerInterface extends NJointControllerInterface
- {
+ interface DSControllerInterface extends NJointControllerInterface{
};
@@ -80,9 +78,9 @@ module armarx
float torqueLimit = 1;
float NullTorqueLimit = 0.2;
-// string tcpName = "";
+ // string tcpName = "";
-// Ice::FloatSeq desiredPosition;
+ // Ice::FloatSeq desiredPosition;
Ice::FloatSeq left_desiredQuaternion;
Ice::FloatSeq right_desiredQuaternion;
@@ -94,7 +92,6 @@ module armarx
float nullspaceDamping;
-
string gmmParamsFile = "";
float positionErrorTolerance;
@@ -127,11 +124,9 @@ module armarx
Ice::FloatSeq forceRightOffset;
};
-
interface DSBimanualControllerInterface extends NJointControllerInterface
{
void setToDefaultTarget();
-
};
class DSJointCarryControllerConfig extends NJointControllerConfig
@@ -166,7 +161,6 @@ module armarx
Ice::FloatSeq defaultRotationStiffness;
};
-
interface DSJointCarryControllerInterface extends NJointControllerInterface
{
void setGuardInHandPosition(Ice::FloatSeq guardCenterToHandsCenter);
@@ -176,5 +170,4 @@ module armarx
void setGuardObsAvoidVel(Ice::FloatSeq guardZVel);
float getGMMVel();
};
-
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointActiveImpedanceController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointActiveImpedanceController.ice
index da687c5d0dd5770d34be1ed680d81f2711543606..d076e749285b9857af73da62eee58db1e107e52d 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointActiveImpedanceController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointActiveImpedanceController.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -41,7 +41,6 @@ module armarx
string deviceName;
};
-
interface NJointActiveImpedanceControllerInterface extends NJointControllerInterface
{
void setControllerTarget(float position, float kp, float kd);
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.ice
index 044d9f64fae26fc6a4f842ca2e82bea5eab8fca9..4f1cdd3083586ea9674b1b8954979aa09a163a43 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualCartesianAdmittanceController.ice
@@ -24,8 +24,8 @@
#include <ArmarXCore/interface/serialization/Eigen.ice>
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -48,6 +48,7 @@ module armarx
float k;
float d;
};
+
struct Impedance
{
Eigen::Vector3f KpXYZ;
@@ -55,6 +56,7 @@ module armarx
Eigen::Vector3f KdXYZ;
Eigen::Vector3f KdRPY;
};
+
struct Admittance
{
Eigen::Vector3f KpXYZ;
@@ -64,6 +66,7 @@ module armarx
Eigen::Vector3f KmXYZ;
Eigen::Vector3f KmRPY;
};
+
struct Force
{
Eigen::Vector3f wrenchXYZ;
@@ -116,16 +119,17 @@ module armarx
void setDesiredJointValuesRight(Ice::FloatSeq cfg);
void setNullspaceConfig(detail::NJBmanCartAdmCtrl::Nullspace nullspace);
void setAdmittanceConfig(detail::NJBmanCartAdmCtrl::Admittance admittanceObject);
- void setForceConfig(detail::NJBmanCartAdmCtrl::Force left, detail::NJBmanCartAdmCtrl::Force right);
- void setImpedanceConfig(detail::NJBmanCartAdmCtrl::Impedance left, detail::NJBmanCartAdmCtrl::Impedance right);
+ void setForceConfig(detail::NJBmanCartAdmCtrl::Force left,
+ detail::NJBmanCartAdmCtrl::Force right);
+ void setImpedanceConfig(detail::NJBmanCartAdmCtrl::Impedance left,
+ detail::NJBmanCartAdmCtrl::Impedance right);
["cpp:const"] Eigen::Matrix4f getBoxPose();
void setBoxPose(Eigen::Matrix4f pose);
void setBoxWidth(float w);
void setBoxVelocity(Eigen::Vector3f velXYZ, Eigen::Vector3f velRPY);
- void setBoxPoseAndVelocity(Eigen::Matrix4f pose,
- Eigen::Vector3f velXYZ,
- Eigen::Vector3f velRPY);
+ void setBoxPoseAndVelocity(
+ Eigen::Matrix4f pose, Eigen::Vector3f velXYZ, Eigen::Vector3f velRPY);
void moveBoxPose(Eigen::Matrix4f pose);
void moveBoxPosition(Eigen::Vector3f pos);
@@ -135,8 +139,4 @@ module armarx
// Eigen::Vector3f velRPY;
*/
};
-
-
-
};
-
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceController.ice
index a173e758341b325de617a2775404ff1de982cb8b..5c27307a9a1c9d4ffa8ce19a147fb2340503a53b 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceController.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -59,8 +59,8 @@ module armarx
Ice::FloatSeq KdAdmittance;
Ice::FloatSeq KmAdmittance;
Ice::FloatSeq KmPID;
-// Ice::FloatSeq objectKp;
-// Ice::FloatSeq objectKd;
+ // Ice::FloatSeq objectKp;
+ // Ice::FloatSeq objectKd;
// pid force controller parameters
Ice::FloatSeq targetWrench;
@@ -74,20 +74,20 @@ module armarx
float massLeft;
Ice::FloatSeq CoMVecLeft;
- Ice::FloatSeq forceOffsetLeft;
+ Ice::FloatSeq forceOffsetLeft;
Ice::FloatSeq torqueOffsetLeft;
float massRight;
Ice::FloatSeq CoMVecRight;
- Ice::FloatSeq forceOffsetRight;
+ Ice::FloatSeq forceOffsetRight;
Ice::FloatSeq torqueOffsetRight;
-// // flags for testing variants
-// int dmpFeedType; // 0: no dmp, 1: frame, 2: vel, 3: acc, 4: force
-// int method; // 1,2,3,4 four diffrent method w.r.t inertia trick; default (0): normal method
-// int jcMethod; // 0: lin's paper, default: Jc = J
-// int pdotMethod; // 0: numerical differential, 1: paper, Tau - Tau.Transpose, 2 test, default: zero matrix
-// int graspingType; // 0: fixed to obj, 1, fixed to hand, 2, dynamically from hand pose, defualt: identity
+ // // flags for testing variants
+ // int dmpFeedType; // 0: no dmp, 1: frame, 2: vel, 3: acc, 4: force
+ // int method; // 1,2,3,4 four diffrent method w.r.t inertia trick; default (0): normal method
+ // int jcMethod; // 0: lin's paper, default: Jc = J
+ // int pdotMethod; // 0: numerical differential, 1: paper, Tau - Tau.Transpose, 2 test, default: zero matrix
+ // int graspingType; // 0: fixed to obj, 1, fixed to hand, 2, dynamically from hand pose, defualt: identity
float knull;
float dnull;
@@ -95,7 +95,6 @@ module armarx
float torqueLimit;
float forceThreshold;
-
};
interface NJointBimanualForceControllerInterface extends NJointControllerInterface
@@ -104,15 +103,9 @@ module armarx
bool isFinished();
void runDMP(Ice::DoubleSeq goals);
-// void setViaPoints(double canVal, Ice::DoubleSeq point);
+ // void setViaPoints(double canVal, Ice::DoubleSeq point);
void setGoals(Ice::DoubleSeq goals);
void setViaPoints(double u, Ice::DoubleSeq viapoint);
double getVirtualTime();
-
-
};
-
-
-
};
-
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
index 49acfeefb9fec34ea497c6209d8a0b1c3c846c9b..66f0f3b61783f779e7759cc468ec32afa26dcdf9 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualForceMPController.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -67,10 +67,8 @@ module armarx
Ice::FloatSeq rightForceOffset;
string debugName;
-
};
-
interface NJointBimanualForceMPControllerInterface extends NJointControllerInterface
{
void learnDMPFromFiles(string whichMP, Ice::StringSeq trajfiles);
@@ -81,4 +79,3 @@ module armarx
void setViaPoints(string whichDMP, double canVal, Ice::DoubleSeq viaPoint);
};
};
-
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
index 7488ad443518fc4fbce80b247a2a0070acafd5e5..a03826f6b1c33d2a611d6c06a42a3e599194f7b3 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -59,8 +59,8 @@ module armarx
Ice::FloatSeq KdAdmittance;
Ice::FloatSeq KmAdmittance;
Ice::FloatSeq KmPID;
-// Ice::FloatSeq objectKp;
-// Ice::FloatSeq objectKd;
+ // Ice::FloatSeq objectKp;
+ // Ice::FloatSeq objectKd;
// pid force controller parameters
Ice::FloatSeq targetWrench;
@@ -74,20 +74,20 @@ module armarx
float massLeft;
Ice::FloatSeq CoMVecLeft;
- Ice::FloatSeq forceOffsetLeft;
+ Ice::FloatSeq forceOffsetLeft;
Ice::FloatSeq torqueOffsetLeft;
float massRight;
Ice::FloatSeq CoMVecRight;
- Ice::FloatSeq forceOffsetRight;
+ Ice::FloatSeq forceOffsetRight;
Ice::FloatSeq torqueOffsetRight;
-// // flags for testing variants
-// int dmpFeedType; // 0: no dmp, 1: frame, 2: vel, 3: acc, 4: force
-// int method; // 1,2,3,4 four diffrent method w.r.t inertia trick; default (0): normal method
-// int jcMethod; // 0: lin's paper, default: Jc = J
-// int pdotMethod; // 0: numerical differential, 1: paper, Tau - Tau.Transpose, 2 test, default: zero matrix
-// int graspingType; // 0: fixed to obj, 1, fixed to hand, 2, dynamically from hand pose, defualt: identity
+ // // flags for testing variants
+ // int dmpFeedType; // 0: no dmp, 1: frame, 2: vel, 3: acc, 4: force
+ // int method; // 1,2,3,4 four diffrent method w.r.t inertia trick; default (0): normal method
+ // int jcMethod; // 0: lin's paper, default: Jc = J
+ // int pdotMethod; // 0: numerical differential, 1: paper, Tau - Tau.Transpose, 2 test, default: zero matrix
+ // int graspingType; // 0: fixed to obj, 1, fixed to hand, 2, dynamically from hand pose, defualt: identity
float knull;
float dnull;
@@ -97,7 +97,6 @@ module armarx
Ice::FloatSeq forceThreshold;
double ftCalibrationTime;
-
};
interface NJointBimanualObjLevelControllerInterface extends NJointControllerInterface
@@ -105,9 +104,10 @@ module armarx
void learnDMPFromFiles(Ice::StringSeq trajfiles);
bool isFinished();
void runDMP(Ice::DoubleSeq goals, double timeDuration);
- void runDMPWithVirtualStart(Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
+ void runDMPWithVirtualStart(
+ Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
-// void setViaPoints(double canVal, Ice::DoubleSeq point);
+ // void setViaPoints(double canVal, Ice::DoubleSeq point);
void setGoals(Ice::DoubleSeq goals);
void setViaPoints(double u, Ice::DoubleSeq viapoint);
void removeAllViaPoints();
@@ -148,7 +148,6 @@ module armarx
float jointVelLimit;
float jointLimitAvoidanceKp;
-
};
interface NJointBimanualObjLevelVelControllerInterface extends NJointControllerInterface
@@ -156,7 +155,8 @@ module armarx
void learnDMPFromFiles(Ice::StringSeq trajfiles);
bool isFinished();
void runDMP(Ice::DoubleSeq goals, double timeDuration);
- void runDMPWithVirtualStart(Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
+ void runDMPWithVirtualStart(
+ Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
void setGoals(Ice::DoubleSeq goals);
void setViaPoints(double u, Ice::DoubleSeq viapoint);
@@ -235,21 +235,27 @@ module armarx
Ice::FloatSeq forceThreshold;
double ftCalibrationTime;
-
};
interface NJointBimanualObjLevelMultiMPControllerInterface extends NJointControllerInterface
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
- void learnMultiDMPFromFiles(Ice::StringSeq objFileNames, Ice::StringSeq leftFileNames, Ice::StringSeq rightFileNames);
+ void learnMultiDMPFromFiles(Ice::StringSeq objFileNames,
+ Ice::StringSeq leftFileNames,
+ Ice::StringSeq rightFileNames);
bool isFinished();
- void runDMP(Ice::DoubleSeq goalObj, Ice::DoubleSeq goalLeft, Ice::DoubleSeq goalRight, double timeDuration);
- void runDMPWithVirtualStart(Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
-
-// void setViaPoints(double canVal, Ice::DoubleSeq point);
+ void runDMP(Ice::DoubleSeq goalObj,
+ Ice::DoubleSeq goalLeft,
+ Ice::DoubleSeq goalRight,
+ double timeDuration);
+ void runDMPWithVirtualStart(
+ Ice::DoubleSeq starts, Ice::DoubleSeq goals, double timeDuration);
+
+ // void setViaPoints(double canVal, Ice::DoubleSeq point);
void setGoals(Ice::DoubleSeq goals);
- void setMultiMPGoals(Ice::DoubleSeq goalObj, Ice::DoubleSeq goalLeft, Ice::DoubleSeq goalRight);
+ void setMultiMPGoals(
+ Ice::DoubleSeq goalObj, Ice::DoubleSeq goalLeft, Ice::DoubleSeq goalRight);
void setViaPoints(double u, Ice::DoubleSeq viapoint);
void removeAllViaPoints();
@@ -266,6 +272,4 @@ module armarx
Ice::FloatSeq getCurrentObjVel();
Ice::FloatSeq getCurrentObjForce();
};
-
};
-
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
index 7516fa77cb26ca6355da211e1db459d640337eee..19d376df5f2f07fdf41007adeb010b5b5526dc66 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
@@ -22,12 +22,12 @@
#pragma once
+#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
#include <ArmarXCore/interface/serialization/Eigen.ice>
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-#include <RobotAPI/interface/core/FTSensorValue.ice>
#include <RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice>
-#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+#include <RobotAPI/interface/core/FTSensorValue.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -49,7 +49,10 @@ module armarx
void setConfig(CartesianNaturalPositionControllerConfig cfg);
void setTarget(Eigen::Matrix4f tcpTarget, Eigen::Vector3f elbowTarget, bool setOrientation);
- void setTargetFeedForward(Eigen::Matrix4f tcpTarget, Eigen::Vector3f elbowTarget, bool setOrientation, Eigen::Vector6f ffVel);
+ void setTargetFeedForward(Eigen::Matrix4f tcpTarget,
+ Eigen::Vector3f elbowTarget,
+ bool setOrientation,
+ Eigen::Vector6f ffVel);
void setFeedForwardVelocity(Eigen::Vector6f vel);
void clearFeedForwardVelocity();
void setNullspaceTarget(Ice::FloatSeq nullspaceTarget);
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianTorqueController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianTorqueController.ice
index d53e0347f931497b77b67474d813570edcacc70f..dc76b734dc079139a45963b37273cf8a703340ac 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianTorqueController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianTorqueController.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -34,10 +34,9 @@ module armarx
string tcpName = "";
};
-
-
interface NJointCartesianTorqueControllerInterface extends NJointControllerInterface
{
- void setControllerTarget(float forceX, float forceY, float forceZ, float torqueX, float torqueY, float torqueZ);
+ void setControllerTarget(
+ float forceX, float forceY, float forceZ, float torqueX, float torqueY, float torqueZ);
};
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.ice
index d9bf18450de19a2a01959f46f17e40b6fedd5bac..d1c8cf9990fdc8a960053d34ee3c396bd53071a9 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -41,14 +41,20 @@ module armarx
{
string nodeSetName = "";
string tcpName = "";
- NJointCartesianVelocityControllerMode::CartesianSelection mode = NJointCartesianVelocityControllerMode::eAll;
+ NJointCartesianVelocityControllerMode::CartesianSelection mode =
+ NJointCartesianVelocityControllerMode::eAll;
};
-
-
interface NJointCartesianVelocityControllerInterface extends NJointControllerInterface
{
- void setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerMode::CartesianSelection mode);
+ void setControllerTarget(float x,
+ float y,
+ float z,
+ float roll,
+ float pitch,
+ float yaw,
+ float avoidJointLimitsKp,
+ NJointCartesianVelocityControllerMode::CartesianSelection mode);
void setTorqueKp(StringFloatDictionary torqueKp);
void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice
index a8d12ab7a089772dd546bce271ccd345e05916b8..b823c143b7f30ba65b89ed09ba9b88a055ce5f34 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/CartesianSelection.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -42,7 +42,9 @@ module armarx
interface NJointCartesianVelocityControllerWithRampInterface extends NJointControllerInterface
{
- void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration);
+ void setMaxAccelerations(float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration);
void setTargetVelocity(float vx, float vy, float vz, float vrx, float vry, float vrz);
void immediateHardStop();
void setJointLimitAvoidanceScale(float scale);
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
index f02c8e8ee8720f7a45fb730f155986016cdb0331..04754194d28cbec7eba1ad299b3266995ea36aca 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -24,9 +24,9 @@
#include <ArmarXCore/interface/serialization/Eigen.ice>
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-#include <RobotAPI/interface/core/FTSensorValue.ice>
#include <RobotAPI/interface/core/CartesianWaypointControllerConfig.ice>
+#include <RobotAPI/interface/core/FTSensorValue.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -57,9 +57,11 @@ module armarx
void setWaypoints(Eigen::Matrix4fSeq wps);
void setWaypoint(Eigen::Matrix4f wp);
void setWaypointAx(Ice::FloatSeq wp);
- void setConfigAndWaypoints(NJointCartesianWaypointControllerRuntimeConfig cfg, Eigen::Matrix4fSeq wps);
- void setConfigAndWaypoint(NJointCartesianWaypointControllerRuntimeConfig cfg, Eigen::Matrix4f wp);
-
+ void setConfigAndWaypoints(NJointCartesianWaypointControllerRuntimeConfig cfg,
+ Eigen::Matrix4fSeq wps);
+ void setConfigAndWaypoint(NJointCartesianWaypointControllerRuntimeConfig cfg,
+ Eigen::Matrix4f wp);
+
FTSensorValue getFTSensorValue();
void setCurrentFTAsOffset();
void stopMovement();
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointController.ice
index f1056467092843c4928f63ba9dcba119ac8c0ebd..f528d6e2b26011956c2e03d2eda47f956cf646b7 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointController.ice
@@ -22,10 +22,10 @@
#pragma once
-#include <ArmarXGui/interface/WidgetDescription.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
+
+#include <ArmarXGui/interface/WidgetDescription.ice>
module armarx
{
@@ -35,13 +35,15 @@ module armarx
module RobotUnitControllerNames
{
const string NJointTrajectoryController = "NJointTrajectoryController";
- const string NJointGlobalTCPController = "NJointGlobalTCPController"; /*@@@TODO: move NJointGlobalTCPController to RobotAPI */
+ const string NJointGlobalTCPController =
+ "NJointGlobalTCPController"; /*@@@TODO: move NJointGlobalTCPController to RobotAPI */
const string NJointTCPController = "NJointTCPController";
const string NJointCartesianVelocityController = "NJointCartesianVelocityController";
};
-
- class NJointControllerConfig {};
+ class NJointControllerConfig
+ {
+ };
struct NJointControllerDescription
{
@@ -52,6 +54,7 @@ module armarx
bool deletable;
bool internal;
};
+
sequence<NJointControllerDescription> NJointControllerDescriptionSeq;
struct NJointControllerStatus
@@ -62,6 +65,7 @@ module armarx
bool error = false;
long timestampUSec = 0;
};
+
sequence<NJointControllerStatus> NJointControllerStatusSeq;
struct NJointControllerDescriptionWithStatus
@@ -69,6 +73,7 @@ module armarx
NJointControllerStatus status;
NJointControllerDescription description;
};
+
sequence<NJointControllerDescriptionWithStatus> NJointControllerDescriptionWithStatusSeq;
interface NJointControllerInterface
@@ -82,7 +87,8 @@ module armarx
["cpp:const"] idempotent bool hasControllerError();
["cpp:const"] idempotent NJointControllerStatus getControllerStatus();
["cpp:const"] idempotent NJointControllerDescription getControllerDescription();
- ["cpp:const"] idempotent NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus();
+ ["cpp:const"] idempotent NJointControllerDescriptionWithStatus
+ getControllerDescriptionWithStatus();
// This method is not used by anybody and just produces cyclic dependencies
// If you were able to create/get an NJointController, you know the RobotUnit already.
//["cpp:const"] idempotent RobotUnitInterface* getRobotUnit();
@@ -92,10 +98,11 @@ module armarx
void deleteController() throws LogicError;
void deactivateAndDeleteController() throws LogicError;
- ["cpp:const"] idempotent WidgetDescription::StringWidgetDictionary getFunctionDescriptions();
- void callDescribedFunction(string fuinctionName, StringVariantBaseMap values) throws InvalidArgumentException;
+ ["cpp:const"] idempotent WidgetDescription::StringWidgetDictionary
+ getFunctionDescriptions();
+ void callDescribedFunction(string fuinctionName, StringVariantBaseMap values)
+ throws InvalidArgumentException;
};
dictionary<string, NJointControllerInterface*> StringNJointControllerPrxDictionary;
};
-
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCurrentController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCurrentController.ice
index 06e648137cca85c6cd913cc5377e247188d196a5..d6731c9e3e15119dca1191a6c49b0e9628122afd 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCurrentController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCurrentController.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -39,7 +39,6 @@ module armarx
string deviceName;
};
-
interface NJointCurrentControllerInterface extends NJointControllerInterface
{
void setControllerTarget(float current);
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
index dd31d78e4e848fc8cf9d3b3bcbe99423a2215777..4ac7375a53079e17e0573a96c925b5cc2dc9c962 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointJointSpaceDMPController.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -58,5 +58,4 @@ module armarx
void setMPWeights(DoubleSeqSeq weights);
DoubleSeqSeq getMPWeights();
};
-
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 178edf202298befc0236ca0e048cc65456601e44..deebf2e38ba0df7fcbc14b7c1816403dc24c6096 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -22,11 +22,11 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-#include <RobotAPI/interface/core/Trajectory.ice>
-
#include <ArmarXCore/interface/serialization/Eigen.ice>
+#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+
module armarx
{
module NJointTaskSpaceDMPControllerMode
@@ -63,7 +63,8 @@ module armarx
string nodeSetName = "";
string tcpName = "";
string frameName = "";
- NJointTaskSpaceDMPControllerMode::CartesianSelection mode = NJointTaskSpaceDMPControllerMode::eAll;
+ NJointTaskSpaceDMPControllerMode::CartesianSelection mode =
+ NJointTaskSpaceDMPControllerMode::eAll;
double maxLinearVel;
double maxAngularVel;
@@ -79,7 +80,6 @@ module armarx
float vel_filter;
};
-
interface NJointTaskSpaceDMPControllerInterface extends NJointControllerInterface
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
@@ -98,7 +98,8 @@ module armarx
void pauseDMP();
void resumeDMP();
- void setControllerTarget(float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
+ void setControllerTarget(float avoidJointLimitsKp,
+ NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
void setTorqueKp(StringFloatDictionary torqueKp);
void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
string getDMPAsString();
@@ -111,7 +112,6 @@ module armarx
void setLinearVelocityKp(float kp);
void setAngularVelocityKd(float kd);
void setAngularVelocityKp(float kp);
-
};
class NJointCCDMPControllerConfig extends NJointControllerConfig
@@ -144,8 +144,8 @@ module armarx
// velocity controller configuration
string nodeSetName = "";
string tcpName = "";
- NJointTaskSpaceDMPControllerMode::CartesianSelection mode = NJointTaskSpaceDMPControllerMode::eAll;
-
+ NJointTaskSpaceDMPControllerMode::CartesianSelection mode =
+ NJointTaskSpaceDMPControllerMode::eAll;
};
interface NJointCCDMPControllerInterface extends NJointControllerInterface
@@ -158,7 +158,8 @@ module armarx
void setViaPoints(int dmpId, double canVal, Ice::DoubleSeq point);
void setGoals(int dmpId, Ice::DoubleSeq goals);
- void setControllerTarget(float avoidJointLimitsKp, NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
+ void setControllerTarget(float avoidJointLimitsKp,
+ NJointTaskSpaceDMPControllerMode::CartesianSelection mode);
void setTorqueKp(StringFloatDictionary torqueKp);
void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
};
@@ -186,8 +187,8 @@ module armarx
Ice::FloatSeq leftDesiredJointValues;
Ice::FloatSeq rightDesiredJointValues;
-// float KoriFollower = 1;
-// float KposFollower = 1;
+ // float KoriFollower = 1;
+ // float KposFollower = 1;
double maxLinearVel;
double maxAngularVel;
@@ -209,15 +210,13 @@ module armarx
float startReduceTorque;
-// Ice::FloatSeq Kpf;
-// Ice::FloatSeq Kif;
-// Ice::FloatSeq DesiredForce;
-
-// float BoxWidth;
-
-// float FilterTimeConstant;
+ // Ice::FloatSeq Kpf;
+ // Ice::FloatSeq Kif;
+ // Ice::FloatSeq DesiredForce;
+ // float BoxWidth;
+ // float FilterTimeConstant;
};
interface NJointBimanualCCDMPControllerInterface extends NJointControllerInterface
@@ -233,10 +232,8 @@ module armarx
double getVirtualTime();
string getLeaderName();
-
};
-
class NJointBimanualCCDMPVelocityControllerConfig extends NJointControllerConfig
{
@@ -289,8 +286,6 @@ module armarx
string getLeaderName();
};
-
-
class NJointTaskSpaceImpedanceDMPControllerConfig extends NJointControllerConfig
{
@@ -424,7 +419,6 @@ module armarx
void setDefaultJointValues(Ice::FloatSeq desiredJointVals);
};
-
class NJointPeriodicTSDMPControllerConfig extends NJointControllerConfig
{
@@ -462,7 +456,6 @@ module armarx
float minimumReactForce = 0;
};
-
interface NJointPeriodicTSDMPControllerInterface extends NJointControllerInterface
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
@@ -477,7 +470,6 @@ module armarx
double getCanVal();
};
-
class NJointPeriodicTSDMPCompliantControllerConfig extends NJointControllerConfig
{
@@ -539,10 +531,8 @@ module armarx
Ice::DoubleSeq kmani;
Ice::DoubleSeq positionManipulability;
Ice::DoubleSeq maniWeight;
-
};
-
interface NJointPeriodicTSDMPCompliantControllerInterface extends NJointControllerInterface
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
@@ -621,7 +611,6 @@ module armarx
float frictionCone;
};
-
interface NJointAdaptiveWipingControllerInterface extends NJointControllerInterface
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
@@ -739,8 +728,8 @@ module armarx
bool useDMPInGlobalFrame;
};
-
- interface NJointAnomalyDetectionAdaptiveWipingControllerInterface extends NJointControllerInterface
+ interface NJointAnomalyDetectionAdaptiveWipingControllerInterface extends
+ NJointControllerInterface
{
void learnDMPFromFiles(Ice::StringSeq trajfiles);
void learnDMPFromTrajectory(TrajectoryBase trajectory);
@@ -764,4 +753,3 @@ module armarx
void resumeDMP();
};
};
-
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice
index 388ed7b14a1daa034b9144c158185fa9772e7b90..dde6c7cf89c4535f53e31a876af06df73865d629 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice
@@ -22,8 +22,8 @@
#pragma once
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
{
@@ -41,7 +41,6 @@ module armarx
bool isPreview = false;
};
-
interface NJointTrajectoryControllerInterface extends NJointControllerInterface
{
void setTrajectory(TrajectoryBase traj);
diff --git a/source/RobotAPI/interface/units/RobotUnit/NjointZeroTorqueController.ice b/source/RobotAPI/interface/units/RobotUnit/NjointZeroTorqueController.ice
index ed1c98692abf97bfc03e73ac9e75fc02639b4c8a..a10efe6d4386c69b984c10add58b43e890e1e55a 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NjointZeroTorqueController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NjointZeroTorqueController.ice
@@ -38,7 +38,6 @@ module armarx
float maxTorque = 10.0f;
};
-
interface NJointZeroTorqueControllerInterface extends NJointControllerInterface
{
void setControllerTarget(Ice::FloatSeq targetTorques);
diff --git a/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice b/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
index 74314446ca7f274d37148c49a54b59e0bbd715b2..854b7e38051a846343ed00a25a15198da2cc8de6 100644
--- a/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.ice
@@ -23,6 +23,7 @@
#pragma once
#include <ArmarXCore/interface/serialization/Eigen.ice>
+
#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
module armarx
@@ -45,6 +46,7 @@ module armarx
float torqueLimit;
};
+
struct NJointTaskSpaceImpedanceControlRuntimeConfig
{
Eigen::Vector3f Kpos;
@@ -65,10 +67,10 @@ module armarx
void setOrientation(Eigen::Quaternionf orientation);
void setPositionOrientation(Eigen::Vector3f target, Eigen::Quaternionf orientation);
void setPose(Eigen::Matrix4f mat);
-
+
void setImpedanceParameters(string paraName, Ice::FloatSeq vals);
- void setNullspaceConfig(Eigen::VectorXf desiredJointPositions, Eigen::VectorXf Knull, Eigen::VectorXf Dnull);
+ void setNullspaceConfig(
+ Eigen::VectorXf desiredJointPositions, Eigen::VectorXf Knull, Eigen::VectorXf Dnull);
void setConfig(NJointTaskSpaceImpedanceControlRuntimeConfig cfg);
};
-
};
diff --git a/source/RobotAPI/interface/units/TCPControlUnit.ice b/source/RobotAPI/interface/units/TCPControlUnit.ice
index 36d0bdf75b3ddde0c8b5a7535a785ed31810aca2..ea90ea29961bd9144b2b6d5c88410fb69a0ab114 100644
--- a/source/RobotAPI/interface/units/TCPControlUnit.ice
+++ b/source/RobotAPI/interface/units/TCPControlUnit.ice
@@ -24,23 +24,24 @@
#pragma once
-#include <RobotAPI/interface/units/UnitInterface.ice>
-#include <RobotAPI/interface/core/RobotState.ice>
-#include <RobotAPI/interface/core/FramedPoseBase.ice>
-
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+#include <RobotAPI/interface/core/RobotState.ice>
+#include <RobotAPI/interface/units/UnitInterface.ice>
module armarx
{
- /**
+ /**
* Implements an interface to an TCPControlUnit. The TCPControlUnit uses differential IK in order to move the TCP a target pose.
**/
- interface TCPControlUnitInterface extends armarx::SensorActorUnitInterface, armarx::KinematicUnitListener
+ interface TCPControlUnitInterface extends armarx::SensorActorUnitInterface,
+ armarx::KinematicUnitListener
{
- /**
+ /**
* setCycleTime allows to set the cycle time with which the TCPControlUnit solves the IK and moves the TCP within a periodic task.
* @param milliseconds Cycle time in milliseconds.
**/
@@ -52,7 +53,10 @@ module armarx
* @param translationVelocity Translational velocity in task space in x, y, and z.
* @param orientationVelocityRPY Orientational velocity in task space in roll, pitch, yaw.
**/
- void setTCPVelocity(string robotNodeSetName, string tcpNodeName, FramedDirectionBase translationVelocity, FramedDirectionBase orientationVelocityRPY);
+ void setTCPVelocity(string robotNodeSetName,
+ string tcpNodeName,
+ FramedDirectionBase translationVelocity,
+ FramedDirectionBase orientationVelocityRPY);
/**
* @brief returns if the TCPControlUnit is requested.
@@ -61,7 +65,7 @@ module armarx
bool isRequested();
};
- /**
+ /**
* [FramedPoseBaseMap] defines a data container which contains nodes, identified by name, and corresponding FramedPoses.
**/
dictionary<string, FramedPoseBase> FramedPoseBaseMap;
@@ -70,7 +74,7 @@ module armarx
**/
interface TCPControlUnitListener
{
- /**
+ /**
* reportTCPPose reports TCPs and corresponding current FramedPoses.
* @param tcpPoses Map of TCP node names and corresponding current FramedPoses.
**/
@@ -80,15 +84,13 @@ module armarx
* @param tcpTranslationVelocities Map of TCP node names and corresponding current translational velocities.
* @param tcpOrienationVelocities Map of TCP node names and corresponding current orientational velocities.
**/
- void reportTCPVelocities(FramedDirectionMap tcpTranslationVelocities, FramedDirectionMap tcpOrienationVelocities);
-
+ void reportTCPVelocities(FramedDirectionMap tcpTranslationVelocities,
+ FramedDirectionMap tcpOrienationVelocities);
};
- /**
+ /**
* Implements an interface to an TCPControlUnitObserver.
**/
- interface TCPControlUnitObserverInterface extends ObserverInterface, TCPControlUnitListener
- {
-
- };
+ interface TCPControlUnitObserverInterface extends ObserverInterface, TCPControlUnitListener{
+ };
};
diff --git a/source/RobotAPI/interface/units/TCPMoverUnitInterface.ice b/source/RobotAPI/interface/units/TCPMoverUnitInterface.ice
index 71f7f1777f011efdcd05e3e5c9c67814c50437e4..b77bf56455a17984a571575acefa322d4dd2825c 100644
--- a/source/RobotAPI/interface/units/TCPMoverUnitInterface.ice
+++ b/source/RobotAPI/interface/units/TCPMoverUnitInterface.ice
@@ -24,13 +24,14 @@
#pragma once
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
-
#include <ArmarXCore/interface/core/UserException.ice>
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+
module armarx
{
- enum HandSelection{
+ enum HandSelection
+ {
eNone,
eLeftHand,
eRightHand
@@ -38,7 +39,7 @@ module armarx
interface TCPMoverUnitInterface
{
-// void moveTCPRelative(bool leftHand, float x, float y, float z, float speedFactor);
+ // void moveTCPRelative(bool leftHand, float x, float y, float z, float speedFactor);
/**
* Resets the joint angles of the arm to the home position.
* NO MOVEMENT! Just sets the angles, so not suitable for a real robot.
@@ -64,7 +65,8 @@ module armarx
* @param z The velocity on the z-axis.
* @param speedFactor A factor by which the x,y,z values should be multiplied.
*/
- void setCartesianTCPVelocity(HandSelection selectedHand, float x, float y, float z, float speedFactor);
+ void setCartesianTCPVelocity(
+ HandSelection selectedHand, float x, float y, float z, float speedFactor);
//! angles in radiant
/**
@@ -109,4 +111,3 @@ module armarx
void stopPlatform();
};
};
-
diff --git a/source/RobotAPI/interface/units/UnitInterface.ice b/source/RobotAPI/interface/units/UnitInterface.ice
index cb3e572e4b4755b69451ea64a78cb711689ee6e1..82d104ce25220127bedd135730c06518550c4cfe 100644
--- a/source/RobotAPI/interface/units/UnitInterface.ice
+++ b/source/RobotAPI/interface/units/UnitInterface.ice
@@ -26,20 +26,17 @@
module armarx
{
- /**
+ /**
* @throws ResourceUnavailableException Raised if the unit resource is not available.
**/
- exception ResourceUnavailableException extends UserException
- {
- };
+ exception ResourceUnavailableException extends UserException{};
- /**
+ /**
* @throws ResourceNotOwnedException Raised if the unit resource is not owned.
**/
- exception ResourceNotOwnedException extends UserException
- {
- };
- /**
+ exception ResourceNotOwnedException extends UserException{};
+
+ /**
* [UnitExecutionState] defines whether a Unit is constructed, initialized, started or stopped.
**/
enum UnitExecutionState
@@ -50,13 +47,13 @@ module armarx
eUnitStarted,
eUnitStopped
};
-
- /**
+
+ /**
* Implements an interface to a UnitExecutionManagement.
**/
interface UnitExecutionManagementInterface
{
- /**
+ /**
* init is called to initialize the unit before starting it. Virtual method which has to implemented by components inheriting from this interface.
**/
void init();
@@ -68,7 +65,7 @@ module armarx
* stop is called to stopthe unit. Virtual method which has to implemented by components inheriting from this interface.
**/
void stop();
-
+
/**
* getExecutionState returns the execution state of the unit. Virtual method which has to implemented by components inheriting from this interface.
* @return UnitExecutionState
@@ -76,13 +73,13 @@ module armarx
**/
UnitExecutionState getExecutionState();
};
-
- /**
+
+ /**
* Implements an interface to a UnitResourceManagement.
**/
interface UnitResourceManagementInterface
{
- /**
+ /**
* request is called to grant another component exclusive access to this unit.
**/
void request() throws ResourceUnavailableException;
@@ -91,11 +88,9 @@ module armarx
**/
void release() throws ResourceNotOwnedException;
};
- /**
+ /**
* Implements an interface to a SensorActorUnit. The SensorActorUnit is the the base component for subsequent actuation and sensor components.
**/
- interface SensorActorUnitInterface extends UnitExecutionManagementInterface, UnitResourceManagementInterface
- {
- };
+ interface SensorActorUnitInterface extends UnitExecutionManagementInterface,
+ UnitResourceManagementInterface{};
};
-
diff --git a/source/RobotAPI/interface/units/WeissHapticUnit.ice b/source/RobotAPI/interface/units/WeissHapticUnit.ice
index 413bc81299648cf573c955f195c29b6b8d0ebe32..1688367baff27474809b19b3e6083c6bc5106e1e 100644
--- a/source/RobotAPI/interface/units/WeissHapticUnit.ice
+++ b/source/RobotAPI/interface/units/WeissHapticUnit.ice
@@ -28,12 +28,12 @@
module armarx
{
- /**
+ /**
* Implements an interface to a HapticUnit for the tactile sensors from Weiss Robotics.
**/
interface WeissHapticUnitInterface extends armarx::HapticUnitInterface
{
- /**
+ /**
* setDeviceTag allows to rename device.
* @param deviceName Original name of tactile sensor device.
* @param tag New name of tactile sensor device.
@@ -48,6 +48,4 @@ module armarx
**/
void stopLogging();
};
-
};
-
diff --git a/source/RobotAPI/interface/visualization/ArViz.ice b/source/RobotAPI/interface/visualization/ArViz.ice
index 8cf53a50f7ca47c82d61e38d4ae649c61d1b55bf..7e98ca8b362cca3357fdb0306a7c180021292dcd 100644
--- a/source/RobotAPI/interface/visualization/ArViz.ice
+++ b/source/RobotAPI/interface/visualization/ArViz.ice
@@ -4,62 +4,61 @@
module armarx
{
-module viz
-{
+ module viz
+ {
-struct Header
-{
- string frame_id;
- int stamp = 0;
-};
+ struct Header
+ {
+ string frame_id;
+ int stamp = 0;
+ };
-struct Vec3
-{
- float x = 0.0f;
- float y = 0.0f;
- float z = 0.0f;
-};
+ struct Vec3
+ {
+ float x = 0.0f;
+ float y = 0.0f;
+ float z = 0.0f;
+ };
-struct Orientation
-{
- float qw = 1.0f;
- float qx = 0.0f;
- float qy = 0.0f;
- float qz = 0.0f;
-};
+ struct Orientation
+ {
+ float qw = 1.0f;
+ float qx = 0.0f;
+ float qy = 0.0f;
+ float qz = 0.0f;
+ };
-struct Color
-{
- float a = 1.0f;
- float r = 0.0f;
- float g = 0.0f;
- float b = 0.0f;
-};
+ struct Color
+ {
+ float a = 1.0f;
+ float r = 0.0f;
+ float g = 0.0f;
+ float b = 0.0f;
+ };
-sequence<Vec3> Vec3List;
-sequence<Color> ColorList;
+ sequence<Vec3> Vec3List;
+ sequence<Color> ColorList;
-struct Marker
-{
- Header header;
- string ns;
- int id = 0;
- int type = 0;
- Vec3 position;
- Orientation orientation;
- Vec3 scale;
- Color color;
- Vec3List points;
- ColorList colors;
- float lifetime_in_seconds = 0.0f;
-};
-
-sequence<Marker> MarkerList;
+ struct Marker
+ {
+ Header header;
+ string ns;
+ int id = 0;
+ int type = 0;
+ Vec3 position;
+ Orientation orientation;
+ Vec3 scale;
+ Color color;
+ Vec3List points;
+ ColorList colors;
+ float lifetime_in_seconds = 0.0f;
+ };
-interface Topic
-{
- void draw(MarkerList markers);
-};
+ sequence<Marker> MarkerList;
-}
+ interface Topic
+ {
+ void draw(MarkerList markers);
+ };
+ }
}
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index d07c6eb97eeb1dd8624823d36cea7065451a868f..9e9ed0a5e964a76cb0259464b4c9d660de8f4112 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -24,8 +24,9 @@
#pragma once
-#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
+
#include <RobotAPI/interface/core/PoseBase.ice>
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
@@ -38,21 +39,21 @@ module armarx
*/
struct DrawColor
{
- float r;
- float g;
- float b;
- float a;
+ float r;
+ float g;
+ float b;
+ float a;
};
+
sequence<DrawColor> DrawColorSequence;
struct DrawColor24Bit
{
- byte r;
- byte g;
- byte b;
+ byte r;
+ byte g;
+ byte b;
};
-
struct HsvColor
{
//! 0-360
@@ -63,7 +64,6 @@ module armarx
float v;
};
-
/*!
* \brief Contains information about a layer. (name, visibility and number of elements)
*/
@@ -85,6 +85,7 @@ module armarx
float y;
float z;
};
+
sequence<DebugDrawerVertex> DebugDrawerVertexSequence;
/*!
@@ -96,6 +97,7 @@ module armarx
DebugDrawerVertex vertex2;
DebugDrawerVertex vertex3;
};
+
sequence<DebugDrawerSimpleFace> DebugDrawerSimpleFaceSequence;
struct DebugDrawerSimpleTriMesh
@@ -113,7 +115,7 @@ module armarx
int normalID;
int colorID;
};
-
+
struct DebugDrawerNormal
{
float x;
@@ -131,6 +133,7 @@ module armarx
DebugDrawerVertexID vertex3;
DebugDrawerNormal normal;
};
+
sequence<DebugDrawerFace> DebugDrawerFaceSequence;
struct DebugDrawerTriMesh
@@ -140,7 +143,6 @@ module armarx
DrawColorSequence colors;
};
-
const int three = 3;
const float zero = 0;
@@ -150,6 +152,7 @@ module armarx
float y;
float z;
};
+
sequence<DebugDrawerPointCloudElement> DebugDrawerPointCloudElementList;
struct DebugDrawerPointCloud
@@ -165,6 +168,7 @@ module armarx
float z;
DrawColor color;
};
+
sequence<DebugDrawerColoredPointCloudElement> DebugDrawerColoredPointCloudElementList;
struct DebugDrawerColoredPointCloud
@@ -180,6 +184,7 @@ module armarx
float z;
DrawColor24Bit color;
};
+
sequence<DebugDrawer24BitColoredPointCloudElement> DebugDrawer24BitColoredPointCloudElementList;
struct DebugDrawer24BitColoredPointCloud
@@ -204,9 +209,13 @@ module armarx
bool useHeatMap = false;
};
- sequence< Vector3Base > PolygonPointList;
+ sequence<Vector3Base> PolygonPointList;
- enum DrawStyle { FullModel, CollisionModel };
+ enum DrawStyle
+ {
+ FullModel,
+ CollisionModel
+ };
struct DebugDrawerSelectionElement
{
@@ -234,19 +243,64 @@ module armarx
*/
void setPoseVisu(string layerName, string poseName, PoseBase globalPose);
void setScaledPoseVisu(string layerName, string poseName, PoseBase globalPose, float scale);
- void setLineVisu(string layerName, string lineName, Vector3Base globalPosition1, Vector3Base globalPosition2, float lineWidth, DrawColor color);
+ void setLineVisu(string layerName,
+ string lineName,
+ Vector3Base globalPosition1,
+ Vector3Base globalPosition2,
+ float lineWidth,
+ DrawColor color);
void setLineSetVisu(string layerName, string lineSetName, DebugDrawerLineSet lineSet);
- void setBoxVisu(string layerName, string boxName, PoseBase globalPose, Vector3Base dimensions, DrawColor color);
- void setTextVisu(string layerName, string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
- void setSphereVisu(string layerName, string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
- void setPointCloudVisu(string layerName, string pointCloudName, DebugDrawerPointCloud pointCloud);
- void setColoredPointCloudVisu(string layerName, string pointCloudName, DebugDrawerColoredPointCloud pointCloud);
- void set24BitColoredPointCloudVisu(string layerName, string pointCloudName, DebugDrawer24BitColoredPointCloud pointCloud);
- void setPolygonVisu(string layerName, string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
+ void setBoxVisu(string layerName,
+ string boxName,
+ PoseBase globalPose,
+ Vector3Base dimensions,
+ DrawColor color);
+ void setTextVisu(string layerName,
+ string textName,
+ string text,
+ Vector3Base globalPosition,
+ DrawColor color,
+ int size);
+ void setSphereVisu(string layerName,
+ string sphereName,
+ Vector3Base globalPosition,
+ DrawColor color,
+ float radius);
+ void setPointCloudVisu(
+ string layerName, string pointCloudName, DebugDrawerPointCloud pointCloud);
+ void setColoredPointCloudVisu(
+ string layerName, string pointCloudName, DebugDrawerColoredPointCloud pointCloud);
+ void set24BitColoredPointCloudVisu(
+ string layerName, string pointCloudName, DebugDrawer24BitColoredPointCloud pointCloud);
+ void setPolygonVisu(string layerName,
+ string polygonName,
+ PolygonPointList polygonPoints,
+ DrawColor colorInner,
+ DrawColor colorBorder,
+ float lineWidth);
void setTriMeshVisu(string layerName, string triMeshName, DebugDrawerTriMesh triMesh);
- void setArrowVisu(string layerName, string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
- void setCylinderVisu(string layerName, string cylinderName, Vector3Base globalPosition, Vector3Base direction, float length, float radius, DrawColor color);
- void setCircleArrowVisu(string layerName, string circleName, Vector3Base globalPosition, Vector3Base directionVec, float radius, float circleCompletion, float width, DrawColor color);
+ void setArrowVisu(string layerName,
+ string arrowName,
+ Vector3Base position,
+ Vector3Base direction,
+ DrawColor color,
+ float length,
+ float width);
+ void setCylinderVisu(string layerName,
+ string cylinderName,
+ Vector3Base globalPosition,
+ Vector3Base direction,
+ float length,
+ float radius,
+ DrawColor color);
+ void setCircleArrowVisu(string layerName,
+ string circleName,
+ Vector3Base globalPosition,
+ Vector3Base directionVec,
+ float radius,
+ float circleCompletion,
+ float width,
+ DrawColor color);
/*!
@@ -258,7 +312,11 @@ module armarx
* \param armarxProject Additional armarx project that should be used to search the robot. Must be locally available and accessible through the armarx cmake search procedure.
*/
- void setRobotVisu(string layerName, string robotName, string robotFile, string armarxProject, DrawStyle drawStyleType);
+ void setRobotVisu(string layerName,
+ string robotName,
+ string robotFile,
+ string armarxProject,
+ DrawStyle drawStyleType);
void updateRobotPose(string layerName, string robotName, PoseBase globalPose);
void updateRobotConfig(string layerName, string robotName, NameValueMap configuration);
/*!
@@ -268,7 +326,8 @@ module armarx
* \param c The draw color, if all is set to 0, the colorization is disabled (i.e. the original vizualization shows up)
*/
void updateRobotColor(string layerName, string robotName, DrawColor c);
- void updateRobotNodeColor(string layerName, string robotName, string robotNodeName, DrawColor c);
+ void updateRobotNodeColor(
+ string layerName, string robotName, string robotNodeName, DrawColor c);
void removeRobotVisu(string layerName, string robotName);
/*!
@@ -278,17 +337,39 @@ module armarx
*/
void setPoseDebugLayerVisu(string poseName, PoseBase globalPose);
void setScaledPoseDebugLayerVisu(string poseName, PoseBase globalPose, float scale);
- void setLineDebugLayerVisu(string lineName, Vector3Base globalPosition1, Vector3Base globalPosition2, float lineWidth, DrawColor color);
+ void setLineDebugLayerVisu(string lineName,
+ Vector3Base globalPosition1,
+ Vector3Base globalPosition2,
+ float lineWidth,
+ DrawColor color);
void setLineSetDebugLayerVisu(string lineSetName, DebugDrawerLineSet lineSet);
- void setBoxDebugLayerVisu(string boxName, PoseBase globalPose, Vector3Base dimensions, DrawColor color);
- void setTextDebugLayerVisu(string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
- void setSphereDebugLayerVisu(string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
+ void setBoxDebugLayerVisu(
+ string boxName, PoseBase globalPose, Vector3Base dimensions, DrawColor color);
+ void setTextDebugLayerVisu(
+ string textName, string text, Vector3Base globalPosition, DrawColor color, int size);
+ void setSphereDebugLayerVisu(
+ string sphereName, Vector3Base globalPosition, DrawColor color, float radius);
void setPointCloudDebugLayerVisu(string pointCloudName, DebugDrawerPointCloud pointCloud);
- void set24BitColoredPointCloudDebugLayerVisu(string pointCloudName, DebugDrawer24BitColoredPointCloud pointCloud);
- void setPolygonDebugLayerVisu(string polygonName, PolygonPointList polygonPoints, DrawColor colorInner, DrawColor colorBorder, float lineWidth);
+ void set24BitColoredPointCloudDebugLayerVisu(string pointCloudName,
+ DebugDrawer24BitColoredPointCloud pointCloud);
+ void setPolygonDebugLayerVisu(string polygonName,
+ PolygonPointList polygonPoints,
+ DrawColor colorInner,
+ DrawColor colorBorder,
+ float lineWidth);
void setTriMeshDebugLayerVisu(string triMeshName, DebugDrawerTriMesh triMesh);
- void setArrowDebugLayerVisu(string arrowName, Vector3Base position, Vector3Base direction, DrawColor color, float length, float width);
- void setCylinderDebugLayerVisu(string cylinderName, Vector3Base globalPosition, Vector3Base direction, float length, float radius, DrawColor color);
+ void setArrowDebugLayerVisu(string arrowName,
+ Vector3Base position,
+ Vector3Base direction,
+ DrawColor color,
+ float length,
+ float width);
+ void setCylinderDebugLayerVisu(string cylinderName,
+ Vector3Base globalPosition,
+ Vector3Base direction,
+ float length,
+ float radius,
+ DrawColor color);
/*!
* \brief Draws a circle into the debug drawer.
* \param circleName
@@ -300,7 +381,13 @@ module armarx
* \param color
* \param circleDrawingDirection in which direction the circle is started to draw. Has only effect if the circle is not complete (circleCompletion parameter).
*/
- void setCircleDebugLayerVisu(string circleName, Vector3Base globalPosition, Vector3Base directionVec, float radius, float circleCompletion, float width, DrawColor color);
+ void setCircleDebugLayerVisu(string circleName,
+ Vector3Base globalPosition,
+ Vector3Base directionVec,
+ float radius,
+ float circleCompletion,
+ float width,
+ DrawColor color);
/*!
* \brief Remove visualization of coordinate system.
@@ -438,8 +525,5 @@ module armarx
void reportSelectionChanged(DebugDrawerSelectionList selectedObjects);
};
- interface DebugDrawerInterfaceAndListener extends DebugDrawerInterface, DebugDrawerListener
- {
- };
+ interface DebugDrawerInterfaceAndListener extends DebugDrawerInterface, DebugDrawerListener{};
};
-
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerInterfaceStdOverloads.h b/source/RobotAPI/interface/visualization/DebugDrawerInterfaceStdOverloads.h
index 955a492a53d29ad71671d886b051a02cb452c87c..4dd2bbc97ea89c6a65410a371ac48f00c21b1872 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterfaceStdOverloads.h
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterfaceStdOverloads.h
@@ -24,32 +24,38 @@
#pragma once
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <cmath>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+
namespace std
{
- inline bool isfinite(const armarx::DrawColor& c)
+ inline bool
+ isfinite(const armarx::DrawColor& c)
{
return isfinite(c.r) && isfinite(c.g) && isfinite(c.b) && isfinite(c.a);
}
- inline bool isfinite(const armarx::DebugDrawerPointCloudElement& e)
+ inline bool
+ isfinite(const armarx::DebugDrawerPointCloudElement& e)
{
return isfinite(e.x) && isfinite(e.y) && isfinite(e.z);
}
-}
+} // namespace std
+
namespace armarx
{
- inline std::ostream& operator<<(std::ostream& o, const DrawColor& c)
+ inline std::ostream&
+ operator<<(std::ostream& o, const DrawColor& c)
{
o << "(r= " << c.r << ", g= " << c.g << ", b= " << c.b << ", a= " << c.a << ")";
return o;
}
- inline std::ostream& operator<<(std::ostream& o, const DebugDrawerPointCloudElement& e)
+
+ inline std::ostream&
+ operator<<(std::ostream& o, const DebugDrawerPointCloudElement& e)
{
o << "(x = " << e.x << ", y = " << e.y << ", z = " << e.z << ")";
return o;
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/interface/visualization/DebugDrawerToArViz.ice b/source/RobotAPI/interface/visualization/DebugDrawerToArViz.ice
index 79f15cfd506da5ea1dc05222ff54c00290514745..2213bdd649a9c63965c600ea901441eb0195922e 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerToArViz.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerToArViz.ice
@@ -31,9 +31,5 @@
module armarx
{
- interface DebugDrawerToArvizInterface extends DebugDrawerInterface, BlackWhitelistTopic
- {
- };
-
+ interface DebugDrawerToArvizInterface extends DebugDrawerInterface, BlackWhitelistTopic{};
};
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index f2c6108a9ace4740e0add45b27ed7aa827347d41..02efa9fc85f3a7ae2fe03081b5eb2070c9957254 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -1,38 +1,39 @@
-#include <VirtualRobot/XML/ObjectIO.h>
+#include "ObjectFinder.h"
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/filesystem/list_directory.h>
+#include <VirtualRobot/XML/ObjectIO.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include "ObjectFinder.h"
-
-
namespace armarx
{
namespace fs = std::filesystem;
-
- ObjectFinder::ObjectFinder(const std::string& objectsPackageName, const ObjectFinder::path& relObjectsDir) :
+ ObjectFinder::ObjectFinder(const std::string& objectsPackageName,
+ const ObjectFinder::path& relObjectsDir) :
packageName(objectsPackageName), relObjectsDir(relObjectsDir)
{
Logging::setTag("ObjectFinder");
}
- void ObjectFinder::setPath(const std::string& path)
+ void
+ ObjectFinder::setPath(const std::string& path)
{
packageName = path;
absPackageDataDir.clear();
}
- std::string ObjectFinder::getPackageName() const
+ std::string
+ ObjectFinder::getPackageName() const
{
return packageName;
}
- void ObjectFinder::init() const
+ void
+ ObjectFinder::init() const
{
if (absPackageDataDir.empty())
{
@@ -48,19 +49,19 @@ namespace armarx
ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
// make sure this data path is available => e.g. for findArticulatedObjects
- armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {absPackageDataDir});
+ armarx::ArmarXDataPath::addDataPaths(std::vector<std::string>{absPackageDataDir});
}
}
}
-
- bool ObjectFinder::isDatasetDirValid(const path& path) const
+ bool
+ ObjectFinder::isDatasetDirValid(const path& path) const
{
return std::filesystem::is_directory(path);
}
-
- std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& dataset, const std::string& name) const
+ std::optional<ObjectInfo>
+ ObjectFinder::findObject(const std::string& dataset, const std::string& name) const
{
init();
if (!_ready())
@@ -93,22 +94,26 @@ namespace armarx
return std::nullopt;
}
- std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& nameOrID) const
+ std::optional<ObjectInfo>
+ ObjectFinder::findObject(const std::string& nameOrID) const
{
return findObject(ObjectID(nameOrID));
}
- std::optional<ObjectInfo> ObjectFinder::findObject(const ObjectID& id) const
+ std::optional<ObjectInfo>
+ ObjectFinder::findObject(const ObjectID& id) const
{
return findObject(id.dataset(), id.className());
}
- std::optional<ObjectInfo> ObjectFinder::findObject(const objpose::ObjectPose& obj) const
+ std::optional<ObjectInfo>
+ ObjectFinder::findObject(const objpose::ObjectPose& obj) const
{
return findObject(obj.objectID);
}
- std::vector<std::string> ObjectFinder::getDatasets() const
+ std::vector<std::string>
+ ObjectFinder::getDatasets() const
{
// init(); // Done by called methods.
std::vector<std::string> datasets;
@@ -119,7 +124,8 @@ namespace armarx
return datasets;
}
- std::vector<ObjectFinder::path> ObjectFinder::getDatasetDirectories() const
+ std::vector<ObjectFinder::path>
+ ObjectFinder::getDatasetDirectories() const
{
init();
if (!_ready())
@@ -139,7 +145,8 @@ namespace armarx
return datasetDirs;
}
- std::vector<ObjectInfo> ObjectFinder::findAllObjects(bool checkPaths) const
+ std::vector<ObjectInfo>
+ ObjectFinder::findAllObjects(bool checkPaths) const
{
init();
if (!_ready())
@@ -162,7 +169,8 @@ namespace armarx
return objects;
}
- std::vector<armem::articulated_object::ArticulatedObjectDescription> ObjectFinder::findAllArticulatedObjects(bool checkPaths) const
+ std::vector<armem::articulated_object::ArticulatedObjectDescription>
+ ObjectFinder::findAllArticulatedObjects(bool checkPaths) const
{
init();
if (!_ready())
@@ -185,7 +193,7 @@ namespace armarx
}
std::map<std::string, std::vector<ObjectInfo>>
- ObjectFinder::findAllObjectsByDataset(bool checkPaths) const
+ ObjectFinder::findAllObjectsByDataset(bool checkPaths) const
{
// init(); // Done by called methods.
std::map<std::string, std::vector<ObjectInfo>> objects;
@@ -196,7 +204,8 @@ namespace armarx
return objects;
}
- std::vector<ObjectInfo> ObjectFinder::findAllObjectsOfDataset(const std::string& dataset, bool checkPaths) const
+ std::vector<ObjectInfo>
+ ObjectFinder::findAllObjectsOfDataset(const std::string& dataset, bool checkPaths) const
{
init();
if (!_ready())
@@ -217,9 +226,10 @@ namespace armarx
{
if (fs::is_directory(datasetDir / dir))
{
- ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
+ ObjectInfo object(
+ packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
object.setLogError(logObjectDiscoveryError);
-
+
if (!checkPaths || object.checkPaths())
{
objects.push_back(object);
@@ -229,9 +239,9 @@ namespace armarx
return objects;
}
-
- std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>
- ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
+ std::unordered_map<std::string,
+ std::vector<armem::articulated_object::ArticulatedObjectDescription>>
+ ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
{
init();
if (!_ready())
@@ -241,7 +251,9 @@ namespace armarx
const bool local = true;
- std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> datasets;
+ std::unordered_map<std::string,
+ std::vector<armem::articulated_object::ArticulatedObjectDescription>>
+ datasets;
for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
if (isDatasetDirValid(_rootDirAbs() / datasetDir))
@@ -253,9 +265,9 @@ namespace armarx
return datasets;
}
-
std::vector<armem::articulated_object::ArticulatedObjectDescription>
- ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const
+ ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset,
+ bool checkPaths) const
{
init();
if (!_ready())
@@ -276,12 +288,12 @@ namespace armarx
{
if (fs::is_directory(datasetDir / dir))
{
- ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
+ ObjectInfo object(
+ packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
std::optional<PackageFileLocation> modelFile = object.getArticulatedModel();
if (modelFile.has_value())
{
- objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
- {
+ objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription{
.name = object.idStr(),
.xml = {modelFile->package, modelFile->relativePath},
.visualization = {},
@@ -310,11 +322,13 @@ namespace armarx
}
return VirtualRobot::ObjectIO::loadManipulationObject(abs);
}
+
VirtualRobot::ManipulationObjectPtr
ObjectFinder::loadManipulationObject(const objpose::ObjectPose& obj) const
{
return loadManipulationObject(findObject(obj));
}
+
VirtualRobot::ObstaclePtr
ObjectFinder::loadObstacle(const std::optional<ObjectInfo>& ts)
{
@@ -331,14 +345,15 @@ namespace armarx
}
return VirtualRobot::ObjectIO::loadObstacle(abs);
}
+
VirtualRobot::ObstaclePtr
ObjectFinder::loadObstacle(const objpose::ObjectPose& obj) const
{
return loadObstacle(findObject(obj));
}
-
- static std::vector<std::string> _loadNames(
+ static std::vector<std::string>
+ _loadNames(
const ObjectFinder& finder,
const ObjectID& objectID,
const bool includeClassName,
@@ -359,39 +374,47 @@ namespace armarx
}
return names;
}
- std::vector<std::string> ObjectFinder::loadRecognizedNames(const ObjectID& objectID, bool includeClassName) const
+
+ std::vector<std::string>
+ ObjectFinder::loadRecognizedNames(const ObjectID& objectID, bool includeClassName) const
{
- return _loadNames(*this, objectID, includeClassName, [](const ObjectInfo & info)
- {
- return info.loadRecognizedNames();
- });
+ return _loadNames(*this,
+ objectID,
+ includeClassName,
+ [](const ObjectInfo& info) { return info.loadRecognizedNames(); });
}
- std::vector<std::string> ObjectFinder::loadSpokenNames(const ObjectID& objectID, bool includeClassName) const
+
+ std::vector<std::string>
+ ObjectFinder::loadSpokenNames(const ObjectID& objectID, bool includeClassName) const
{
- return _loadNames(*this, objectID, includeClassName, [](const ObjectInfo & info)
- {
- return info.loadSpokenNames();
- });
+ return _loadNames(*this,
+ objectID,
+ includeClassName,
+ [](const ObjectInfo& info) { return info.loadSpokenNames(); });
}
-
- void ObjectFinder::setLogObjectDiscoveryError(bool logEnabled)
+
+ void
+ ObjectFinder::setLogObjectDiscoveryError(bool logEnabled)
{
logObjectDiscoveryError = logEnabled;
}
- ObjectFinder::path ObjectFinder::_rootDirAbs() const
+ ObjectFinder::path
+ ObjectFinder::_rootDirAbs() const
{
return absPackageDataDir / packageName / relObjectsDir;
}
- ObjectFinder::path ObjectFinder::_rootDirRel() const
+ ObjectFinder::path
+ ObjectFinder::_rootDirRel() const
{
return packageName;
}
- bool ObjectFinder::_ready() const
+ bool
+ ObjectFinder::_ready() const
{
return !absPackageDataDir.empty();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index 5c461033d3d75b4d097ec2763928a9e94aab6389..d268e7e76a57a71e31bee93609b067bea49d7370 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -12,7 +12,6 @@
#include "ObjectInfo.h"
#include "ObjectPose.h"
-
namespace armarx
{
/**
@@ -23,20 +22,18 @@ namespace armarx
class ObjectFinder : Logging
{
public:
-
using path = std::filesystem::path;
inline static const std::string DefaultObjectsPackageName = "PriorKnowledgeData";
inline static const std::string DefaultObjectsDirectory = "objects";
public:
-
ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName,
const path& relObjectsDir = DefaultObjectsDirectory);
- ObjectFinder(ObjectFinder&&) = default;
- ObjectFinder(const ObjectFinder&) = default;
- ObjectFinder& operator=(ObjectFinder&&) = default;
+ ObjectFinder(ObjectFinder&&) = default;
+ ObjectFinder(const ObjectFinder&) = default;
+ ObjectFinder& operator=(ObjectFinder&&) = default;
ObjectFinder& operator=(const ObjectFinder&) = default;
@@ -44,7 +41,8 @@ namespace armarx
std::string getPackageName() const;
- std::optional<ObjectInfo> findObject(const std::string& dataset, const std::string& name) const;
+ std::optional<ObjectInfo> findObject(const std::string& dataset,
+ const std::string& name) const;
std::optional<ObjectInfo> findObject(const std::string& nameOrID) const;
std::optional<ObjectInfo> findObject(const ObjectID& id) const;
std::optional<ObjectInfo> findObject(const objpose::ObjectPose& obj) const;
@@ -53,22 +51,26 @@ namespace armarx
std::vector<path> getDatasetDirectories() const;
std::vector<ObjectInfo> findAllObjects(bool checkPaths = true) const;
- std::map<std::string, std::vector<ObjectInfo>> findAllObjectsByDataset(bool checkPaths = true) const;
- std::vector<ObjectInfo> findAllObjectsOfDataset(const std::string& dataset, bool checkPaths = true) const;
-
- std::vector<armem::articulated_object::ArticulatedObjectDescription> findAllArticulatedObjects(bool checkPaths) const;
- std::vector<armem::articulated_object::ArticulatedObjectDescription> findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const;
- std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> findAllArticulatedObjectsByDataset(bool checkPaths = true) const;
-
- VirtualRobot::ManipulationObjectPtr
- static loadManipulationObject(const std::optional<ObjectInfo>& ts);
+ std::map<std::string, std::vector<ObjectInfo>>
+ findAllObjectsByDataset(bool checkPaths = true) const;
+ std::vector<ObjectInfo> findAllObjectsOfDataset(const std::string& dataset,
+ bool checkPaths = true) const;
+
+ std::vector<armem::articulated_object::ArticulatedObjectDescription>
+ findAllArticulatedObjects(bool checkPaths) const;
+ std::vector<armem::articulated_object::ArticulatedObjectDescription>
+ findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const;
+ std::unordered_map<std::string,
+ std::vector<armem::articulated_object::ArticulatedObjectDescription>>
+ findAllArticulatedObjectsByDataset(bool checkPaths = true) const;
+
+ VirtualRobot::ManipulationObjectPtr static loadManipulationObject(
+ const std::optional<ObjectInfo>& ts);
VirtualRobot::ManipulationObjectPtr
loadManipulationObject(const objpose::ObjectPose& obj) const;
- VirtualRobot::ObstaclePtr
- static loadObstacle(const std::optional<ObjectInfo>& ts);
- VirtualRobot::ObstaclePtr
- loadObstacle(const objpose::ObjectPose& obj) const;
+ VirtualRobot::ObstaclePtr static loadObstacle(const std::optional<ObjectInfo>& ts);
+ VirtualRobot::ObstaclePtr loadObstacle(const objpose::ObjectPose& obj) const;
/**
@@ -81,7 +83,8 @@ namespace armarx
* @param includeClassName If true, include the raw class name in the result.
* @see `ObjectInfo::loadRecognizedNames()`
*/
- std::vector<std::string> loadRecognizedNames(const ObjectID& objectID, bool includeClassName = false) const;
+ std::vector<std::string> loadRecognizedNames(const ObjectID& objectID,
+ bool includeClassName = false) const;
/**
* @brief Load names to use when verbalizing an object name.
*
@@ -92,13 +95,13 @@ namespace armarx
* @param includeClassName If true, include the raw class name in the result.
* @see `ObjectInfo::loadSpokenNames()`
*/
- std::vector<std::string> loadSpokenNames(const ObjectID& objectID, bool includeClassName = false) const;
+ std::vector<std::string> loadSpokenNames(const ObjectID& objectID,
+ bool includeClassName = false) const;
void setLogObjectDiscoveryError(bool logEnabled);
private:
-
void init() const;
bool isDatasetDirValid(const std::filesystem::path& path) const;
@@ -109,7 +112,6 @@ namespace armarx
private:
-
/// Name of package containing the object models (ArmarXObjects by default).
mutable std::string packageName;
@@ -125,6 +127,5 @@ namespace armarx
// see: ObjectInfo::checkPaths()
bool logObjectDiscoveryError = true;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
index 8a2856b2516784d6d5c0413a5123af9665696d22..8267b541afb95761850f150c0b015f2b85226826 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
@@ -1,8 +1,8 @@
#include "ObjectID.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx
{
@@ -10,7 +10,9 @@ namespace armarx
{
}
- ObjectID::ObjectID(const std::string& dataset, const std::string& className, const std::string& instancName) :
+ ObjectID::ObjectID(const std::string& dataset,
+ const std::string& className,
+ const std::string& instancName) :
_dataset(dataset), _className(className), _instanceName(instancName)
{
}
@@ -28,20 +30,22 @@ namespace armarx
}
}
- ObjectID ObjectID::FromString(const std::string& idString)
+ ObjectID
+ ObjectID::FromString(const std::string& idString)
{
ObjectID id;
id.setFromString(idString);
return id;
}
- void ObjectID::setFromString(const std::string& idString)
+ void
+ ObjectID::setFromString(const std::string& idString)
{
const std::vector<std::string> split = simox::alg::split(idString, "/", true);
ARMARX_CHECK(split.size() == 2 || split.size() == 3)
- << "Expected ID of format 'Dataset/ClassName' or 'Dataset/ClassName/InstanceName'"
- << ", but got: '" << idString << "' "
- << "(expected 2 or 3 '/'s, but found " << split.size() << ").";
+ << "Expected ID of format 'Dataset/ClassName' or 'Dataset/ClassName/InstanceName'"
+ << ", but got: '" << idString << "' "
+ << "(expected 2 or 3 '/'s, but found " << split.size() << ").";
_dataset = split[0];
_className = split[1];
@@ -52,7 +56,8 @@ namespace armarx
}
}
- std::string ObjectID::str() const
+ std::string
+ ObjectID::str() const
{
std::string _str = _dataset + "/" + _className;
if (!_instanceName.empty())
@@ -62,29 +67,33 @@ namespace armarx
return _str;
}
- ObjectID ObjectID::getClassID() const
+ ObjectID
+ ObjectID::getClassID() const
{
return ObjectID(_dataset, _className);
}
- bool ObjectID::equalClass(const ObjectID& rhs) const
+ bool
+ ObjectID::equalClass(const ObjectID& rhs) const
{
return _className == rhs._className && _dataset == rhs._dataset;
}
- ObjectID ObjectID::withInstanceName(const std::string& instanceName) const
+ ObjectID
+ ObjectID::withInstanceName(const std::string& instanceName) const
{
return ObjectID(_dataset, _className, instanceName);
}
- bool ObjectID::operator==(const ObjectID& rhs) const
+ bool
+ ObjectID::operator==(const ObjectID& rhs) const
{
- return _className == rhs._className
- && _dataset == rhs._dataset
- && _instanceName == rhs._instanceName;
+ return _className == rhs._className && _dataset == rhs._dataset &&
+ _instanceName == rhs._instanceName;
}
- bool ObjectID::operator<(const ObjectID& rhs) const
+ bool
+ ObjectID::operator<(const ObjectID& rhs) const
{
int c = _dataset.compare(rhs._dataset);
if (c != 0)
@@ -101,10 +110,10 @@ namespace armarx
return _instanceName < rhs._instanceName;
}
-}
-
+} // namespace armarx
-std::ostream& armarx::operator<<(std::ostream& os, const ObjectID& id)
+std::ostream&
+armarx::operator<<(std::ostream& os, const ObjectID& id)
{
return os << "'" << id.str() << "'";
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
index b152844b17f150b80c44f8ed3c849e263b2e8fd4..b9120b6cfec8a91c70be4073c47be319dbfd966e 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
@@ -2,7 +2,6 @@
#include <string>
-
namespace armarx
{
/**
@@ -11,29 +10,36 @@ namespace armarx
class ObjectID
{
public:
-
ObjectID();
- ObjectID(const std::string& dataset, const std::string& className, const std::string& instancName = "");
+ ObjectID(const std::string& dataset,
+ const std::string& className,
+ const std::string& instancName = "");
/// Construct from either a class name ("myobject") or ID ("mydataset/myobject", "mydataset/myclass/myinstance").
ObjectID(const std::string& nameOrID);
/// Construct from a string produced by `str()`, e.g. ("mydataset/myobject", "mydataset/myclass/myinstance").
static ObjectID FromString(const std::string& idString);
-
- inline std::string dataset() const
+ inline std::string
+ dataset() const
{
return _dataset;
}
- inline std::string className() const
+
+ inline std::string
+ className() const
{
return _className;
}
- inline std::string instanceName() const
+
+ inline std::string
+ instanceName() const
{
return _instanceName;
}
- inline void setInstanceName(const std::string& instanceName)
+
+ inline void
+ setInstanceName(const std::string& instanceName)
{
this->_instanceName = instanceName;
}
@@ -52,40 +58,44 @@ namespace armarx
/// Indicates whether dataset, class name and instance name are equal.
bool operator==(const ObjectID& rhs) const;
- inline bool operator!=(const ObjectID& rhs) const
+
+ inline bool
+ operator!=(const ObjectID& rhs) const
{
return !operator==(rhs);
}
- bool operator< (const ObjectID& rhs) const;
- inline bool operator> (const ObjectID& rhs) const
+
+ bool operator<(const ObjectID& rhs) const;
+
+ inline bool
+ operator>(const ObjectID& rhs) const
{
return rhs < (*this);
}
- inline bool operator<=(const ObjectID& rhs) const
+
+ inline bool
+ operator<=(const ObjectID& rhs) const
{
- return !operator> (rhs);
+ return !operator>(rhs);
}
- inline bool operator>=(const ObjectID& rhs) const
+
+ inline bool
+ operator>=(const ObjectID& rhs) const
{
- return !operator< (rhs);
+ return !operator<(rhs);
}
private:
-
/// The dataset name in ArmarXObjects, e.g. "KIT", "YCB", "SecondHands", ...
std::string _dataset;
/// The class name in ArmarXObjects, e.g. "Amicelli", "001_chips_can", ...
std::string _className;
/// An optional instance name, chosen by the user.
std::string _instanceName;
-
};
std::ostream& operator<<(std::ostream& os, const ObjectID& rhs);
-
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 4b813223c4b667cb3dd2e4ffb3e57d862d18c521..dc59da0079b9db56837840fca6fc9fa9fe18b772 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -381,7 +381,7 @@ namespace armarx
if (!fs::is_regular_file(simoxXML().absolutePath))
{
- if (false and _logError )
+ if (false and _logError)
{
ARMARX_WARNING << "Expected simox object file for object " << *this << ": "
<< simoxXML().absolutePath;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index ccbd4eb1be32116d8fc7ced05d8148f027fefccf..e4b3d80f7630c006fb3834e53177c54106652493 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -7,14 +7,14 @@
#include "ObjectID.h"
-
namespace simox
{
// #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
struct AxisAlignedBoundingBox;
// #include <SimoxUtility/shapes/OrientedBox.h>
- template<class FloatT> class OrientedBox;
-}
+ template <class FloatT>
+ class OrientedBox;
+} // namespace simox
namespace armarx
{
@@ -30,7 +30,6 @@ namespace armarx
std::filesystem::path absolutePath;
};
-
/**
* @brief Accessor for the object files.
*/
@@ -40,7 +39,6 @@ namespace armarx
using path = std::filesystem::path;
public:
-
/**
* @brief ObjectInfo
*
@@ -49,11 +47,15 @@ namespace armarx
* @param localObjectsPath The path where objects are stored in the data directory.
* @param id The object class ID (with dataset and class name).
*/
- ObjectInfo(const std::string& packageName, const path& absPackageDataDir,
- const path& relObjectsPath, const ObjectID& id);
- ObjectInfo(const std::string& packageName, const path& packageDataDir,
+ ObjectInfo(const std::string& packageName,
+ const path& absPackageDataDir,
+ const path& relObjectsPath,
+ const ObjectID& id);
+ ObjectInfo(const std::string& packageName,
+ const path& packageDataDir,
const path& relObjectsPath,
- const std::string& dataset, const std::string& className);
+ const std::string& dataset,
+ const std::string& className);
virtual ~ObjectInfo() = default;
@@ -65,8 +67,9 @@ namespace armarx
std::string dataset() const;
std::string className() const;
- [[deprecated("This function is deprecated. Use className() instead.")]]
- std::string name() const
+
+ [[deprecated("This function is deprecated. Use className() instead.")]] std::string
+ name() const
{
return className();
}
@@ -76,7 +79,9 @@ namespace armarx
std::string idStr() const;
- PackageFileLocation file(const std::string& extension, const std::string& suffix = "", bool fixDataPath = false) const;
+ PackageFileLocation file(const std::string& extension,
+ const std::string& suffix = "",
+ bool fixDataPath = false) const;
PackageFileLocation simoxXML() const;
@@ -133,16 +138,13 @@ namespace armarx
virtual bool checkPaths() const;
-
private:
-
path objectDirectory(bool fixDataPath) const;
std::optional<std::vector<std::string>> loadNames(const std::string& jsonKey) const;
private:
-
std::string _packageName;
path _absPackageDataDir;
path _relObjectsPath;
@@ -150,35 +152,44 @@ namespace armarx
ObjectID _id;
bool _logError = true;
-
};
std::ostream& operator<<(std::ostream& os, const ObjectInfo& rhs);
-
- inline bool operator==(const ObjectInfo& lhs, const ObjectInfo& rhs)
+ inline bool
+ operator==(const ObjectInfo& lhs, const ObjectInfo& rhs)
{
return lhs.id() == rhs.id();
}
- inline bool operator!=(const ObjectInfo& lhs, const ObjectInfo& rhs)
+
+ inline bool
+ operator!=(const ObjectInfo& lhs, const ObjectInfo& rhs)
{
return lhs.id() != rhs.id();
}
- inline bool operator< (const ObjectInfo& lhs, const ObjectInfo& rhs)
+
+ inline bool
+ operator<(const ObjectInfo& lhs, const ObjectInfo& rhs)
{
return lhs.id() < rhs.id();
}
- inline bool operator> (const ObjectInfo& lhs, const ObjectInfo& rhs)
+
+ inline bool
+ operator>(const ObjectInfo& lhs, const ObjectInfo& rhs)
{
return lhs.id() > rhs.id();
}
- inline bool operator<=(const ObjectInfo& lhs, const ObjectInfo& rhs)
+
+ inline bool
+ operator<=(const ObjectInfo& lhs, const ObjectInfo& rhs)
{
return lhs.id() <= rhs.id();
}
- inline bool operator>=(const ObjectInfo& lhs, const ObjectInfo& rhs)
+
+ inline bool
+ operator>=(const ObjectInfo& lhs, const ObjectInfo& rhs)
{
return lhs.id() >= rhs.id();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 4bb9a4a1d7078a761b8ac79c99464e1766d8d9fb..2c8260b4198b8da41582f15baeaf9c5980c04831 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -2,7 +2,6 @@
#include <SimoxUtility/math/pose/invert.h>
#include <SimoxUtility/math/pose/pose.h>
-
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
@@ -10,14 +9,13 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/ice_conversions.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include "ice_conversions.h"
-
namespace armarx::objpose
{
ObjectPose::ObjectPose()
@@ -29,7 +27,8 @@ namespace armarx::objpose
fromIce(ice);
}
- void ObjectPose::fromIce(const data::ObjectPose& ice)
+ void
+ ObjectPose::fromIce(const data::ObjectPose& ice)
{
armarx::fromIce(ice.objectID, objectID);
@@ -61,14 +60,16 @@ namespace armarx::objpose
objpose::fromIce(ice.localOOBB, localOOBB);
}
- data::ObjectPose ObjectPose::toIce() const
+ data::ObjectPose
+ ObjectPose::toIce() const
{
data::ObjectPose ice;
toIce(ice);
return ice;
}
- void ObjectPose::toIce(data::ObjectPose& ice) const
+ void
+ ObjectPose::toIce(data::ObjectPose& ice) const
{
armarx::toIce(ice.objectID, objectID);
@@ -100,8 +101,9 @@ namespace armarx::objpose
objpose::toIce(ice.localOOBB, localOOBB);
}
- void ObjectPose::fromProvidedPose(
- const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot)
+ void
+ ObjectPose::fromProvidedPose(const data::ProvidedObjectPose& provided,
+ VirtualRobot::RobotPtr robot)
{
armarx::fromIce(provided.objectID, objectID);
@@ -117,24 +119,29 @@ namespace armarx::objpose
if (robot)
{
- armarx::FramedPose framed(objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
+ armarx::FramedPose framed(
+ objectPoseOriginal, objectPoseOriginalFrame, robot->getName());
framed.changeFrame(robot, robot->getRootNode()->getName());
objectPoseRobot = framed.toEigen();
objectPoseRobotGaussian = std::nullopt;
objectPoseRobotGaussian = objectPoseOriginalGaussian;
- if (objectPoseOriginalGaussian.has_value() and objectPoseOriginalFrame != robot->getRootNode()->getName())
+ if (objectPoseOriginalGaussian.has_value() and
+ objectPoseOriginalFrame != robot->getRootNode()->getName())
{
- objectPoseRobotGaussian = objectPoseOriginalGaussian->getTransformed(objectPoseOriginal, objectPoseRobot);
+ objectPoseRobotGaussian =
+ objectPoseOriginalGaussian->getTransformed(objectPoseOriginal, objectPoseRobot);
}
framed.changeToGlobal(robot);
objectPoseGlobal = framed.toEigen();
objectPoseGlobalGaussian = objectPoseOriginalGaussian;
- if (objectPoseOriginalGaussian.has_value() and objectPoseOriginalFrame != armarx::GlobalFrame)
+ if (objectPoseOriginalGaussian.has_value() and
+ objectPoseOriginalFrame != armarx::GlobalFrame)
{
- objectPoseGlobalGaussian = objectPoseOriginalGaussian->getTransformed(objectPoseOriginal, objectPoseGlobal);
+ objectPoseGlobalGaussian = objectPoseOriginalGaussian->getTransformed(
+ objectPoseOriginal, objectPoseGlobal);
}
robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
@@ -174,7 +181,8 @@ namespace armarx::objpose
}
// ToDo: We can provide ...Robot() and ...Original() versions as well.
- objpose::ProvidedObjectPose ObjectPose::toProvidedObjectPoseGlobal() const
+ objpose::ProvidedObjectPose
+ ObjectPose::toProvidedObjectPoseGlobal() const
{
objpose::ProvidedObjectPose providedObjectPose;
@@ -198,8 +206,9 @@ namespace armarx::objpose
return providedObjectPose;
}
- void ObjectPose::setObjectPoseRobot(
- const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal)
+ void
+ ObjectPose::setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot,
+ bool updateObjectPoseGlobal)
{
this->objectPoseRobot = objectPoseRobot;
if (updateObjectPoseGlobal)
@@ -208,8 +217,9 @@ namespace armarx::objpose
}
}
- void ObjectPose::setObjectPoseGlobal(
- const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot)
+ void
+ ObjectPose::setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal,
+ bool updateObjectPoseRobot)
{
this->objectPoseGlobal = objectPoseGlobal;
if (updateObjectPoseRobot)
@@ -218,8 +228,8 @@ namespace armarx::objpose
}
}
-
- std::optional<simox::OrientedBoxf> ObjectPose::oobbRobot() const
+ std::optional<simox::OrientedBoxf>
+ ObjectPose::oobbRobot() const
{
if (localOOBB)
{
@@ -228,7 +238,8 @@ namespace armarx::objpose
return std::nullopt;
}
- std::optional<simox::OrientedBoxf> ObjectPose::oobbGlobal() const
+ std::optional<simox::OrientedBoxf>
+ ObjectPose::oobbGlobal() const
{
if (localOOBB)
{
@@ -237,7 +248,8 @@ namespace armarx::objpose
return std::nullopt;
}
- objpose::ObjectPose ObjectPose::getAttached(VirtualRobot::RobotPtr agent) const
+ objpose::ObjectPose
+ ObjectPose::getAttached(VirtualRobot::RobotPtr agent) const
{
ARMARX_CHECK(attachment);
@@ -246,7 +258,8 @@ namespace armarx::objpose
return updated;
}
- void ObjectPose::updateAttached(VirtualRobot::RobotPtr agent)
+ void
+ ObjectPose::updateAttached(VirtualRobot::RobotPtr agent)
{
ARMARX_CHECK(attachment);
ARMARX_CHECK_NOT_NULL(agent);
@@ -265,20 +278,22 @@ namespace armarx::objpose
robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
}
-}
-
+} // namespace armarx::objpose
namespace armarx
{
- void objpose::fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment)
+ void
+ objpose::fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment)
{
attachment.agentName = ice.agentName;
attachment.frameName = ice.frameName;
attachment.poseInFrame = ::armarx::fromIce(ice.poseInFrame);
}
- void objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice, std::optional<ObjectAttachmentInfo>& attachment)
+ void
+ objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice,
+ std::optional<ObjectAttachmentInfo>& attachment)
{
if (ice)
{
@@ -291,7 +306,8 @@ namespace armarx
}
}
- std::optional<objpose::ObjectAttachmentInfo> objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice)
+ std::optional<objpose::ObjectAttachmentInfo>
+ objpose::fromIce(const data::ObjectAttachmentInfoPtr& ice)
{
if (!ice)
{
@@ -302,14 +318,17 @@ namespace armarx
return info;
}
- void objpose::toIce(data::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment)
+ void
+ objpose::toIce(data::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment)
{
ice.agentName = attachment.agentName;
ice.frameName = attachment.frameName;
ice.poseInFrame = new Pose(attachment.poseInFrame);
}
- void objpose::toIce(data::ObjectAttachmentInfoPtr& ice, const std::optional<ObjectAttachmentInfo>& attachment)
+ void
+ objpose::toIce(data::ObjectAttachmentInfoPtr& ice,
+ const std::optional<ObjectAttachmentInfo>& attachment)
{
if (attachment)
{
@@ -322,7 +341,8 @@ namespace armarx
}
}
- objpose::data::ObjectAttachmentInfoPtr objpose::toIce(const std::optional<ObjectAttachmentInfo>& attachment)
+ objpose::data::ObjectAttachmentInfoPtr
+ objpose::toIce(const std::optional<ObjectAttachmentInfo>& attachment)
{
if (!attachment)
{
@@ -333,17 +353,20 @@ namespace armarx
return ice;
}
-
- void objpose::fromIce(const data::ObjectPose& ice, ObjectPose& pose)
+ void
+ objpose::fromIce(const data::ObjectPose& ice, ObjectPose& pose)
{
pose.fromIce(ice);
}
- objpose::ObjectPose objpose::fromIce(const data::ObjectPose& ice)
+
+ objpose::ObjectPose
+ objpose::fromIce(const data::ObjectPose& ice)
{
return ObjectPose(ice);
}
- void objpose::fromIce(const data::ObjectPoseSeq& ice, ObjectPoseSeq& poses)
+ void
+ objpose::fromIce(const data::ObjectPoseSeq& ice, ObjectPoseSeq& poses)
{
poses.clear();
poses.reserve(ice.size());
@@ -352,24 +375,29 @@ namespace armarx
poses.emplace_back(i);
}
}
- objpose::ObjectPoseSeq objpose::fromIce(const data::ObjectPoseSeq& ice)
+
+ objpose::ObjectPoseSeq
+ objpose::fromIce(const data::ObjectPoseSeq& ice)
{
ObjectPoseSeq poses;
fromIce(ice, poses);
return poses;
}
-
- void objpose::toIce(data::ObjectPose& ice, const ObjectPose& pose)
+ void
+ objpose::toIce(data::ObjectPose& ice, const ObjectPose& pose)
{
pose.toIce(ice);
}
- objpose::data::ObjectPose objpose::toIce(const ObjectPose& pose)
+
+ objpose::data::ObjectPose
+ objpose::toIce(const ObjectPose& pose)
{
return pose.toIce();
}
- void objpose::toIce(data::ObjectPoseSeq& ice, const ObjectPoseSeq& poses)
+ void
+ objpose::toIce(data::ObjectPoseSeq& ice, const ObjectPoseSeq& poses)
{
ice.clear();
ice.reserve(poses.size());
@@ -378,15 +406,17 @@ namespace armarx
ice.emplace_back(p.toIce());
}
}
- objpose::data::ObjectPoseSeq objpose::toIce(const ObjectPoseSeq& poses)
+
+ objpose::data::ObjectPoseSeq
+ objpose::toIce(const ObjectPoseSeq& poses)
{
data::ObjectPoseSeq ice;
toIce(ice, poses);
return ice;
}
-
- objpose::ObjectPose* objpose::findObjectPoseByID(ObjectPoseSeq& objectPoses, const ObjectID& id)
+ objpose::ObjectPose*
+ objpose::findObjectPoseByID(ObjectPoseSeq& objectPoses, const ObjectID& id)
{
for (objpose::ObjectPose& pose : objectPoses)
{
@@ -398,7 +428,8 @@ namespace armarx
return nullptr;
}
- const objpose::ObjectPose* objpose::findObjectPoseByID(const ObjectPoseSeq& objectPoses, const ObjectID& id)
+ const objpose::ObjectPose*
+ objpose::findObjectPoseByID(const ObjectPoseSeq& objectPoses, const ObjectID& id)
{
for (const objpose::ObjectPose& pose : objectPoses)
{
@@ -410,7 +441,8 @@ namespace armarx
return nullptr;
}
- objpose::data::ObjectPose* objpose::findObjectPoseByID(data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id)
+ objpose::data::ObjectPose*
+ objpose::findObjectPoseByID(data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id)
{
for (objpose::data::ObjectPose& pose : objectPoses)
{
@@ -422,7 +454,9 @@ namespace armarx
return nullptr;
}
- const objpose::data::ObjectPose* objpose::findObjectPoseByID(const data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id)
+ const objpose::data::ObjectPose*
+ objpose::findObjectPoseByID(const data::ObjectPoseSeq& objectPoses,
+ const armarx::data::ObjectID& id)
{
for (const objpose::data::ObjectPose& pose : objectPoses)
{
@@ -434,4 +468,4 @@ namespace armarx
return nullptr;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 6ee27fa6e039a862217d48601ad8d2d02881a615..419e4dd5f067e261aa6ce80ccb7de9009e235ce0 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -1,23 +1,21 @@
#pragma once
-#include <optional>
#include <map>
+#include <optional>
#include <vector>
#include <Eigen/Core>
#include <SimoxUtility/shapes/OrientedBox.h>
-
#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <RobotAPI/interface/objectpose/object_pose_types.h>
-#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
namespace armarx::objpose
{
@@ -29,14 +27,12 @@ namespace armarx::objpose
Eigen::Matrix4f poseInFrame = Eigen::Matrix4f::Identity();
};
-
/**
* @brief An object pose as stored by the ObjectPoseStorage.
*/
struct ObjectPose
{
public:
-
ObjectPose();
ObjectPose(const data::ObjectPose& ice);
@@ -45,15 +41,17 @@ namespace armarx::objpose
data::ObjectPose toIce() const;
void toIce(data::ObjectPose& ice) const;
- void fromProvidedPose(const data::ProvidedObjectPose& provided, VirtualRobot::RobotPtr robot = nullptr);
+ void fromProvidedPose(const data::ProvidedObjectPose& provided,
+ VirtualRobot::RobotPtr robot = nullptr);
objpose::ProvidedObjectPose toProvidedObjectPoseGlobal() const;
- void setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot, bool updateObjectPoseGlobal = true);
- void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal, bool updateObjectPoseRobot = true);
+ void setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot,
+ bool updateObjectPoseGlobal = true);
+ void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal,
+ bool updateObjectPoseRobot = true);
public:
-
/// The object ID, i.e. dataset, class name and instance name.
armarx::ObjectID objectID;
@@ -123,12 +121,11 @@ namespace armarx::objpose
std::optional<PackagePath> articulatedSimoxXmlPath;
-
};
-
void fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment);
- void fromIce(const data::ObjectAttachmentInfoPtr& ice, std::optional<ObjectAttachmentInfo>& attachment);
+ void fromIce(const data::ObjectAttachmentInfoPtr& ice,
+ std::optional<ObjectAttachmentInfo>& attachment);
std::optional<ObjectAttachmentInfo> fromIce(const data::ObjectAttachmentInfoPtr& ice);
void fromIce(const data::ObjectPose& ice, ObjectPose& pose);
@@ -139,7 +136,8 @@ namespace armarx::objpose
void toIce(data::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment);
- void toIce(data::ObjectAttachmentInfoPtr& ice, const std::optional<ObjectAttachmentInfo>& attachment);
+ void toIce(data::ObjectAttachmentInfoPtr& ice,
+ const std::optional<ObjectAttachmentInfo>& attachment);
data::ObjectAttachmentInfoPtr toIce(const std::optional<ObjectAttachmentInfo>& ice);
void toIce(data::ObjectPose& ice, const ObjectPose& pose);
@@ -160,7 +158,9 @@ namespace armarx::objpose
ObjectPose* findObjectPoseByID(ObjectPoseSeq& objectPoses, const ObjectID& id);
const ObjectPose* findObjectPoseByID(const ObjectPoseSeq& objectPoses, const ObjectID& id);
- data::ObjectPose* findObjectPoseByID(data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id);
- const data::ObjectPose* findObjectPoseByID(const data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id);
+ data::ObjectPose* findObjectPoseByID(data::ObjectPoseSeq& objectPoses,
+ const armarx::data::ObjectID& id);
+ const data::ObjectPose* findObjectPoseByID(const data::ObjectPoseSeq& objectPoses,
+ const armarx::data::ObjectID& id);
-}
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
index daeef9aa09ffeb808a1fe4e799140121f1c6971c..bf08d9f7dc39e1e377f232b645e679a4442a5992 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -1,7 +1,8 @@
#include "ObjectPoseClient.h"
+
#include <optional>
-#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
namespace armarx::objpose
{
@@ -10,31 +11,25 @@ namespace armarx::objpose
{
}
-
- ObjectPoseClient::ObjectPoseClient(
- const ObjectPoseStorageInterfacePrx& objectPoseStorage,
- const ObjectFinder& finder) :
+ ObjectPoseClient::ObjectPoseClient(const ObjectPoseStorageInterfacePrx& objectPoseStorage,
+ const ObjectFinder& finder) :
objectFinder(finder)
{
this->connect(objectPoseStorage);
}
-
void
- ObjectPoseClient::connect(
- const ObjectPoseStorageInterfacePrx& objectPoseStorage)
+ ObjectPoseClient::connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage)
{
this->objectPoseStorage = objectPoseStorage;
}
-
bool
ObjectPoseClient::isConnected() const
{
return bool(objectPoseStorage);
}
-
ObjectPoseSeq
ObjectPoseClient::fetchObjectPoses() const
{
@@ -46,7 +41,6 @@ namespace armarx::objpose
return fromIce(objectPoseStorage->getObjectPoses());
}
-
ObjectPoseMap
ObjectPoseClient::fetchObjectPosesAsMap() const
{
@@ -58,14 +52,13 @@ namespace armarx::objpose
return map;
}
-
std::optional<ObjectPose>
ObjectPoseClient::fetchObjectPose(const ObjectID& objectID) const
{
const auto objectPoses = fetchObjectPoses();
- const auto *object = findObjectPoseByID(objectPoses, objectID);
+ const auto* object = findObjectPoseByID(objectPoses, objectID);
- if(object != nullptr)
+ if (object != nullptr)
{
return *object;
}
@@ -74,19 +67,19 @@ namespace armarx::objpose
}
std::optional<ObjectPose>
- ObjectPoseClient::fetchObjectPoseFromProvider(const ObjectID& objectID, const std::string& providerName) const
+ ObjectPoseClient::fetchObjectPoseFromProvider(const ObjectID& objectID,
+ const std::string& providerName) const
{
const auto objectPoses = fetchObjectPosesFromProvider(providerName);
- const auto *object = findObjectPoseByID(objectPoses, objectID);
+ const auto* object = findObjectPoseByID(objectPoses, objectID);
- if(object != nullptr)
+ if (object != nullptr)
{
return *object;
}
return std::nullopt;
}
-
ObjectPoseSeq
ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName) const
@@ -99,18 +92,16 @@ namespace armarx::objpose
return fromIce(objectPoseStorage->getObjectPosesByProvider(providerName));
}
-
const ObjectPoseStorageInterfacePrx&
ObjectPoseClient::getObjectPoseStorage() const
{
return objectPoseStorage;
}
-
const ObjectFinder&
ObjectPoseClient::getObjectFinder() const
{
return objectFinder;
}
-}
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
index f2bfb04410bd1d0ba0a3cec4bc9eed14edd26dfb..52825c0f23155a0582d117472e2cee053c05d97f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
@@ -3,11 +3,10 @@
#include <optional>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
namespace armarx::objpose
{
@@ -18,14 +17,11 @@ namespace armarx::objpose
class ObjectPoseClient
{
public:
-
/// Construct a disconnected client.
ObjectPoseClient();
/// Construct a client and connect it to the object pose storage.
- ObjectPoseClient(
- const ObjectPoseStorageInterfacePrx& objectPoseStorage,
- const ObjectFinder& finder = {}
- );
+ ObjectPoseClient(const ObjectPoseStorageInterfacePrx& objectPoseStorage,
+ const ObjectFinder& finder = {});
/**
* @brief Connect to the given object pose storage.
@@ -34,8 +30,7 @@ namespace armarx::objpose
*
* @param objectPoseStorage The object pose storage.
*/
- void
- connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
+ void connect(const ObjectPoseStorageInterfacePrx& objectPoseStorage);
/**
* @brief Indicate whether this client is connected to an object pose
@@ -48,23 +43,20 @@ namespace armarx::objpose
*
* @return True if connected
*/
- bool
- isConnected() const;
+ bool isConnected() const;
/**
* @brief Fetch all known object poses.
* @return The known object poses.
*/
- ObjectPoseSeq
- fetchObjectPoses() const;
+ ObjectPoseSeq fetchObjectPoses() const;
/**
* @brief Fetch all known object poses.
* @return The known object poses, with object ID as key.
*/
- ObjectPoseMap
- fetchObjectPosesAsMap() const;
+ ObjectPoseMap fetchObjectPosesAsMap() const;
/**
* @brief Fetch the pose of a single object.
@@ -75,9 +67,8 @@ namespace armarx::objpose
* @param objectID The object's ID.
* @return The object's pose, if known.
*/
- std::optional<ObjectPose>
- fetchObjectPose(const ObjectID& objectID) const;
-
+ std::optional<ObjectPose> fetchObjectPose(const ObjectID& objectID) const;
+
/**
* @brief Fetch the pose of a single object and a single provider.
*
@@ -88,36 +79,32 @@ namespace armarx::objpose
* @return The object's pose, if known.
*/
std::optional<ObjectPose>
- fetchObjectPoseFromProvider(const ObjectID& objectID, const std::string& providerName) const;
+ fetchObjectPoseFromProvider(const ObjectID& objectID,
+ const std::string& providerName) const;
/**
* @brief Fetch object poses from a specific provider.
* @param providerName The provider's name.
* @return The object poses from that provider.
*/
- ObjectPoseSeq
- fetchObjectPosesFromProvider(const std::string& providerName) const;
+ ObjectPoseSeq fetchObjectPosesFromProvider(const std::string& providerName) const;
/**
* @brief Get the object pose storage's proxy.
*/
- const ObjectPoseStorageInterfacePrx&
- getObjectPoseStorage() const;
+ const ObjectPoseStorageInterfacePrx& getObjectPoseStorage() const;
/**
* @brief Get the internal object finder.
*/
- const ObjectFinder&
- getObjectFinder() const;
+ const ObjectFinder& getObjectFinder() const;
public:
-
ObjectPoseStorageInterfacePrx objectPoseStorage;
ObjectFinder objectFinder;
-
};
-}
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
index 58eaacb3199fe90eccc6c42ab1401d36eefbd452..a366f977a8709fd291e3335f9c942ae23d41cc9e 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.cpp
@@ -1,8 +1,7 @@
#include "PoseManifoldGaussian.h"
-#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/pose/invert.h>
-
+#include <SimoxUtility/math/pose/pose.h>
namespace armarx::objpose
{
@@ -31,7 +30,7 @@ namespace armarx::objpose
* ( e_y ) = ev, (e_x >= e_y >= e_z)
* ( e_z )
*/
- result.orientation = evec.determinant() > 0 ? evec : - evec;
+ result.orientation = evec.determinant() > 0 ? evec : -evec;
result.size = eval;
@@ -41,7 +40,6 @@ namespace armarx::objpose
return result;
}
-
Eigen::AngleAxisf
PoseManifoldGaussian::getScaledRotationAxis(int index, bool global) const
{
@@ -55,12 +53,12 @@ namespace armarx::objpose
return {angle, axis};
}
-
PoseManifoldGaussian
PoseManifoldGaussian::getTransformed(const Eigen::Matrix4f& transform) const
{
Eigen::Matrix<float, 6, 6> covRotation = Eigen::Matrix<float, 6, 6>::Zero();
- covRotation.block<3, 3>(0, 0) = covRotation.block<3, 3>(3, 3) = simox::math::orientation(transform);
+ covRotation.block<3, 3>(0, 0) = covRotation.block<3, 3>(3, 3) =
+ simox::math::orientation(transform);
PoseManifoldGaussian transformed = *this;
transformed.mean = transform * transformed.mean;
@@ -69,9 +67,9 @@ namespace armarx::objpose
return transformed;
}
-
PoseManifoldGaussian
- PoseManifoldGaussian::getTransformed(const Eigen::Matrix4f& fromFrame, const Eigen::Matrix4f& toFrame) const
+ PoseManifoldGaussian::getTransformed(const Eigen::Matrix4f& fromFrame,
+ const Eigen::Matrix4f& toFrame) const
{
// T_to = T_(from->to) * T_from
// T_(from->to) = T_to * (T_from)^-1
@@ -79,4 +77,4 @@ namespace armarx::objpose
return getTransformed(transform);
}
-}
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h
index 0de894c8a945336e14c6af618fdc1660d21eab01..22cabc3687411d9e2be17ce6ed5486042c49b072 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h
@@ -3,7 +3,6 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
-
namespace armarx::objpose
{
@@ -25,7 +24,6 @@ namespace armarx::objpose
struct PoseManifoldGaussian
{
public:
-
/// The mean (i.e. a pose).
Eigen::Matrix4f mean = Eigen::Matrix4f::Identity();
/// The covariance matrix.
@@ -33,13 +31,13 @@ namespace armarx::objpose
public:
-
/// Get the pure positional part of the covariance matrix.
Eigen::Block<Eigen::Matrix<float, 6, 6>, 3, 3>
positionCovariance()
{
return covariance.block<3, 3>(0, 0);
}
+
Eigen::Block<const Eigen::Matrix<float, 6, 6>, 3, 3>
positionCovariance() const
{
@@ -52,13 +50,13 @@ namespace armarx::objpose
{
return covariance.block<3, 3>(3, 3);
}
+
Eigen::Block<const Eigen::Matrix<float, 6, 6>, 3, 3>
orientationCovariance() const
{
return covariance.block<3, 3>(3, 3);
}
-
struct Ellipsoid
{
Eigen::Vector3f center;
@@ -67,8 +65,7 @@ namespace armarx::objpose
};
/// Get the parameters of a 3D ellipsoid illustrating this gaussian.
- Ellipsoid
- getPositionEllipsoid() const;
+ Ellipsoid getPositionEllipsoid() const;
/**
@@ -81,8 +78,7 @@ namespace armarx::objpose
* If true, rotate the axis by the mean orientation.
* @return
*/
- Eigen::AngleAxisf
- getScaledRotationAxis(int index, bool global = false) const;
+ Eigen::AngleAxisf getScaledRotationAxis(int index, bool global = false) const;
/**
@@ -90,8 +86,7 @@ namespace armarx::objpose
* @param transform The transform to apply.
* @return The transformed gaussian.
*/
- PoseManifoldGaussian
- getTransformed(const Eigen::Matrix4f& transform) const;
+ PoseManifoldGaussian getTransformed(const Eigen::Matrix4f& transform) const;
/**
* @brief Transform this gaussian from one base coordinate system to another one.
@@ -99,9 +94,8 @@ namespace armarx::objpose
* @param toFrame The new base coordinate system' pose.
* @return The same gaussian in thenew coordinate system.
*/
- PoseManifoldGaussian
- getTransformed(const Eigen::Matrix4f& fromFrame, const Eigen::Matrix4f& toFrame) const;
-
+ PoseManifoldGaussian getTransformed(const Eigen::Matrix4f& fromFrame,
+ const Eigen::Matrix4f& toFrame) const;
};
-}
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp
index c07cea85884a957218d9a9903ffcdd8c674b37bd..a4699d510a548c0f5c366a1edaedfbc09e3d49f9 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.cpp
@@ -3,13 +3,12 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/ice_conversions.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include "ice_conversions.h"
-
namespace armarx::objpose
{
ProvidedObjectPose::ProvidedObjectPose()
@@ -21,7 +20,8 @@ namespace armarx::objpose
fromIce(ice);
}
- void ProvidedObjectPose::fromIce(const data::ProvidedObjectPose& ice)
+ void
+ ProvidedObjectPose::fromIce(const data::ProvidedObjectPose& ice)
{
providerName = ice.providerName;
objectType = ice.objectType;
@@ -40,14 +40,16 @@ namespace armarx::objpose
objpose::fromIce(ice.localOOBB, localOOBB);
}
- data::ProvidedObjectPose ProvidedObjectPose::toIce() const
+ data::ProvidedObjectPose
+ ProvidedObjectPose::toIce() const
{
data::ProvidedObjectPose ice;
toIce(ice);
return ice;
}
- void ProvidedObjectPose::toIce(data::ProvidedObjectPose& ice) const
+ void
+ ProvidedObjectPose::toIce(data::ProvidedObjectPose& ice) const
{
ice.providerName = providerName;
ice.objectType = objectType;
@@ -65,21 +67,25 @@ namespace armarx::objpose
objpose::toIce(ice.localOOBB, localOOBB);
}
-}
+} // namespace armarx::objpose
namespace armarx
{
- void objpose::fromIce(const data::ProvidedObjectPose& ice, ProvidedObjectPose& pose)
+ void
+ objpose::fromIce(const data::ProvidedObjectPose& ice, ProvidedObjectPose& pose)
{
pose.fromIce(ice);
}
- objpose::ProvidedObjectPose objpose::fromIce(const data::ProvidedObjectPose& ice)
+
+ objpose::ProvidedObjectPose
+ objpose::fromIce(const data::ProvidedObjectPose& ice)
{
return ProvidedObjectPose(ice);
}
- void objpose::fromIce(const data::ProvidedObjectPoseSeq& ice, ProvidedObjectPoseSeq& poses)
+ void
+ objpose::fromIce(const data::ProvidedObjectPoseSeq& ice, ProvidedObjectPoseSeq& poses)
{
poses.clear();
poses.reserve(ice.size());
@@ -89,26 +95,28 @@ namespace armarx
}
}
- objpose::ProvidedObjectPoseSeq objpose::fromIce(const data::ProvidedObjectPoseSeq& ice)
+ objpose::ProvidedObjectPoseSeq
+ objpose::fromIce(const data::ProvidedObjectPoseSeq& ice)
{
ProvidedObjectPoseSeq poses;
fromIce(ice, poses);
return poses;
}
-
- void objpose::toIce(data::ProvidedObjectPose& ice, const ProvidedObjectPose& pose)
+ void
+ objpose::toIce(data::ProvidedObjectPose& ice, const ProvidedObjectPose& pose)
{
pose.toIce(ice);
}
- objpose::data::ProvidedObjectPose objpose::toIce(const ProvidedObjectPose& pose)
+ objpose::data::ProvidedObjectPose
+ objpose::toIce(const ProvidedObjectPose& pose)
{
return pose.toIce();
}
-
- void objpose::toIce(data::ProvidedObjectPoseSeq& ice, const ProvidedObjectPoseSeq& poses)
+ void
+ objpose::toIce(data::ProvidedObjectPoseSeq& ice, const ProvidedObjectPoseSeq& poses)
{
ice.clear();
ice.reserve(poses.size());
@@ -118,14 +126,12 @@ namespace armarx
}
}
- objpose::data::ProvidedObjectPoseSeq objpose::toIce(const ProvidedObjectPoseSeq& poses)
+ objpose::data::ProvidedObjectPoseSeq
+ objpose::toIce(const ProvidedObjectPoseSeq& poses)
{
data::ProvidedObjectPoseSeq ice;
toIce(ice, poses);
return ice;
}
-}
-
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h
index c8d846893dbf857f1ca8bb5dbaabae1ba0b1b14b..dd19de5107620e1a59926ffcee3bbe15b6ef78f2 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h
@@ -1,7 +1,7 @@
#pragma once
-#include <optional>
#include <map>
+#include <optional>
#include <vector>
#include <Eigen/Core>
@@ -11,10 +11,9 @@
#include <ArmarXCore/core/time/DateTime.h>
#include <RobotAPI/interface/objectpose/object_pose_types.h>
-#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
namespace armarx::objpose
{
@@ -25,7 +24,6 @@ namespace armarx::objpose
class ProvidedObjectPose
{
public:
-
ProvidedObjectPose();
ProvidedObjectPose(const data::ProvidedObjectPose& ice);
@@ -35,7 +33,6 @@ namespace armarx::objpose
void toIce(data::ProvidedObjectPose& ice) const;
public:
-
/// Name of the providing component.
std::string providerName;
/// Known or unknown object.
@@ -64,8 +61,6 @@ namespace armarx::objpose
std::optional<simox::OrientedBoxf> localOOBB;
};
-
-
void fromIce(const data::ProvidedObjectPose& ice, ProvidedObjectPose& pose);
ProvidedObjectPose fromIce(const data::ProvidedObjectPose& ice);
@@ -80,4 +75,4 @@ namespace armarx::objpose
data::ProvidedObjectPoseSeq toIce(const ProvidedObjectPoseSeq& poses);
-}
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/Scene.cpp b/source/RobotAPI/libraries/ArmarXObjects/Scene.cpp
index 1437da55faff8976d7c87ac9f76f9a2adf11432f..066a87dd3b3ce27d5b997e647507e22e1413943a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/Scene.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/Scene.cpp
@@ -1,19 +1,19 @@
#include "Scene.h"
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
namespace armarx::objects
{
- ObjectID SceneObject::getClassID() const
+ ObjectID
+ SceneObject::getClassID() const
{
return ObjectID(className);
}
-
- ObjectID SceneObject::getClassID(ObjectFinder& finder) const
+ ObjectID
+ SceneObject::getClassID(ObjectFinder& finder) const
{
ObjectID id = getClassID();
if (id.dataset().empty())
@@ -26,19 +26,16 @@ namespace armarx::objects
return id;
}
-
- ObjectID SceneObject::getObjectID() const
+ ObjectID
+ SceneObject::getObjectID() const
{
return getClassID().withInstanceName(instanceName);
}
-
- ObjectID SceneObject::getObjectID(ObjectFinder& finder) const
+ ObjectID
+ SceneObject::getObjectID(ObjectFinder& finder) const
{
return getClassID(finder).withInstanceName(instanceName);
}
-}
-
-
-
+} // namespace armarx::objects
diff --git a/source/RobotAPI/libraries/ArmarXObjects/Scene.h b/source/RobotAPI/libraries/ArmarXObjects/Scene.h
index 3c2767c017ce2ac19faef9bde78b7ede5bba8b13..53e6bb929cf45483105769c9af0f53d69ae152b4 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/Scene.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/Scene.h
@@ -22,16 +22,15 @@
#pragma once
#include <map>
+#include <optional>
#include <string>
#include <vector>
-#include <optional>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "forward_declarations.h"
-
namespace armarx::objects
{
@@ -58,4 +57,4 @@ namespace armarx::objects
std::vector<SceneObject> objects;
};
-}
+} // namespace armarx::objects
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.cpp
index 84679a92c420257f7b072c749ba9b4dfb5eb5760..d487cf2d7238af72f2a9f8a6eb73da2fa99ac6a7 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.cpp
@@ -2,30 +2,31 @@
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-void armarx::fromAron(const arondto::ObjectID& dto, ObjectID& bo)
+void
+armarx::fromAron(const arondto::ObjectID& dto, ObjectID& bo)
{
bo = ObjectID(dto.dataset, dto.className, dto.instanceName);
}
-void armarx::toAron(arondto::ObjectID& dto, const ObjectID& bo)
+
+void
+armarx::toAron(arondto::ObjectID& dto, const ObjectID& bo)
{
aron::toAron(dto.dataset, bo.dataset());
aron::toAron(dto.className, bo.className());
aron::toAron(dto.instanceName, bo.instanceName());
-
}
-
-void armarx::fromAron(const armarx::arondto::PackagePath& dto, armarx::PackageFileLocation& bo)
+void
+armarx::fromAron(const armarx::arondto::PackagePath& dto, armarx::PackageFileLocation& bo)
{
aron::toAron(bo.package, dto.package);
aron::toAron(bo.relativePath, dto.path);
aron::toAron(bo.absolutePath, std::filesystem::path(""));
-
}
-void armarx::toAron(armarx::arondto::PackagePath& dto, const armarx::PackageFileLocation& bo)
+void
+armarx::toAron(armarx::arondto::PackagePath& dto, const armarx::PackageFileLocation& bo)
{
aron::toAron(dto.package, bo.package);
aron::toAron(dto.path, bo.relativePath);
-
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h
index 08c60f45a3e27d1372f77d620a3e5efffd5c7400..b68ad4cfe260ac3308d460028dbc546f9be9c393 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h
@@ -1,11 +1,9 @@
#pragma once
-#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h> // For PackageFileLocation
-#include <RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>
-
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h> // For PackageFileLocation
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
-
+#include <RobotAPI/libraries/aron/common/aron/PackagePath.aron.generated.h>
namespace armarx
{
@@ -14,4 +12,4 @@ namespace armarx
void fromAron(const arondto::ObjectID& dto, ObjectID& bo);
void toAron(arondto::ObjectID& dto, const ObjectID& bo);
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
index 13fd177b7a546ad5663464833629199c5a52a79e..5e7f4e694d923b5cebc02b09547fc014dbfbb90a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.cpp
@@ -2,47 +2,45 @@
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
-
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/PoseManifoldGaussian.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-void armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
+void
+armarx::objpose::fromAron(const arondto::ObjectAttachmentInfo& dto, ObjectAttachmentInfo& bo)
{
aron::fromAron(dto.frameName, bo.frameName);
aron::fromAron(dto.agentName, bo.agentName);
aron::fromAron(dto.poseInFrame, bo.poseInFrame);
-
}
-void armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo)
+
+void
+armarx::objpose::toAron(arondto::ObjectAttachmentInfo& dto, const ObjectAttachmentInfo& bo)
{
aron::toAron(dto.frameName, bo.frameName);
aron::toAron(dto.agentName, bo.agentName);
aron::toAron(dto.poseInFrame, bo.poseInFrame);
-
}
-
-void armarx::objpose::fromAron(const arondto::PoseManifoldGaussian& dto, PoseManifoldGaussian& bo)
+void
+armarx::objpose::fromAron(const arondto::PoseManifoldGaussian& dto, PoseManifoldGaussian& bo)
{
aron::fromAron(dto.mean, bo.mean);
aron::fromAron(dto.covariance, bo.covariance);
-
}
-void armarx::objpose::toAron(arondto::PoseManifoldGaussian& dto, const PoseManifoldGaussian& bo)
+
+void
+armarx::objpose::toAron(arondto::PoseManifoldGaussian& dto, const PoseManifoldGaussian& bo)
{
aron::toAron(dto.mean, bo.mean);
aron::toAron(dto.covariance, bo.covariance);
-
}
-
-void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectType& bo)
+void
+armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectType& bo)
{
switch (dto.value)
{
@@ -58,7 +56,9 @@ void armarx::objpose::fromAron(const arondto::ObjectType& dto, ObjectType& bo)
}
ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
}
-void armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectType& bo)
+
+void
+armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectType& bo)
{
switch (bo)
{
@@ -75,8 +75,8 @@ void armarx::objpose::toAron(arondto::ObjectType& dto, const ObjectType& bo)
ARMARX_UNEXPECTED_ENUM_VALUE(ObjectTypeEnum, bo);
}
-
-void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
+void
+armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
{
aron::fromAron(dto.providerName, bo.providerName);
fromAron(dto.objectType, bo.objectType);
@@ -120,8 +120,8 @@ void armarx::objpose::fromAron(const arondto::ObjectPose& dto, ObjectPose& bo)
}
}
-
-void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
+void
+armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
{
aron::toAron(dto.providerName, bo.providerName);
toAron(dto.objectType, bo.objectType);
@@ -167,4 +167,3 @@ void armarx::objpose::toAron(arondto::ObjectPose& dto, const ObjectPose& bo)
toAron(dto.localOOBB, simox::OrientedBoxf());
}
}
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
index 32dfe160ccd41007c1ad7d69c39042e8e795c571..e6617ad149ad5272423787c8a0f1bd68e249b71d 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h
@@ -3,7 +3,6 @@
#include <RobotAPI/interface/objectpose/object_pose_types.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
-
namespace armarx::objpose
{
@@ -19,4 +18,4 @@ namespace armarx::objpose
void fromAron(const arondto::ObjectPose& dto, ObjectPose& bo);
void toAron(arondto::ObjectPose& dto, const ObjectPose& bo);
-}
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h b/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h
index 320305ba22474d6eca649966c7988ad7b3048c0b..09a63b2cf75646030976dbcf225b6baaf0157896 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h
@@ -3,13 +3,13 @@
#include <map>
#include <vector>
-
namespace armarx
{
class ObjectID;
class ObjectInfo;
class ObjectFinder;
-}
+} // namespace armarx
+
namespace armarx::objpose
{
struct ObjectAttachmentInfo;
@@ -26,19 +26,20 @@ namespace armarx::objpose
using ProvidedObjectPoseMap = std::map<ObjectID, ProvidedObjectPose>;
class ObjectPoseClient;
-}
+} // namespace armarx::objpose
+
namespace armarx::objects
{
struct Scene;
struct SceneObject;
-}
-
+} // namespace armarx::objects
// Ice Types
namespace armarx::objpose::data
{
class PoseManifoldGaussian;
}
+
// Aron Types
namespace armarx::objpose::arondto
{
@@ -46,4 +47,4 @@ namespace armarx::objpose::arondto
class ObjectType;
class ObjectPose;
struct PoseManifoldGaussian;
-}
+} // namespace armarx::objpose::arondto
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
index de7c0595614d31765639592e0d5babb7858b8912..076efcfe15a6199baa22dda5e59ddf69871d4a8e 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp
@@ -1,24 +1,22 @@
#include "ice_conversions.h"
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotConfig.h>
-
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx
{
- std::ostream& data::operator<<(std::ostream& os, const ObjectID& id)
+ std::ostream&
+ data::operator<<(std::ostream& os, const ObjectID& id)
{
os << "'" << id.dataset << "/" << id.className;
if (!id.instanceName.empty())
@@ -28,54 +26,66 @@ namespace armarx
return os << "'";
}
-}
+} // namespace armarx
-void armarx::fromIce(const data::ObjectID& ice, ObjectID& id)
+void
+armarx::fromIce(const data::ObjectID& ice, ObjectID& id)
{
id = fromIce(ice);
}
-armarx::ObjectID armarx::fromIce(const data::ObjectID& ice)
+armarx::ObjectID
+armarx::fromIce(const data::ObjectID& ice)
{
- return { ice.dataset, ice.className, ice.instanceName };
+ return {ice.dataset, ice.className, ice.instanceName};
}
-void armarx::toIce(data::ObjectID& ice, const ObjectID& id)
+void
+armarx::toIce(data::ObjectID& ice, const ObjectID& id)
{
ice.dataset = id.dataset();
ice.className = id.className();
ice.instanceName = id.instanceName();
}
-armarx::data::ObjectID armarx::toIce(const ObjectID& id)
+armarx::data::ObjectID
+armarx::toIce(const ObjectID& id)
{
data::ObjectID ice;
toIce(ice, id);
return ice;
}
-void armarx::fromIce(const data::ObjectIDSeq& ice, std::vector<ObjectID>& ids)
+void
+armarx::fromIce(const data::ObjectIDSeq& ice, std::vector<ObjectID>& ids)
{
ids.clear();
- std::transform(ice.begin(), ice.end(), std::back_inserter(ids),
- static_cast<ObjectID(*)(const data::ObjectID&)>(&fromIce));
+ std::transform(ice.begin(),
+ ice.end(),
+ std::back_inserter(ids),
+ static_cast<ObjectID (*)(const data::ObjectID&)>(&fromIce));
}
-std::vector<armarx::ObjectID> armarx::fromIce(const data::ObjectIDSeq& ice)
+std::vector<armarx::ObjectID>
+armarx::fromIce(const data::ObjectIDSeq& ice)
{
std::vector<ObjectID> ids;
fromIce(ice, ids);
return ids;
}
-void armarx::toIce(data::ObjectIDSeq& ice, const std::vector<ObjectID>& ids)
+void
+armarx::toIce(data::ObjectIDSeq& ice, const std::vector<ObjectID>& ids)
{
ice.clear();
- std::transform(ids.begin(), ids.end(), std::back_inserter(ice),
- static_cast<data::ObjectID(*)(const ObjectID&)>(&toIce));
+ std::transform(ids.begin(),
+ ids.end(),
+ std::back_inserter(ice),
+ static_cast<data::ObjectID (*)(const ObjectID&)>(&toIce));
}
-armarx::data::ObjectIDSeq armarx::toIce(const std::vector<ObjectID>& ids)
+armarx::data::ObjectIDSeq
+armarx::toIce(const std::vector<ObjectID>& ids)
{
data::ObjectIDSeq ice;
toIce(ice, ids);
@@ -84,14 +94,13 @@ armarx::data::ObjectIDSeq armarx::toIce(const std::vector<ObjectID>& ids)
namespace armarx
{
- const simox::meta::EnumNames<objpose::ObjectType> objpose::ObjectTypeNames =
- {
- { objpose::ObjectType::AnyObject, "AnyObject" },
- { objpose::ObjectType::KnownObject, "KnownObject" },
- { objpose::ObjectType::UnknownObject, "UnknownObject" }
- };
+ const simox::meta::EnumNames<objpose::ObjectType> objpose::ObjectTypeNames = {
+ {objpose::ObjectType::AnyObject, "AnyObject"},
+ {objpose::ObjectType::KnownObject, "KnownObject"},
+ {objpose::ObjectType::UnknownObject, "UnknownObject"}};
- objpose::AABB objpose::toIce(const simox::AxisAlignedBoundingBox& aabb)
+ objpose::AABB
+ objpose::toIce(const simox::AxisAlignedBoundingBox& aabb)
{
objpose::AABB ice;
ice.center = new Vector3(aabb.center());
@@ -99,7 +108,8 @@ namespace armarx
return ice;
}
- void objpose::fromIce(const Box& box, simox::OrientedBoxf& oobb)
+ void
+ objpose::fromIce(const Box& box, simox::OrientedBoxf& oobb)
{
try
{
@@ -108,10 +118,8 @@ namespace armarx
Eigen::Vector3f extents = armarx::fromIce(box.extents);
Eigen::Vector3f corner = pos - ori * extents / 2;
- oobb = simox::OrientedBox<float> (corner,
- ori.col(0) * extents(0),
- ori.col(1) * extents(1),
- ori.col(2) * extents(2));
+ oobb = simox::OrientedBox<float>(
+ corner, ori.col(0) * extents(0), ori.col(1) * extents(1), ori.col(2) * extents(2));
}
catch (const armarx::LocalException&)
{
@@ -119,7 +127,9 @@ namespace armarx
oobb = {};
}
}
- void objpose::fromIce(const BoxPtr& box, std::optional<simox::OrientedBox<float>>& oobb)
+
+ void
+ objpose::fromIce(const BoxPtr& box, std::optional<simox::OrientedBox<float>>& oobb)
{
if (box)
{
@@ -131,21 +141,24 @@ namespace armarx
}
}
- simox::OrientedBoxf objpose::fromIce(const Box& box)
+ simox::OrientedBoxf
+ objpose::fromIce(const Box& box)
{
simox::OrientedBoxf oobb;
fromIce(box, oobb);
return oobb;
}
- void objpose::toIce(Box& box, const simox::OrientedBoxf& oobb)
+ void
+ objpose::toIce(Box& box, const simox::OrientedBoxf& oobb)
{
box.position = new Vector3(oobb.center());
box.orientation = new Quaternion(oobb.rotation().eval());
box.extents = new Vector3(oobb.dimensions());
}
- void objpose::toIce(BoxPtr& box, const std::optional<simox::OrientedBox<float>>& oobb)
+ void
+ objpose::toIce(BoxPtr& box, const std::optional<simox::OrientedBox<float>>& oobb)
{
if (oobb)
{
@@ -154,33 +167,37 @@ namespace armarx
}
}
- objpose::Box objpose::toIce(const simox::OrientedBoxf& oobb)
+ objpose::Box
+ objpose::toIce(const simox::OrientedBoxf& oobb)
{
objpose::Box box;
toIce(box, oobb);
return box;
}
-
- void objpose::fromIce(const data::PoseManifoldGaussian& ice, PoseManifoldGaussian& cov)
+ void
+ objpose::fromIce(const data::PoseManifoldGaussian& ice, PoseManifoldGaussian& cov)
{
// Only construct error message if necessary.
if (static_cast<Eigen::Index>(ice.covariance6x6.size()) != cov.covariance.size())
{
- ARMARX_CHECK_EQUAL(static_cast<Eigen::Index>(ice.covariance6x6.size()), cov.covariance.size())
- << "Float sequence representing 6x6 covariance matrix must have " << cov.covariance.size()
- << " values, but has " << ice.covariance6x6.size() << ": \n"
- << "[" << simox::alg::join(simox::alg::multi_to_string(ice.covariance6x6), ", ") << "]"
- ;
+ ARMARX_CHECK_EQUAL(static_cast<Eigen::Index>(ice.covariance6x6.size()),
+ cov.covariance.size())
+ << "Float sequence representing 6x6 covariance matrix must have "
+ << cov.covariance.size() << " values, but has " << ice.covariance6x6.size()
+ << ": \n"
+ << "[" << simox::alg::join(simox::alg::multi_to_string(ice.covariance6x6), ", ")
+ << "]";
}
armarx::fromIce(ice.mean, cov.mean);
- cov.covariance = Eigen::MatrixXf::Map(ice.covariance6x6.data(),
- cov.covariance.rows(),
- cov.covariance.cols());
+ cov.covariance = Eigen::MatrixXf::Map(
+ ice.covariance6x6.data(), cov.covariance.rows(), cov.covariance.cols());
}
- void objpose::fromIce(const data::PoseManifoldGaussianPtr& ice, std::optional<PoseManifoldGaussian>& cov)
+ void
+ objpose::fromIce(const data::PoseManifoldGaussianPtr& ice,
+ std::optional<PoseManifoldGaussian>& cov)
{
if (ice)
{
@@ -193,15 +210,16 @@ namespace armarx
}
}
- std::optional<objpose::PoseManifoldGaussian> objpose::fromIce(const data::PoseManifoldGaussianPtr& ice)
+ std::optional<objpose::PoseManifoldGaussian>
+ objpose::fromIce(const data::PoseManifoldGaussianPtr& ice)
{
std::optional<objpose::PoseManifoldGaussian> cov;
fromIce(ice, cov);
return cov;
}
-
- void objpose::toIce(data::PoseManifoldGaussian& ice, const PoseManifoldGaussian& cov)
+ void
+ objpose::toIce(data::PoseManifoldGaussian& ice, const PoseManifoldGaussian& cov)
{
armarx::toIce(ice.mean, cov.mean);
@@ -211,7 +229,9 @@ namespace armarx
cov.covariance.cols()) = cov.covariance;
}
- void objpose::toIce(data::PoseManifoldGaussianPtr& ice, const std::optional<PoseManifoldGaussian>& cov)
+ void
+ objpose::toIce(data::PoseManifoldGaussianPtr& ice,
+ const std::optional<PoseManifoldGaussian>& cov)
{
if (cov.has_value())
{
@@ -224,11 +244,12 @@ namespace armarx
}
}
- objpose::data::PoseManifoldGaussianPtr objpose::toIce(const std::optional<PoseManifoldGaussian>& cov)
+ objpose::data::PoseManifoldGaussianPtr
+ objpose::toIce(const std::optional<PoseManifoldGaussian>& cov)
{
data::PoseManifoldGaussianPtr ice;
toIce(ice, cov);
return ice;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
index 9ee3dd712a79f8e0f812134b6588778a2b540dea..bbbf1453aeb995cc00ae2d6b5f833c8fd48e177f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h
@@ -4,19 +4,18 @@
#include <RobotAPI/interface/ArmarXObjects/ArmarXObjectsTypes.h>
#include <RobotAPI/interface/objectpose/object_pose_types.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
namespace simox
{
// #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
struct AxisAlignedBoundingBox;
// #include <SimoxUtility/shapes/OrientedBox.h>
- template<class FloatT> class OrientedBox;
-}
+ template <class FloatT>
+ class OrientedBox;
+} // namespace simox
namespace armarx::data
{
@@ -38,7 +37,7 @@ namespace armarx
void toIce(data::ObjectIDSeq& ice, const std::vector<ObjectID>& ids);
data::ObjectIDSeq toIce(const std::vector<ObjectID>& ids);
-}
+} // namespace armarx
namespace armarx::objpose
{
@@ -56,11 +55,12 @@ namespace armarx::objpose
void fromIce(const data::PoseManifoldGaussian& ice, PoseManifoldGaussian& cov);
- void fromIce(const data::PoseManifoldGaussianPtr& ice, std::optional<PoseManifoldGaussian>& cov);
+ void fromIce(const data::PoseManifoldGaussianPtr& ice,
+ std::optional<PoseManifoldGaussian>& cov);
std::optional<PoseManifoldGaussian> fromIce(const data::PoseManifoldGaussianPtr& ice);
void toIce(data::PoseManifoldGaussian& ice, const PoseManifoldGaussian& cov);
void toIce(data::PoseManifoldGaussianPtr& ice, const std::optional<PoseManifoldGaussian>& cov);
data::PoseManifoldGaussianPtr toIce(const std::optional<PoseManifoldGaussian>& ice);
-}
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
index 446b625e4e91359fadd8dd43b4dacdc460a9d513..0ea12be7f87ba3a89671670bec1e4082fc03a9b5 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
@@ -8,8 +8,8 @@
#include "Scene.h"
#include "ice_conversions.h"
-
-void armarx::to_json(simox::json::json& j, const ObjectID& id)
+void
+armarx::to_json(simox::json::json& j, const ObjectID& id)
{
j["dataset"] = id.dataset();
j["className"] = id.className();
@@ -17,17 +17,16 @@ void armarx::to_json(simox::json::json& j, const ObjectID& id)
j["str"] = id.str();
}
-void armarx::from_json(const simox::json::json& j, ObjectID& id)
+void
+armarx::from_json(const simox::json::json& j, ObjectID& id)
{
- id =
- {
- j.at("dataset").get<std::string>(),
- j.at("className").get<std::string>(),
- j.at("instanceName").get<std::string>()
- };
+ id = {j.at("dataset").get<std::string>(),
+ j.at("className").get<std::string>(),
+ j.at("instanceName").get<std::string>()};
}
-void armarx::objpose::to_json(simox::json::json& j, const ObjectPose& op)
+void
+armarx::objpose::to_json(simox::json::json& j, const ObjectPose& op)
{
j["providerName"] = op.providerName;
j["objectType"] = ObjectTypeNames.to_name(op.objectType);
@@ -51,8 +50,8 @@ void armarx::objpose::to_json(simox::json::json& j, const ObjectPose& op)
}
}
-
-void armarx::objpose::from_json(const simox::json::json& j, ObjectPose& op)
+void
+armarx::objpose::from_json(const simox::json::json& j, ObjectPose& op)
{
op.providerName = j.at("providerName");
op.objectType = ObjectTypeNames.from_name(j.at("objectType"));
@@ -76,8 +75,8 @@ void armarx::objpose::from_json(const simox::json::json& j, ObjectPose& op)
}
}
-
-void armarx::objects::to_json(simox::json::json& j, const SceneObject& rhs)
+void
+armarx::objects::to_json(simox::json::json& j, const SceneObject& rhs)
{
j["class"] = rhs.className;
j["instanceName"] = rhs.instanceName;
@@ -91,8 +90,8 @@ void armarx::objects::to_json(simox::json::json& j, const SceneObject& rhs)
j["jointValues"] = rhs.jointValues;
}
-
-void armarx::objects::from_json(const simox::json::json& j, SceneObject& rhs)
+void
+armarx::objects::from_json(const simox::json::json& j, SceneObject& rhs)
{
j.at("class").get_to(rhs.className);
if (j.count("instanceName"))
@@ -127,15 +126,14 @@ void armarx::objects::from_json(const simox::json::json& j, SceneObject& rhs)
}
}
-
-
-void armarx::objects::to_json(simox::json::json& j, const Scene& rhs)
+void
+armarx::objects::to_json(simox::json::json& j, const Scene& rhs)
{
j["objects"] = rhs.objects;
}
-
-void armarx::objects::from_json(const simox::json::json& j, Scene& rhs)
+void
+armarx::objects::from_json(const simox::json::json& j, Scene& rhs)
{
j.at("objects").get_to(rhs.objects);
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h
index 0a5e83b61aa9479d3d67344f585564c2c60d2d75..4f93c1e32567673a4f41ec9a9e9116ae4b1717e8 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h
@@ -4,19 +4,17 @@
#include "forward_declarations.h"
-
namespace armarx
{
void to_json(simox::json::json& j, const ObjectID& value);
void from_json(const simox::json::json& j, ObjectID& value);
-}
+} // namespace armarx
namespace armarx::objpose
{
void to_json(simox::json::json& j, const ObjectPose& op);
void from_json(const simox::json::json& j, ObjectPose& op);
-}
-
+} // namespace armarx::objpose
namespace armarx::objects
{
@@ -25,4 +23,4 @@ namespace armarx::objects
void to_json(simox::json::json& j, const Scene& rhs);
void from_json(const simox::json::json& j, Scene& rhs);
-}
+} // namespace armarx::objects
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
index 6525be6b5414daa0d3052f3bc2d6effc7acf5c46..b772b14003fc9b824bcbae64630bac5ce1995431 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
@@ -2,51 +2,56 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx::plugins
{
- void ObjectPoseClientPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ void
+ ObjectPoseClientPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(makePropertyName(PROPERTY_NAME)))
{
properties->defineOptionalProperty<std::string>(
- makePropertyName(PROPERTY_NAME),
- "ObjectMemory",
- "Name of the object memory.");
+ makePropertyName(PROPERTY_NAME), "ObjectMemory", "Name of the object memory.");
}
}
- void ObjectPoseClientPlugin::preOnInitComponent()
+ void
+ ObjectPoseClientPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(makePropertyName(PROPERTY_NAME));
}
- void ObjectPoseClientPlugin::preOnConnectComponent()
+ void
+ ObjectPoseClientPlugin::preOnConnectComponent()
{
_objectPoseStorage = createObjectPoseStorage();
}
- objpose::ObjectPoseStorageInterfacePrx ObjectPoseClientPlugin::createObjectPoseStorage()
+ objpose::ObjectPoseStorageInterfacePrx
+ ObjectPoseClientPlugin::createObjectPoseStorage()
{
- return parent<Component>().getProxyFromProperty<objpose::ObjectPoseStorageInterfacePrx>(makePropertyName(PROPERTY_NAME));
+ return parent<Component>().getProxyFromProperty<objpose::ObjectPoseStorageInterfacePrx>(
+ makePropertyName(PROPERTY_NAME));
}
- objpose::ObjectPoseClient ObjectPoseClientPlugin::createClient()
+ objpose::ObjectPoseClient
+ ObjectPoseClientPlugin::createClient()
{
return objpose::ObjectPoseClient(createObjectPoseStorage(), getObjectFinder());
}
- const ObjectFinder& ObjectPoseClientPlugin::setObjectFinderPath(const std::string& path)
+ const ObjectFinder&
+ ObjectPoseClientPlugin::setObjectFinderPath(const std::string& path)
{
_finder.setPath(path);
return _finder;
}
- const ObjectFinder& ObjectPoseClientPlugin::getObjectFinder() const
+ const ObjectFinder&
+ ObjectPoseClientPlugin::getObjectFinder() const
{
return _finder;
}
-}
+} // namespace armarx::plugins
namespace armarx
{
@@ -55,40 +60,45 @@ namespace armarx
addPlugin(plugin);
}
- objpose::ObjectPoseStorageInterfacePrx ObjectPoseClientPluginUser::createObjectPoseStorage()
+ objpose::ObjectPoseStorageInterfacePrx
+ ObjectPoseClientPluginUser::createObjectPoseStorage()
{
return plugin->createObjectPoseStorage();
}
-
- objpose::ObjectPoseClient ObjectPoseClientPluginUser::getClient() const
+ objpose::ObjectPoseClient
+ ObjectPoseClientPluginUser::getClient() const
{
return plugin->createClient();
}
-
- objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPoses()
+ objpose::ObjectPoseSeq
+ ObjectPoseClientPluginUser::getObjectPoses()
{
return getClient().fetchObjectPoses();
}
- objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPosesByProvider(const std::string& providerName)
+ objpose::ObjectPoseSeq
+ ObjectPoseClientPluginUser::getObjectPosesByProvider(const std::string& providerName)
{
return getClient().fetchObjectPosesFromProvider(providerName);
}
-
- plugins::ObjectPoseClientPlugin& ObjectPoseClientPluginUser::getObjectPoseClientPlugin()
+ plugins::ObjectPoseClientPlugin&
+ ObjectPoseClientPluginUser::getObjectPoseClientPlugin()
{
return *plugin;
}
- const plugins::ObjectPoseClientPlugin& ObjectPoseClientPluginUser::getObjectPoseClientPlugin() const
+
+ const plugins::ObjectPoseClientPlugin&
+ ObjectPoseClientPluginUser::getObjectPoseClientPlugin() const
{
return *plugin;
}
- const ObjectFinder& ObjectPoseClientPluginUser::getObjectFinder() const
+ const ObjectFinder&
+ ObjectPoseClientPluginUser::getObjectFinder() const
{
return plugin->getObjectFinder();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
index 7f27d9768474ea7ff916d3b65fb03c7fff97aa56..481753b8ba7d74ce262174551c531855cf6aeb7f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
@@ -7,30 +7,30 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
-
namespace armarx::plugins
{
class ObjectPoseClientPlugin : public ComponentPlugin
{
public:
-
using ComponentPlugin::ComponentPlugin;
objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
objpose::ObjectPoseClient createClient();
- template<class...Ts>
- std::optional<ObjectInfo> findObject(Ts&& ...ts) const
+ template <class... Ts>
+ std::optional<ObjectInfo>
+ findObject(Ts&&... ts) const
{
return _finder.findObject(std::forward<Ts>(ts)...);
}
- template<class...Ts>
+ template <class... Ts>
VirtualRobot::ManipulationObjectPtr
- findAndLoadObject(Ts&& ...ts) const
+ findAndLoadObject(Ts&&... ts) const
{
return findAndLoadObject(findObject(std::forward<Ts>(ts)...));
}
+
VirtualRobot::ManipulationObjectPtr
findAndLoadObject(const std::optional<ObjectInfo>& ts) const
{
@@ -42,7 +42,6 @@ namespace armarx::plugins
private:
-
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
void preOnInitComponent() override;
@@ -52,10 +51,8 @@ namespace armarx::plugins
ObjectFinder _finder;
objpose::ObjectPoseStorageInterfacePrx _objectPoseStorage;
-
};
-}
-
+} // namespace armarx::plugins
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -64,11 +61,9 @@ namespace armarx
/**
* @brief Provides an `objpose::ObjectPoseTopicPrx objectPoseTopic` as member variable.
*/
- class ObjectPoseClientPluginUser :
- virtual public ManagedIceObject
+ class ObjectPoseClientPluginUser : virtual public ManagedIceObject
{
public:
-
/// Allow usage like: ObjectPoseClient::getObjects()
using ObjectPoseClient = ObjectPoseClientPluginUser;
@@ -88,8 +83,6 @@ namespace armarx
private:
-
armarx::plugins::ObjectPoseClientPlugin* plugin = nullptr;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
index 6e7fd2ebb28c330c81ec378acc3eb218329e4fb5..94302e6cac57c7d654d4b7d1c2ccfa9877ec2c9c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseProviderPlugin.h
@@ -21,7 +21,6 @@ namespace armarx::plugins
private:
static constexpr const char* PROPERTY_NAME = "ObjectMemoryName";
-
};
} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
index c67e2f98dbdaf9c555890be875b4624075428fa4..f2ddada585e3dc40578a276b6e876a4c42e6b7f8 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.cpp
@@ -1,8 +1,8 @@
#include "RequestedObjects.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
@@ -12,18 +12,23 @@ namespace armarx::objpose
{
}
- void RequestedObjects::requestObjects(const std::vector<armarx::data::ObjectID>& objectIDs, long relativeTimeOutMS)
+ void
+ RequestedObjects::requestObjects(const std::vector<armarx::data::ObjectID>& objectIDs,
+ long relativeTimeOutMS)
{
requestObjects(::armarx::fromIce(objectIDs), relativeTimeOutMS);
}
- void RequestedObjects::requestObjects(const std::vector<armarx::ObjectID>& objectIDs, long relativeTimeOutMS)
+ void
+ RequestedObjects::requestObjects(const std::vector<armarx::ObjectID>& objectIDs,
+ long relativeTimeOutMS)
{
requestObjects(objectIDs, IceUtil::Time::milliSeconds(relativeTimeOutMS));
}
- void RequestedObjects::requestObjects(
- const std::vector<armarx::ObjectID>& objectIDs, IceUtil::Time relativeTimeout)
+ void
+ RequestedObjects::requestObjects(const std::vector<armarx::ObjectID>& objectIDs,
+ IceUtil::Time relativeTimeout)
{
ARMARX_INFO << VAROUT(relativeTimeout.toMilliSeconds());
@@ -37,7 +42,8 @@ namespace armarx::objpose
}
else
{
- ARMARX_INFO << "Localization request for " << relativeTimeout << " for object ids :" << objectIDs;
+ ARMARX_INFO << "Localization request for " << relativeTimeout
+ << " for object ids :" << objectIDs;
IceUtil::Time absoluteTimeout = TimeUtil::GetTime() + relativeTimeout;
Request req;
@@ -46,18 +52,20 @@ namespace armarx::objpose
}
}
- RequestedObjects::Update RequestedObjects::updateRequestedObjects()
+ RequestedObjects::Update
+ RequestedObjects::updateRequestedObjects()
{
return updateRequestedObjects(TimeUtil::GetTime());
}
- RequestedObjects::Update RequestedObjects::updateRequestedObjects(IceUtil::Time now)
+ RequestedObjects::Update
+ RequestedObjects::updateRequestedObjects(IceUtil::Time now)
{
// Remove requests with timeout.
- if(not currentRequests.empty())
+ if (not currentRequests.empty())
{
- ARMARX_INFO << currentRequests.begin()->first - now;
+ ARMARX_INFO << currentRequests.begin()->first - now;
}
while (not currentRequests.empty() and currentRequests.begin()->first <= now)
@@ -84,19 +92,23 @@ namespace armarx::objpose
}
Update update;
- update.current = { current.begin(), current.end() };
+ update.current = {current.begin(), current.end()};
// added = current - last
- std::set_difference(update.current.begin(), update.current.end(),
- lastCurrent.begin(), lastCurrent.end(),
+ std::set_difference(update.current.begin(),
+ update.current.end(),
+ lastCurrent.begin(),
+ lastCurrent.end(),
std::inserter(update.added, update.added.begin()));
// removed = last - current
- std::set_difference(lastCurrent.begin(), lastCurrent.end(),
- update.current.begin(), update.current.end(),
+ std::set_difference(lastCurrent.begin(),
+ lastCurrent.end(),
+ update.current.begin(),
+ update.current.end(),
std::inserter(update.removed, update.removed.begin()));
this->lastCurrent = update.current;
return update;
}
-}
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h
index eebaf7af21e54ee9995d68940390c4cae48176f8..eddff95d0f8a5a3b8ed33cee83669760a7c4b147 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/RequestedObjects.h
@@ -1,15 +1,12 @@
#pragma once
-#include <set>
#include <deque>
+#include <set>
#include <vector>
-
#include <RobotAPI/interface/objectpose/object_pose_types.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-
-
namespace armarx::objpose
{
/**
@@ -24,6 +21,7 @@ namespace armarx::objpose
std::vector<armarx::ObjectID> objectIDs;
bool infinite = false;
};
+
struct Update
{
std::vector<armarx::ObjectID> added;
@@ -33,12 +31,13 @@ namespace armarx::objpose
public:
-
RequestedObjects();
- void requestObjects(const std::vector<armarx::data::ObjectID>& objectIDs, long relativeTimeOutMS);
+ void requestObjects(const std::vector<armarx::data::ObjectID>& objectIDs,
+ long relativeTimeOutMS);
void requestObjects(const std::vector<armarx::ObjectID>& objectIDs, long relativeTimeOutMS);
- void requestObjects(const std::vector<armarx::ObjectID>& objectIDs, IceUtil::Time relativeTimeout);
+ void requestObjects(const std::vector<armarx::ObjectID>& objectIDs,
+ IceUtil::Time relativeTimeout);
Update updateRequestedObjects();
@@ -46,7 +45,6 @@ namespace armarx::objpose
public:
-
/// Map from (absolute timeout) to (request)
std::map<IceUtil::Time, std::vector<Request>> currentRequests;
std::vector<armarx::ObjectID> infiniteRequests;
@@ -54,6 +52,5 @@ namespace armarx::objpose
private:
std::vector<armarx::ObjectID> lastCurrent;
-
};
-}
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
index e74da09bfa4a0c26adf4f1579ead89947f282fba..098a24f71881730a15ed19224920764d45750623 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
@@ -22,21 +22,21 @@
#include "predictions.h"
-#include <manif/SE3.h>
-
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/regression/linear.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <manif/SE3.h>
+
namespace armarx::objpose
{
objpose::ObjectPosePredictionResult
predictObjectPoseLinear(const std::map<DateTime, ObjectPose>& poses,
- const DateTime& time,
- const ObjectPose& latestPose)
+ const DateTime& time,
+ const ObjectPose& latestPose)
{
objpose::ObjectPosePredictionResult result;
result.success = false;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.h b/source/RobotAPI/libraries/ArmarXObjects/predictions.h
index 03b3f7de2eb2267ba43c356c44988e374792c591..608bc8dffa8aed14626e0fe4c26530dcac78dfb9 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/predictions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.h
@@ -41,9 +41,8 @@ namespace armarx::objpose
* @return the result of the prediction
*/
objpose::ObjectPosePredictionResult
- predictObjectPoseLinear(
- const std::map<DateTime, ObjectPose>& poses,
- const DateTime& time,
- const ObjectPose& latestPose);
+ predictObjectPoseLinear(const std::map<DateTime, ObjectPose>& poses,
+ const DateTime& time,
+ const ObjectPose& latestPose);
} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
index 4a775da39159a8e0a8435f88679368b3cf39a0dc..43f32e28a805561ef4f1d461e49f82f849f72844 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
@@ -26,11 +26,12 @@
#include "../ArmarXObjects.h"
+#include <RobotAPI/Test.h>
+
#include <iostream>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <RobotAPI/Test.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp
index 7063ef69b2490bdea0ba91d76a2a19da6c96e057..d079c0c3c1f3db7bdfcfd8ebe09e51e7d830975e 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ObjectIDTest.cpp
@@ -24,60 +24,42 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
-#include "../ArmarXObjects.h"
-
#include "../ObjectID.h"
+#include <RobotAPI/Test.h>
+
#include <iostream>
+#include "../ArmarXObjects.h"
namespace armarx
{
struct Fixture
{
- std::vector<armarx::ObjectID> dcsStringConstructor
- {
- { "Data/Class/0" },
- { "Data/Class/1" },
- { "Data/Class/2" }
- };
- std::vector<armarx::ObjectID> dcsFromString
- {
- ObjectID::FromString("Data/Class/0"),
- ObjectID::FromString("Data/Class/1"),
- ObjectID::FromString("Data/Class/2")
- };
- std::vector<std::vector<armarx::ObjectID>*> dcs {
- &dcsStringConstructor,
- &dcsFromString
- };
-
- armarx::ObjectID dc0StringConstructor { "Data/Class/0" };
+ std::vector<armarx::ObjectID> dcsStringConstructor{{"Data/Class/0"},
+ {"Data/Class/1"},
+ {"Data/Class/2"}};
+ std::vector<armarx::ObjectID> dcsFromString{ObjectID::FromString("Data/Class/0"),
+ ObjectID::FromString("Data/Class/1"),
+ ObjectID::FromString("Data/Class/2")};
+ std::vector<std::vector<armarx::ObjectID>*> dcs{&dcsStringConstructor, &dcsFromString};
+
+ armarx::ObjectID dc0StringConstructor{"Data/Class/0"};
armarx::ObjectID dc0FromString = ObjectID::FromString("Data/Class/0");
- std::vector<armarx::ObjectID*> dc0s {
+ std::vector<armarx::ObjectID*> dc0s{
&dc0StringConstructor,
&dc0FromString,
};
- std::vector<armarx::ObjectID> otsStringConstructor
- {
- { "Other/Type/0" },
- { "Other/Type/1" },
- { "Other/Type/2" }
- };
- std::vector<armarx::ObjectID> otsFromString
- {
- ObjectID::FromString("Other/Type/0"),
- ObjectID::FromString("Other/Type/1"),
- ObjectID::FromString("Other/Type/2")
- };
- std::vector<std::vector<armarx::ObjectID>*> ots {
- &otsStringConstructor,
- &otsFromString
- };
+ std::vector<armarx::ObjectID> otsStringConstructor{{"Other/Type/0"},
+ {"Other/Type/1"},
+ {"Other/Type/2"}};
+ std::vector<armarx::ObjectID> otsFromString{ObjectID::FromString("Other/Type/0"),
+ ObjectID::FromString("Other/Type/1"),
+ ObjectID::FromString("Other/Type/2")};
+ std::vector<std::vector<armarx::ObjectID>*> ots{&otsStringConstructor, &otsFromString};
};
-}
+} // namespace armarx
BOOST_FIXTURE_TEST_SUITE(ObjectIDTests, armarx::Fixture)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.cpp b/source/RobotAPI/libraries/ArmarXObjects/util.cpp
index 3dd90c141d81b77fcce1e0b91f8a5963d84a2726..c178c759b2cd0f65c7312e76fe2c0d756320fc68 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/util.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/util.cpp
@@ -48,7 +48,6 @@ namespace armarx::objpose
return objects;
}
-
VirtualRobot::ManipulationObjectPtr
asManipulationObject(const objpose::ObjectPose& objectPose)
{
@@ -65,7 +64,6 @@ namespace armarx::objpose
return nullptr;
}
-
VirtualRobot::SceneObjectSetPtr
asSceneObjects(const objpose::ObjectPoseSeq& objectPoses)
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/util.h b/source/RobotAPI/libraries/ArmarXObjects/util.h
index bd8ebf0c8397ae1a26b899a8d48c1f9a303377f9..da9535020fd06bbfa1b5402e590639b5eab1f7eb 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/util.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/util.h
@@ -32,5 +32,5 @@ namespace armarx::objpose
VirtualRobot::ManipulationObjectPtr asManipulationObject(const objpose::ObjectPose& objectPose);
VirtualRobot::SceneObjectSetPtr asSceneObjects(const objpose::ObjectPoseSeq& objectPoses);
-
+
} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.cpp b/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.cpp
index 504409aee28f308fddce88de06155340af1b527e..128d1613cb198f46aa3c53509060246d27e19df2 100644
--- a/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.cpp
+++ b/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.cpp
@@ -21,29 +21,50 @@
*/
#include "DebugDrawerConfigWidget.h"
+
#include <QInputDialog>
namespace armarx
{
- DebugDrawerConfigWidget::DebugDrawerConfigWidget(const DebugDrawerComponentPtr& debugDrawer, QWidget* parent)
- : QWidget(parent),
- debugDrawer(debugDrawer)
+ DebugDrawerConfigWidget::DebugDrawerConfigWidget(const DebugDrawerComponentPtr& debugDrawer,
+ QWidget* parent) :
+ QWidget(parent), debugDrawer(debugDrawer)
{
ui.setupUi(this);
refresh();
- connect(ui.pushButtonRefresh, &QPushButton::released, this, &DebugDrawerConfigWidget::refresh);
- connect(ui.listWidgetLayerVisibility, &QListWidget::itemChanged, this, &DebugDrawerConfigWidget::onVisibilityChanged);
- connect(ui.checkBoxDefaultLayerVisibility, &QCheckBox::stateChanged, this, &DebugDrawerConfigWidget::onDefaultLayerVisibilityChanged);
- connect(ui.listWidgetDefaultLayerVisibility, &QListWidget::itemChanged, this, &DebugDrawerConfigWidget::onDefaultLayerVisibilityPerLayerChanged);
- connect(ui.pushButtonDefaultLayerVisibilityAdd, &QPushButton::released, this, &DebugDrawerConfigWidget::onAddDefaultLayerVisibility);
- connect(ui.pushButtonDefaultLayerVisibilityRemove, &QPushButton::released, this, &DebugDrawerConfigWidget::onRemoveDefaultLayerVisibility);
+ connect(
+ ui.pushButtonRefresh, &QPushButton::released, this, &DebugDrawerConfigWidget::refresh);
+ connect(ui.listWidgetLayerVisibility,
+ &QListWidget::itemChanged,
+ this,
+ &DebugDrawerConfigWidget::onVisibilityChanged);
+ connect(ui.checkBoxDefaultLayerVisibility,
+ &QCheckBox::stateChanged,
+ this,
+ &DebugDrawerConfigWidget::onDefaultLayerVisibilityChanged);
+ connect(ui.listWidgetDefaultLayerVisibility,
+ &QListWidget::itemChanged,
+ this,
+ &DebugDrawerConfigWidget::onDefaultLayerVisibilityPerLayerChanged);
+ connect(ui.pushButtonDefaultLayerVisibilityAdd,
+ &QPushButton::released,
+ this,
+ &DebugDrawerConfigWidget::onAddDefaultLayerVisibility);
+ connect(ui.pushButtonDefaultLayerVisibilityRemove,
+ &QPushButton::released,
+ this,
+ &DebugDrawerConfigWidget::onRemoveDefaultLayerVisibility);
}
- void DebugDrawerConfigWidget::loadSettings(QSettings* settings)
+ void
+ DebugDrawerConfigWidget::loadSettings(QSettings* settings)
{
- ui.checkBoxDefaultLayerVisibility->setCheckState(settings->value(QString::fromStdString("DefaultLayerVisibility")).toBool() ? Qt::Checked : Qt::Unchecked);
+ ui.checkBoxDefaultLayerVisibility->setCheckState(
+ settings->value(QString::fromStdString("DefaultLayerVisibility")).toBool()
+ ? Qt::Checked
+ : Qt::Unchecked);
settings->beginGroup("layer");
for (const auto& key : settings->allKeys())
{
@@ -54,9 +75,11 @@ namespace armarx
settings->endGroup();
}
- void DebugDrawerConfigWidget::saveSettings(QSettings* settings)
+ void
+ DebugDrawerConfigWidget::saveSettings(QSettings* settings)
{
- settings->setValue(QString::fromStdString("DefaultLayerVisibility"), ui.checkBoxDefaultLayerVisibility->checkState() == Qt::Checked);
+ settings->setValue(QString::fromStdString("DefaultLayerVisibility"),
+ ui.checkBoxDefaultLayerVisibility->checkState() == Qt::Checked);
settings->beginGroup("layer");
for (const auto& layerInfo : debugDrawer->getAllDefaultLayerVisibilities())
{
@@ -65,7 +88,8 @@ namespace armarx
settings->endGroup();
}
- void DebugDrawerConfigWidget::setDebugDrawer(const DebugDrawerComponentPtr& debugDrawer)
+ void
+ DebugDrawerConfigWidget::setDebugDrawer(const DebugDrawerComponentPtr& debugDrawer)
{
this->debugDrawer = debugDrawer;
onDefaultLayerVisibilityChanged(ui.checkBoxDefaultLayerVisibility->checkState());
@@ -76,12 +100,14 @@ namespace armarx
refresh();
}
- DebugDrawerComponentPtr DebugDrawerConfigWidget::getDebugDrawer() const
+ DebugDrawerComponentPtr
+ DebugDrawerConfigWidget::getDebugDrawer() const
{
return debugDrawer;
}
- void DebugDrawerConfigWidget::refresh()
+ void
+ DebugDrawerConfigWidget::refresh()
{
if (!debugDrawer)
{
@@ -90,21 +116,25 @@ namespace armarx
ui.listWidgetLayerVisibility->clear();
for (const auto& layerInfo : debugDrawer->layerInformation())
{
- QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(layerInfo.layerName), ui.listWidgetLayerVisibility);
+ QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(layerInfo.layerName),
+ ui.listWidgetLayerVisibility);
item->setCheckState(layerInfo.visible ? Qt::Checked : Qt::Unchecked);
ui.listWidgetLayerVisibility->addItem(item);
}
- ui.checkBoxDefaultLayerVisibility->setCheckState(debugDrawer->getDefaultLayerVisibility() ? Qt::Checked : Qt::Unchecked);
+ ui.checkBoxDefaultLayerVisibility->setCheckState(
+ debugDrawer->getDefaultLayerVisibility() ? Qt::Checked : Qt::Unchecked);
ui.listWidgetDefaultLayerVisibility->clear();
for (const auto& layerInfo : debugDrawer->getAllDefaultLayerVisibilities())
{
- QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(layerInfo.first), ui.listWidgetDefaultLayerVisibility);
+ QListWidgetItem* item = new QListWidgetItem(QString::fromStdString(layerInfo.first),
+ ui.listWidgetDefaultLayerVisibility);
item->setCheckState(layerInfo.second ? Qt::Checked : Qt::Unchecked);
ui.listWidgetDefaultLayerVisibility->addItem(item);
}
}
- void DebugDrawerConfigWidget::onVisibilityChanged(QListWidgetItem* item)
+ void
+ DebugDrawerConfigWidget::onVisibilityChanged(QListWidgetItem* item)
{
if (!debugDrawer)
{
@@ -113,7 +143,8 @@ namespace armarx
debugDrawer->enableLayerVisu(item->text().toStdString(), item->checkState() == Qt::Checked);
}
- void DebugDrawerConfigWidget::onDefaultLayerVisibilityChanged(int checkState)
+ void
+ DebugDrawerConfigWidget::onDefaultLayerVisibilityChanged(int checkState)
{
if (!debugDrawer)
{
@@ -122,30 +153,41 @@ namespace armarx
debugDrawer->setDefaultLayerVisibility(checkState != Qt::Unchecked);
}
- void DebugDrawerConfigWidget::onDefaultLayerVisibilityPerLayerChanged(QListWidgetItem* item)
+ void
+ DebugDrawerConfigWidget::onDefaultLayerVisibilityPerLayerChanged(QListWidgetItem* item)
{
if (!debugDrawer)
{
return;
}
- debugDrawer->setDefaultLayerVisibility(item->text().toStdString(), item->checkState() == Qt::Checked);
+ debugDrawer->setDefaultLayerVisibility(item->text().toStdString(),
+ item->checkState() == Qt::Checked);
}
- void DebugDrawerConfigWidget::onAddDefaultLayerVisibility()
+ void
+ DebugDrawerConfigWidget::onAddDefaultLayerVisibility()
{
bool ok;
- QString text = QInputDialog::getText(this, "Layer name",
- "Layer name:", QLineEdit::Normal,
- ui.listWidgetLayerVisibility->selectedItems().empty() ? "" : ui.listWidgetLayerVisibility->selectedItems().front()->text(), &ok);
+ QString text = QInputDialog::getText(
+ this,
+ "Layer name",
+ "Layer name:",
+ QLineEdit::Normal,
+ ui.listWidgetLayerVisibility->selectedItems().empty()
+ ? ""
+ : ui.listWidgetLayerVisibility->selectedItems().front()->text(),
+ &ok);
if (ok && !text.isEmpty())
{
QListWidgetItem* item = new QListWidgetItem(text, ui.listWidgetDefaultLayerVisibility);
- item->setCheckState(debugDrawer->getDefaultLayerVisibility() ? Qt::Checked : Qt::Unchecked);
+ item->setCheckState(debugDrawer->getDefaultLayerVisibility() ? Qt::Checked
+ : Qt::Unchecked);
ui.listWidgetDefaultLayerVisibility->addItem(item);
}
}
- void DebugDrawerConfigWidget::onRemoveDefaultLayerVisibility()
+ void
+ DebugDrawerConfigWidget::onRemoveDefaultLayerVisibility()
{
for (const auto& selected : ui.listWidgetDefaultLayerVisibility->selectedItems())
{
@@ -153,8 +195,9 @@ namespace armarx
{
debugDrawer->removeDefaultLayerVisibility(selected->text().toStdString());
}
- ui.listWidgetDefaultLayerVisibility->takeItem(ui.listWidgetDefaultLayerVisibility->row(selected));
+ ui.listWidgetDefaultLayerVisibility->takeItem(
+ ui.listWidgetDefaultLayerVisibility->row(selected));
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.h b/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.h
index 6680464000af1a9706c765955979dc18ea8d297c..d67d67b9a149beb8f0858f3a6c399d791215f6aa 100644
--- a/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.h
+++ b/source/RobotAPI/libraries/DebugDrawerConfigWidget/DebugDrawerConfigWidget.h
@@ -22,13 +22,11 @@
#pragma once
-#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
-
-#include <RobotAPI/libraries/DebugDrawerConfigWidget/ui_DebugDrawerConfigWidget.h>
-
-#include <QWidget>
#include <QSettings>
+#include <QWidget>
+#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
+#include <RobotAPI/libraries/DebugDrawerConfigWidget/ui_DebugDrawerConfigWidget.h>
namespace armarx
{
@@ -46,7 +44,8 @@ namespace armarx
class DebugDrawerConfigWidget : public QWidget
{
public:
- DebugDrawerConfigWidget(const DebugDrawerComponentPtr& debugDrawer, QWidget* parent = nullptr);
+ DebugDrawerConfigWidget(const DebugDrawerComponentPtr& debugDrawer,
+ QWidget* parent = nullptr);
void loadSettings(QSettings* settings);
void saveSettings(QSettings* settings);
@@ -70,4 +69,4 @@ namespace armarx
Ui::DebugDrawerConfigWidget ui;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/DebugDrawerConfigWidget/test/DebugDrawerConfigWidgetTest.cpp b/source/RobotAPI/libraries/DebugDrawerConfigWidget/test/DebugDrawerConfigWidgetTest.cpp
index e10bd7c4b3ba362cb6ef5dab600b0660351f54d8..086936eae5c2cee4dfcee469869fc50680c657bb 100644
--- a/source/RobotAPI/libraries/DebugDrawerConfigWidget/test/DebugDrawerConfigWidgetTest.cpp
+++ b/source/RobotAPI/libraries/DebugDrawerConfigWidget/test/DebugDrawerConfigWidgetTest.cpp
@@ -24,9 +24,10 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
#include "../DebugDrawerConfigWidget.h"
+#include <RobotAPI/Test.h>
+
#include <iostream>
BOOST_AUTO_TEST_CASE(testExample)
diff --git a/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.cpp b/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.cpp
index af5dec6f77b98de2ea386a39ffa4f5335f118e04..f018ac2f3f530e896227b5c2ef16ef03a9314a9c 100644
--- a/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.cpp
@@ -22,58 +22,71 @@
*/
#include "BimanualGraspCandidateHelper.h"
-#include <VirtualRobot/math/Helpers.h>
+
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
using namespace armarx;
-
-BimanualGraspCandidateHelper::BimanualGraspCandidateHelper(const grasping::BimanualGraspCandidatePtr& candidate, VirtualRobot::RobotPtr robot)
- : candidate(candidate), robot(robot)
+BimanualGraspCandidateHelper::BimanualGraspCandidateHelper(
+ const grasping::BimanualGraspCandidatePtr& candidate,
+ VirtualRobot::RobotPtr robot) :
+ candidate(candidate), robot(robot)
{
ARMARX_CHECK_NOT_NULL(candidate);
ARMARX_CHECK_NOT_NULL(robot);
}
-Eigen::Matrix4f BimanualGraspCandidateHelper::Side::getGraspPoseInRobotRoot() const
+Eigen::Matrix4f
+BimanualGraspCandidateHelper::Side::getGraspPoseInRobotRoot() const
{
const Eigen::Matrix4f curRobotPose = helper->robot->getGlobalPose();
const Eigen::Matrix4f graspPose = curRobotPose.inverse() * getGraspPoseInGlobal();
return graspPose;
}
-Eigen::Matrix4f BimanualGraspCandidateHelper::Side::getPrePoseInRobotRoot(float approachDistance) const
+Eigen::Matrix4f
+BimanualGraspCandidateHelper::Side::getPrePoseInRobotRoot(float approachDistance) const
{
- return math::Helpers::TranslatePose(getGraspPoseInRobotRoot(), getApproachVector() * approachDistance);
+ return math::Helpers::TranslatePose(getGraspPoseInRobotRoot(),
+ getApproachVector() * approachDistance);
}
-Eigen::Matrix3f BimanualGraspCandidateHelper::Side::getGraspOrientationInRobotRoot() const
+Eigen::Matrix3f
+BimanualGraspCandidateHelper::Side::getGraspOrientationInRobotRoot() const
{
return getGraspPoseInRobotRoot().topLeftCorner<3, 3>();
}
-Eigen::Vector3f BimanualGraspCandidateHelper::Side::getGraspPositionInRobotRoot() const
+Eigen::Vector3f
+BimanualGraspCandidateHelper::Side::getGraspPositionInRobotRoot() const
{
return math::Helpers::GetPosition(getGraspPoseInRobotRoot());
}
-Eigen::Matrix4f BimanualGraspCandidateHelper::Side::getGraspPoseInGlobal() const
+Eigen::Matrix4f
+BimanualGraspCandidateHelper::Side::getGraspPoseInGlobal() const
{
const Eigen::Matrix4f oldRobotPose = helper->defrost(helper->candidate->robotPose);
return oldRobotPose * graspPose;
}
-Eigen::Matrix3f BimanualGraspCandidateHelper::Side::getGraspOrientationInGlobal() const
+
+Eigen::Matrix3f
+BimanualGraspCandidateHelper::Side::getGraspOrientationInGlobal() const
{
return getGraspPoseInGlobal().topLeftCorner<3, 3>();
}
-Eigen::Vector3f BimanualGraspCandidateHelper::Side::getGraspPositionInGlobal() const
+
+Eigen::Vector3f
+BimanualGraspCandidateHelper::Side::getGraspPositionInGlobal() const
{
return math::Helpers::GetPosition(getGraspPoseInGlobal());
}
-Eigen::Vector3f BimanualGraspCandidateHelper::Side::getApproachVector() const
+Eigen::Vector3f
+BimanualGraspCandidateHelper::Side::getApproachVector() const
{
return approachVector;
}
diff --git a/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h b/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h
index 9a861d414a7d35a13e500cc1365a99d8096a40ca..8d71cd03119ae26467998c004550885eea62b8a4 100644
--- a/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h
+++ b/source/RobotAPI/libraries/GraspingUtility/BimanualGraspCandidateHelper.h
@@ -24,13 +24,12 @@
#pragma once
-#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <memory>
#include <VirtualRobot/VirtualRobot.h>
-#include <memory>
-
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx
{
@@ -39,7 +38,8 @@ namespace armarx
class BimanualGraspCandidateHelper
{
public:
- BimanualGraspCandidateHelper(const grasping::BimanualGraspCandidatePtr& candidate, VirtualRobot::RobotPtr robot);
+ BimanualGraspCandidateHelper(const grasping::BimanualGraspCandidatePtr& candidate,
+ VirtualRobot::RobotPtr robot);
BimanualGraspCandidateHelper(const BimanualGraspCandidateHelper&) = default;
BimanualGraspCandidateHelper(BimanualGraspCandidateHelper&&) = default;
BimanualGraspCandidateHelper& operator=(const BimanualGraspCandidateHelper&) = default;
@@ -66,35 +66,42 @@ namespace armarx
Eigen::Vector3f inwardsVector;
};
- Side left() const
+ Side
+ left() const
{
Side s;
- s.helper = this;
- s.graspPose = defrost(candidate->graspPoseLeft);
+ s.helper = this;
+ s.graspPose = defrost(candidate->graspPoseLeft);
s.approachVector = defrost(candidate->approachVectorLeft);
- s.inwardsVector = defrost(candidate->inwardsVectorLeft);
+ s.inwardsVector = defrost(candidate->inwardsVectorLeft);
return s;
}
- Side right() const
+
+ Side
+ right() const
{
Side s;
- s.helper = this;
- s.graspPose = defrost(candidate->graspPoseRight);
+ s.helper = this;
+ s.graspPose = defrost(candidate->graspPoseRight);
s.approachVector = defrost(candidate->approachVectorRight);
- s.inwardsVector = defrost(candidate->inwardsVectorRight);
+ s.inwardsVector = defrost(candidate->inwardsVectorRight);
return s;
}
- const grasping::BimanualGraspCandidatePtr getGraspCandidate() const
+ const grasping::BimanualGraspCandidatePtr
+ getGraspCandidate() const
{
return candidate;
}
- Eigen::Vector3f defrost(const Vector3BasePtr& base) const
+ Eigen::Vector3f
+ defrost(const Vector3BasePtr& base) const
{
return Vector3Ptr::dynamicCast(base)->toEigen();
}
- Eigen::Matrix4f defrost(const PoseBasePtr& base) const
+
+ Eigen::Matrix4f
+ defrost(const PoseBasePtr& base) const
{
return PosePtr::dynamicCast(base)->toEigen();
}
@@ -103,4 +110,4 @@ namespace armarx
grasping::BimanualGraspCandidatePtr candidate;
VirtualRobot::RobotPtr robot;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
index b8eafe669d406b838852dd10a9ab7a65aca10aec..f1d82e92a45c6baaa31949a8d9d3c2dd09669f4f 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
@@ -22,22 +22,24 @@
*/
#include "GraspCandidateHelper.h"
-#include <VirtualRobot/math/Helpers.h>
+
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
using namespace armarx;
-
-GraspCandidateHelper::GraspCandidateHelper(const grasping::GraspCandidatePtr& candidate, VirtualRobot::RobotPtr robot)
- : candidate(candidate), robot(robot)
+GraspCandidateHelper::GraspCandidateHelper(const grasping::GraspCandidatePtr& candidate,
+ VirtualRobot::RobotPtr robot) :
+ candidate(candidate), robot(robot)
{
ARMARX_CHECK_NOT_NULL(candidate);
ARMARX_CHECK_NOT_NULL(robot);
}
-Eigen::Matrix4f GraspCandidateHelper::getGraspPoseInRobotRoot() const
+Eigen::Matrix4f
+GraspCandidateHelper::getGraspPoseInRobotRoot() const
{
/* As the (current) robot might have moved after the grasp was generated,
* we must first transform the grasp to global with its original robot pose (stored in the grasp),
@@ -47,54 +49,66 @@ Eigen::Matrix4f GraspCandidateHelper::getGraspPoseInRobotRoot() const
return graspPose;
}
-Eigen::Matrix4f GraspCandidateHelper::getPrePoseInRobotRoot(float approachDistance) const
+Eigen::Matrix4f
+GraspCandidateHelper::getPrePoseInRobotRoot(float approachDistance) const
{
- return math::Helpers::TranslatePose(getGraspPoseInRobotRoot(), getApproachVector() * approachDistance);
+ return math::Helpers::TranslatePose(getGraspPoseInRobotRoot(),
+ getApproachVector() * approachDistance);
}
-Eigen::Matrix3f GraspCandidateHelper::getGraspOrientationInRobotRoot() const
+Eigen::Matrix3f
+GraspCandidateHelper::getGraspOrientationInRobotRoot() const
{
return getGraspPoseInRobotRoot().topLeftCorner<3, 3>();
}
-Eigen::Vector3f GraspCandidateHelper::getGraspPositionInRobotRoot() const
+Eigen::Vector3f
+GraspCandidateHelper::getGraspPositionInRobotRoot() const
{
return math::Helpers::GetPosition(getGraspPoseInRobotRoot());
}
-Eigen::Matrix4f GraspCandidateHelper::getGraspPoseInGlobal() const
+Eigen::Matrix4f
+GraspCandidateHelper::getGraspPoseInGlobal() const
{
// We must use the original robot pose from when the grasp was generated (stored in the grasp candidate).
const Eigen::Matrix4f originalGraspPose = fromIce(candidate->graspPose);
const Eigen::Matrix4f originalRobotPose = fromIce(candidate->robotPose);
return originalRobotPose * originalGraspPose;
}
-Eigen::Matrix3f GraspCandidateHelper::getGraspOrientationInGlobal() const
+
+Eigen::Matrix3f
+GraspCandidateHelper::getGraspOrientationInGlobal() const
{
return getGraspPoseInGlobal().topLeftCorner<3, 3>();
}
-Eigen::Vector3f GraspCandidateHelper::getGraspPositionInGlobal() const
+
+Eigen::Vector3f
+GraspCandidateHelper::getGraspPositionInGlobal() const
{
return math::Helpers::GetPosition(getGraspPoseInGlobal());
}
-Eigen::Vector3f GraspCandidateHelper::getApproachVector() const
+Eigen::Vector3f
+GraspCandidateHelper::getApproachVector() const
{
return fromIce(candidate->approachVector);
}
-bool GraspCandidateHelper::isTopGrasp()
+bool
+GraspCandidateHelper::isTopGrasp()
{
return candidate->executionHints->approach == grasping::ApproachType::TopApproach;
}
-bool GraspCandidateHelper::isSideGrasp()
+bool
+GraspCandidateHelper::isSideGrasp()
{
return candidate->executionHints->approach == grasping::ApproachType::SideApproach;
}
-
-void GraspCandidateHelper::setGraspCandidate(const grasping::GraspCandidatePtr& p)
+void
+GraspCandidateHelper::setGraspCandidate(const grasping::GraspCandidatePtr& p)
{
ARMARX_CHECK_NOT_NULL(p);
candidate = p;
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
index d47e00f3f922ec170d28ceb484b5a1539658e0f6..8e4d204e7e5d00fc83001750a216a15dd073b5d5 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
@@ -23,13 +23,12 @@
#pragma once
-#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <memory>
#include <VirtualRobot/VirtualRobot.h>
-#include <memory>
-
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx
{
@@ -38,7 +37,8 @@ namespace armarx
class GraspCandidateHelper
{
public:
- GraspCandidateHelper(const grasping::GraspCandidatePtr& candidate, VirtualRobot::RobotPtr robot);
+ GraspCandidateHelper(const grasping::GraspCandidatePtr& candidate,
+ VirtualRobot::RobotPtr robot);
GraspCandidateHelper() = default;
GraspCandidateHelper(GraspCandidateHelper&&) = default;
GraspCandidateHelper(const GraspCandidateHelper&) = default;
@@ -58,22 +58,28 @@ namespace armarx
bool isTopGrasp();
bool isSideGrasp();
- const grasping::GraspCandidatePtr getGraspCandidate() const
+ const grasping::GraspCandidatePtr
+ getGraspCandidate() const
{
return candidate;
}
+
void setGraspCandidate(const grasping::GraspCandidatePtr& p);
- Eigen::Vector3f defrost(const Vector3BasePtr& base) const
+ Eigen::Vector3f
+ defrost(const Vector3BasePtr& base) const
{
return Vector3Ptr::dynamicCast(base)->toEigen();
}
- Eigen::Matrix4f defrost(const PoseBasePtr& base) const
+
+ Eigen::Matrix4f
+ defrost(const PoseBasePtr& base) const
{
return PosePtr::dynamicCast(base)->toEigen();
}
- const VirtualRobot::RobotPtr& getRobot() const
+ const VirtualRobot::RobotPtr&
+ getRobot() const
{
return robot;
}
@@ -82,4 +88,4 @@ namespace armarx
grasping::GraspCandidatePtr candidate;
VirtualRobot::RobotPtr robot;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.cpp
index 99a80a53febe01e9f91cdedee69e4bbc91a35555..b8748f609efcf27615c23838774a2a3cfd0653c1 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.cpp
@@ -99,5 +99,3 @@
// return info;
// }
//}
-
-
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.h
index 7b2095c08364c4d0a5cd870f908ff6e45d38e6b9..b086867028c89b567a8bcc3545b7ee9dca4444b9 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateProvider.h
@@ -61,4 +61,4 @@ namespace armarx
// armarx::grasping::GraspCandidatesTopicInterfacePrx _gc_topic;
// armarx::RequestableServiceListenerInterfacePrx _service_request_topic;
// };
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
index 1066f1c6fb62621d0265b35ad3c37f8f16698ca5..bf3d87d7f34f13666878b1ba3ce3b320574b7dec 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
@@ -1,8 +1,7 @@
#include "GraspCandidateVisu.h"
-#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/components/ArViz/Client/elements/RobotHand.h>
-
+#include <RobotAPI/libraries/core/Pose.h>
namespace armarx::grasping
{
@@ -10,10 +9,9 @@ namespace armarx::grasping
{
}
-
- void GraspCandidateVisu::visualize(
- const grasping::GraspCandidateDict& candidates,
- viz::Client& arviz)
+ void
+ GraspCandidateVisu::visualize(const grasping::GraspCandidateDict& candidates,
+ viz::Client& arviz)
{
viz::Layer graspsLayer = arviz.layer("Grasps");
viz::Layer graspsNonReachableLayer = arviz.layer("Grasps Non-Reachable");
@@ -22,10 +20,11 @@ namespace armarx::grasping
arviz.commit({graspsLayer, graspsNonReachableLayer});
}
-
- void GraspCandidateVisu::visualize(const std::string &id, const GraspCandidate &candidate,
- viz::Layer& layerReachable,
- viz::Layer& layerNonReachable)
+ void
+ GraspCandidateVisu::visualize(const std::string& id,
+ const GraspCandidate& candidate,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable)
{
int alpha = alpha_default;
if (auto it = alphasByKey.find(id); it != alphasByKey.end())
@@ -33,7 +32,8 @@ namespace armarx::grasping
alpha = it->second;
}
- ARMARX_CHECK(candidate.tcpPoseInHandRoot) << "Candidate needs to have tcpPoseInHandRoot to be visualized!";
+ ARMARX_CHECK(candidate.tcpPoseInHandRoot)
+ << "Candidate needs to have tcpPoseInHandRoot to be visualized!";
viz::Robot hand = visualize("Grasp_" + id, candidate, alpha);
if (candidate.reachabilityInfo && candidate.reachabilityInfo->reachable)
@@ -46,9 +46,10 @@ namespace armarx::grasping
}
}
- void GraspCandidateVisu::visualize(const grasping::GraspCandidateDict& candidates,
- viz::Layer& layerReachable,
- viz::Layer& layerNonReachable)
+ void
+ GraspCandidateVisu::visualize(const grasping::GraspCandidateDict& candidates,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable)
{
for (auto& [id, candidate] : candidates)
{
@@ -56,20 +57,16 @@ namespace armarx::grasping
}
}
-
- viz::Robot GraspCandidateVisu::visualize(
- const std::string& name,
- const GraspCandidate& candidate)
+ viz::Robot
+ GraspCandidateVisu::visualize(const std::string& name, const GraspCandidate& candidate)
{
return visualize(name, candidate, alpha_default);
}
-
viz::Robot
- GraspCandidateVisu::visualize(
- const std::string& name,
- const grasping::GraspCandidate& candidate,
- int alpha)
+ GraspCandidateVisu::visualize(const std::string& name,
+ const grasping::GraspCandidate& candidate,
+ int alpha)
{
bool isReachable = candidate.reachabilityInfo && candidate.reachabilityInfo->reachable;
viz::Color color = isReachable ? viz::Color::green() : viz::Color::orange();
@@ -80,11 +77,10 @@ namespace armarx::grasping
std::string modelFile = "rt/robotmodel/Armar6-SH/Armar6-" + candidate.side + "Hand-v3.xml";
viz::Robot hand = viz::RobotHand(name)
- .file("Armar6RT", modelFile)
- .pose(fromIce(candidate.robotPose) * graspPose * tcp2handRoot)
- .overrideColor(color);
+ .file("Armar6RT", modelFile)
+ .pose(fromIce(candidate.robotPose) * graspPose * tcp2handRoot)
+ .overrideColor(color);
return hand;
}
-}
-
+} // namespace armarx::grasping
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h
index 8eb298e4183d0daef8d004d4d3fc3cacc8cd30e7..edb1b3a686a8d5b63ba4b706b5ca510fad7be179 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h
@@ -1,11 +1,9 @@
#pragma once
-#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
-
+#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
-
namespace armarx::grasping
{
class GraspCandidateVisu
@@ -14,35 +12,24 @@ namespace armarx::grasping
GraspCandidateVisu();
- void visualize(
- const grasping::GraspCandidateDict& candidates,
- viz::Client& arviz);
+ void visualize(const grasping::GraspCandidateDict& candidates, viz::Client& arviz);
- void visualize(
- const grasping::GraspCandidateDict& candidates,
- viz::Layer& layerReachable,
- viz::Layer& layerNonReachable);
+ void visualize(const grasping::GraspCandidateDict& candidates,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable);
- void visualize(
- const std::string& id,
- const grasping::GraspCandidate& candidate,
- viz::Layer& layerReachable,
- viz::Layer& layerNonReachable);
+ void visualize(const std::string& id,
+ const grasping::GraspCandidate& candidate,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable);
- viz::Robot visualize(
- const std::string& name,
- const grasping::GraspCandidate& candidate);
- viz::Robot visualize(
- const std::string& name,
- const grasping::GraspCandidate& candidate,
- int alpha);
+ viz::Robot visualize(const std::string& name, const grasping::GraspCandidate& candidate);
+ viz::Robot
+ visualize(const std::string& name, const grasping::GraspCandidate& candidate, int alpha);
public:
-
int alpha_default = 255;
std::map<std::string, int> alphasByKey = {};
-
-
};
-}
+} // namespace armarx::grasping
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
index efa9b038a15f3a446af7063f05ccb93cf31b45de..cf544de9c0c584ad9b8916e05e34c34dd8229550 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -47,7 +47,6 @@
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
-
namespace armarx
{
@@ -201,7 +200,7 @@ namespace armarx
{
ARMARX_CHECK(std::filesystem::exists(filename));
ARMARX_INFO << "Reading from file `" << filename << "`";
-
+
std::ifstream ifs(filename);
const nlohmann::json j = nlohmann::json::parse(ifs);
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.h b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.h
index 9e1ac4ca2f78637660c12250a0dfd4dd4b8250b5..e71604d6be2e65c3b1e418702c053cb20b9bba38 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.h
+++ b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.h
@@ -1,8 +1,8 @@
#pragma once
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
-#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectType.aron.generated.h>
+#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
namespace armarx
{
@@ -20,16 +20,20 @@ namespace armarx
void fromAron(const arondto::GraspCandidateSourceInfo& dto, GraspCandidateSourceInfo& bo);
void toAron(arondto::GraspCandidateSourceInfo& dto, const GraspCandidateSourceInfo& bo);
- void fromAron(const arondto::GraspCandidateExecutionHints& dto, GraspCandidateExecutionHints& bo);
- void toAron(arondto::GraspCandidateExecutionHints& dto, const GraspCandidateExecutionHints& bo);
+ void fromAron(const arondto::GraspCandidateExecutionHints& dto,
+ GraspCandidateExecutionHints& bo);
+ void toAron(arondto::GraspCandidateExecutionHints& dto,
+ const GraspCandidateExecutionHints& bo);
- void fromAron(const arondto::GraspCandidateReachabilityInfo& dto, GraspCandidateReachabilityInfo& bo);
- void toAron(arondto::GraspCandidateReachabilityInfo& dto, const GraspCandidateReachabilityInfo& bo);
+ void fromAron(const arondto::GraspCandidateReachabilityInfo& dto,
+ GraspCandidateReachabilityInfo& bo);
+ void toAron(arondto::GraspCandidateReachabilityInfo& dto,
+ const GraspCandidateReachabilityInfo& bo);
void fromAron(const arondto::GraspCandidate& dto, GraspCandidate& bo);
void toAron(arondto::GraspCandidate& dto, const GraspCandidate& bo);
void fromAron(const arondto::BimanualGraspCandidate& dto, BimanualGraspCandidate& bo);
void toAron(arondto::BimanualGraspCandidate& dto, const BimanualGraspCandidate& bo);
- }
-}
+ } // namespace grasping
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
index 00a84074915aa9a7979ae47e08548c60d145de21..51267331fba123f69dfa2dfd8c1d2adeef287239 100644
--- a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.cpp
@@ -1,26 +1,26 @@
-#include <sstream>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "box_to_grasp_candidates.h"
-#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <sstream>
#include <VirtualRobot/Robot.h>
-#include "box_to_grasp_candidates.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx
{
- void box_to_grasp_candidates::add_armar6_defaults()
+ void
+ box_to_grasp_candidates::add_armar6_defaults()
{
auto& l = side("Left");
auto& r = side("Right");
l.hand_transverse = Eigen::Vector3f{0, 1, 0};
r.hand_transverse = Eigen::Vector3f{0, 1, 0};
- l.hand_up = Eigen::Vector3f{-1, 0, -1};
- r.hand_up = Eigen::Vector3f{+1, 0, -1};
- l.tcp_shift = Eigen::Vector3f{0, -30, 0};
- r.tcp_shift = Eigen::Vector3f{0, -30, 0};
+ l.hand_up = Eigen::Vector3f{-1, 0, -1};
+ r.hand_up = Eigen::Vector3f{+1, 0, -1};
+ l.tcp_shift = Eigen::Vector3f{0, -30, 0};
+ r.tcp_shift = Eigen::Vector3f{0, -30, 0};
}
Eigen::Vector3f
@@ -29,35 +29,59 @@ namespace armarx
// *INDENT-OFF*
switch (align)
{
- case box_alignment::pos_y_pos_x: [[fallthrough]];
- case box_alignment::neg_y_pos_x: [[fallthrough]];
- case box_alignment::pos_z_pos_x: [[fallthrough]];
- case box_alignment::neg_z_pos_x: return box.axis_x();
-
- case box_alignment::pos_y_neg_x: [[fallthrough]];
- case box_alignment::neg_y_neg_x: [[fallthrough]];
- case box_alignment::pos_z_neg_x: [[fallthrough]];
- case box_alignment::neg_z_neg_x: return -box.axis_x();
-
- case box_alignment::pos_x_pos_y: [[fallthrough]];
- case box_alignment::neg_x_pos_y: [[fallthrough]];
- case box_alignment::pos_z_pos_y: [[fallthrough]];
- case box_alignment::neg_z_pos_y: return box.axis_y();
-
- case box_alignment::pos_x_neg_y: [[fallthrough]];
- case box_alignment::neg_x_neg_y: [[fallthrough]];
- case box_alignment::pos_z_neg_y: [[fallthrough]];
- case box_alignment::neg_z_neg_y: return -box.axis_y();
-
- case box_alignment::pos_x_pos_z: [[fallthrough]];
- case box_alignment::neg_x_pos_z: [[fallthrough]];
- case box_alignment::pos_y_pos_z: [[fallthrough]];
- case box_alignment::neg_y_pos_z: return box.axis_z();
-
- case box_alignment::pos_x_neg_z: [[fallthrough]];
- case box_alignment::neg_x_neg_z: [[fallthrough]];
- case box_alignment::pos_y_neg_z: [[fallthrough]];
- case box_alignment::neg_y_neg_z: return -box.axis_z();
+ case box_alignment::pos_y_pos_x:
+ [[fallthrough]];
+ case box_alignment::neg_y_pos_x:
+ [[fallthrough]];
+ case box_alignment::pos_z_pos_x:
+ [[fallthrough]];
+ case box_alignment::neg_z_pos_x:
+ return box.axis_x();
+
+ case box_alignment::pos_y_neg_x:
+ [[fallthrough]];
+ case box_alignment::neg_y_neg_x:
+ [[fallthrough]];
+ case box_alignment::pos_z_neg_x:
+ [[fallthrough]];
+ case box_alignment::neg_z_neg_x:
+ return -box.axis_x();
+
+ case box_alignment::pos_x_pos_y:
+ [[fallthrough]];
+ case box_alignment::neg_x_pos_y:
+ [[fallthrough]];
+ case box_alignment::pos_z_pos_y:
+ [[fallthrough]];
+ case box_alignment::neg_z_pos_y:
+ return box.axis_y();
+
+ case box_alignment::pos_x_neg_y:
+ [[fallthrough]];
+ case box_alignment::neg_x_neg_y:
+ [[fallthrough]];
+ case box_alignment::pos_z_neg_y:
+ [[fallthrough]];
+ case box_alignment::neg_z_neg_y:
+ return -box.axis_y();
+
+ case box_alignment::pos_x_pos_z:
+ [[fallthrough]];
+ case box_alignment::neg_x_pos_z:
+ [[fallthrough]];
+ case box_alignment::pos_y_pos_z:
+ [[fallthrough]];
+ case box_alignment::neg_y_pos_z:
+ return box.axis_z();
+
+ case box_alignment::pos_x_neg_z:
+ [[fallthrough]];
+ case box_alignment::neg_x_neg_z:
+ [[fallthrough]];
+ case box_alignment::pos_y_neg_z:
+ [[fallthrough]];
+ case box_alignment::neg_y_neg_z:
+ return -box.axis_z();
}
// *INDENT-ON*
ARMARX_TRACE;
@@ -70,35 +94,59 @@ namespace armarx
// *INDENT-OFF*
switch (align)
{
- case box_alignment::pos_y_pos_x: [[fallthrough]];
- case box_alignment::neg_y_pos_x: [[fallthrough]];
- case box_alignment::pos_z_pos_x: [[fallthrough]];
- case box_alignment::neg_z_pos_x: [[fallthrough]];
-
- case box_alignment::pos_y_neg_x: [[fallthrough]];
- case box_alignment::neg_y_neg_x: [[fallthrough]];
- case box_alignment::pos_z_neg_x: [[fallthrough]];
- case box_alignment::neg_z_neg_x: return box.dimension_x();
-
- case box_alignment::pos_x_pos_y: [[fallthrough]];
- case box_alignment::neg_x_pos_y: [[fallthrough]];
- case box_alignment::pos_z_pos_y: [[fallthrough]];
- case box_alignment::neg_z_pos_y: [[fallthrough]];
-
- case box_alignment::pos_x_neg_y: [[fallthrough]];
- case box_alignment::neg_x_neg_y: [[fallthrough]];
- case box_alignment::pos_z_neg_y: [[fallthrough]];
- case box_alignment::neg_z_neg_y: return box.dimension_y();
-
- case box_alignment::pos_x_pos_z: [[fallthrough]];
- case box_alignment::neg_x_pos_z: [[fallthrough]];
- case box_alignment::pos_y_pos_z: [[fallthrough]];
- case box_alignment::neg_y_pos_z: [[fallthrough]];
-
- case box_alignment::pos_x_neg_z: [[fallthrough]];
- case box_alignment::neg_x_neg_z: [[fallthrough]];
- case box_alignment::pos_y_neg_z: [[fallthrough]];
- case box_alignment::neg_y_neg_z: return box.dimension_z();
+ case box_alignment::pos_y_pos_x:
+ [[fallthrough]];
+ case box_alignment::neg_y_pos_x:
+ [[fallthrough]];
+ case box_alignment::pos_z_pos_x:
+ [[fallthrough]];
+ case box_alignment::neg_z_pos_x:
+ [[fallthrough]];
+
+ case box_alignment::pos_y_neg_x:
+ [[fallthrough]];
+ case box_alignment::neg_y_neg_x:
+ [[fallthrough]];
+ case box_alignment::pos_z_neg_x:
+ [[fallthrough]];
+ case box_alignment::neg_z_neg_x:
+ return box.dimension_x();
+
+ case box_alignment::pos_x_pos_y:
+ [[fallthrough]];
+ case box_alignment::neg_x_pos_y:
+ [[fallthrough]];
+ case box_alignment::pos_z_pos_y:
+ [[fallthrough]];
+ case box_alignment::neg_z_pos_y:
+ [[fallthrough]];
+
+ case box_alignment::pos_x_neg_y:
+ [[fallthrough]];
+ case box_alignment::neg_x_neg_y:
+ [[fallthrough]];
+ case box_alignment::pos_z_neg_y:
+ [[fallthrough]];
+ case box_alignment::neg_z_neg_y:
+ return box.dimension_y();
+
+ case box_alignment::pos_x_pos_z:
+ [[fallthrough]];
+ case box_alignment::neg_x_pos_z:
+ [[fallthrough]];
+ case box_alignment::pos_y_pos_z:
+ [[fallthrough]];
+ case box_alignment::neg_y_pos_z:
+ [[fallthrough]];
+
+ case box_alignment::pos_x_neg_z:
+ [[fallthrough]];
+ case box_alignment::neg_x_neg_z:
+ [[fallthrough]];
+ case box_alignment::pos_y_neg_z:
+ [[fallthrough]];
+ case box_alignment::neg_y_neg_z:
+ return box.dimension_z();
}
// *INDENT-ON*
ARMARX_TRACE;
@@ -111,35 +159,59 @@ namespace armarx
// *INDENT-OFF*
switch (align)
{
- case box_alignment::pos_x_pos_y: [[fallthrough]];
- case box_alignment::pos_x_pos_z: [[fallthrough]];
- case box_alignment::pos_x_neg_y: [[fallthrough]];
- case box_alignment::pos_x_neg_z: return box.axis_x();
-
- case box_alignment::neg_x_pos_y: [[fallthrough]];
- case box_alignment::neg_x_pos_z: [[fallthrough]];
- case box_alignment::neg_x_neg_y: [[fallthrough]];
- case box_alignment::neg_x_neg_z: return -box.axis_x();
-
- case box_alignment::pos_y_pos_x: [[fallthrough]];
- case box_alignment::pos_y_pos_z: [[fallthrough]];
- case box_alignment::pos_y_neg_x: [[fallthrough]];
- case box_alignment::pos_y_neg_z: return box.axis_y();
-
- case box_alignment::neg_y_pos_x: [[fallthrough]];
- case box_alignment::neg_y_pos_z: [[fallthrough]];
- case box_alignment::neg_y_neg_x: [[fallthrough]];
- case box_alignment::neg_y_neg_z: return -box.axis_y();
-
- case box_alignment::pos_z_pos_x: [[fallthrough]];
- case box_alignment::pos_z_pos_y: [[fallthrough]];
- case box_alignment::pos_z_neg_x: [[fallthrough]];
- case box_alignment::pos_z_neg_y: return box.axis_z();
-
- case box_alignment::neg_z_pos_x: [[fallthrough]];
- case box_alignment::neg_z_pos_y: [[fallthrough]];
- case box_alignment::neg_z_neg_x: [[fallthrough]];
- case box_alignment::neg_z_neg_y: return -box.axis_z();
+ case box_alignment::pos_x_pos_y:
+ [[fallthrough]];
+ case box_alignment::pos_x_pos_z:
+ [[fallthrough]];
+ case box_alignment::pos_x_neg_y:
+ [[fallthrough]];
+ case box_alignment::pos_x_neg_z:
+ return box.axis_x();
+
+ case box_alignment::neg_x_pos_y:
+ [[fallthrough]];
+ case box_alignment::neg_x_pos_z:
+ [[fallthrough]];
+ case box_alignment::neg_x_neg_y:
+ [[fallthrough]];
+ case box_alignment::neg_x_neg_z:
+ return -box.axis_x();
+
+ case box_alignment::pos_y_pos_x:
+ [[fallthrough]];
+ case box_alignment::pos_y_pos_z:
+ [[fallthrough]];
+ case box_alignment::pos_y_neg_x:
+ [[fallthrough]];
+ case box_alignment::pos_y_neg_z:
+ return box.axis_y();
+
+ case box_alignment::neg_y_pos_x:
+ [[fallthrough]];
+ case box_alignment::neg_y_pos_z:
+ [[fallthrough]];
+ case box_alignment::neg_y_neg_x:
+ [[fallthrough]];
+ case box_alignment::neg_y_neg_z:
+ return -box.axis_y();
+
+ case box_alignment::pos_z_pos_x:
+ [[fallthrough]];
+ case box_alignment::pos_z_pos_y:
+ [[fallthrough]];
+ case box_alignment::pos_z_neg_x:
+ [[fallthrough]];
+ case box_alignment::pos_z_neg_y:
+ return box.axis_z();
+
+ case box_alignment::neg_z_pos_x:
+ [[fallthrough]];
+ case box_alignment::neg_z_pos_y:
+ [[fallthrough]];
+ case box_alignment::neg_z_neg_x:
+ [[fallthrough]];
+ case box_alignment::neg_z_neg_y:
+ return -box.axis_z();
}
// *INDENT-ON*
ARMARX_TRACE;
@@ -152,35 +224,59 @@ namespace armarx
// *INDENT-OFF*
switch (align)
{
- case box_alignment::pos_x_pos_y: [[fallthrough]];
- case box_alignment::pos_x_pos_z: [[fallthrough]];
- case box_alignment::pos_x_neg_y: [[fallthrough]];
- case box_alignment::pos_x_neg_z: [[fallthrough]];
-
- case box_alignment::neg_x_pos_y: [[fallthrough]];
- case box_alignment::neg_x_pos_z: [[fallthrough]];
- case box_alignment::neg_x_neg_y: [[fallthrough]];
- case box_alignment::neg_x_neg_z: return box.dimension_x();
-
- case box_alignment::pos_y_pos_x: [[fallthrough]];
- case box_alignment::pos_y_pos_z: [[fallthrough]];
- case box_alignment::pos_y_neg_x: [[fallthrough]];
- case box_alignment::pos_y_neg_z: [[fallthrough]];
-
- case box_alignment::neg_y_pos_x: [[fallthrough]];
- case box_alignment::neg_y_pos_z: [[fallthrough]];
- case box_alignment::neg_y_neg_x: [[fallthrough]];
- case box_alignment::neg_y_neg_z: return box.dimension_y();
-
- case box_alignment::pos_z_pos_x: [[fallthrough]];
- case box_alignment::pos_z_pos_y: [[fallthrough]];
- case box_alignment::pos_z_neg_x: [[fallthrough]];
- case box_alignment::pos_z_neg_y: [[fallthrough]];
-
- case box_alignment::neg_z_pos_x: [[fallthrough]];
- case box_alignment::neg_z_pos_y: [[fallthrough]];
- case box_alignment::neg_z_neg_x: [[fallthrough]];
- case box_alignment::neg_z_neg_y: return box.dimension_z();
+ case box_alignment::pos_x_pos_y:
+ [[fallthrough]];
+ case box_alignment::pos_x_pos_z:
+ [[fallthrough]];
+ case box_alignment::pos_x_neg_y:
+ [[fallthrough]];
+ case box_alignment::pos_x_neg_z:
+ [[fallthrough]];
+
+ case box_alignment::neg_x_pos_y:
+ [[fallthrough]];
+ case box_alignment::neg_x_pos_z:
+ [[fallthrough]];
+ case box_alignment::neg_x_neg_y:
+ [[fallthrough]];
+ case box_alignment::neg_x_neg_z:
+ return box.dimension_x();
+
+ case box_alignment::pos_y_pos_x:
+ [[fallthrough]];
+ case box_alignment::pos_y_pos_z:
+ [[fallthrough]];
+ case box_alignment::pos_y_neg_x:
+ [[fallthrough]];
+ case box_alignment::pos_y_neg_z:
+ [[fallthrough]];
+
+ case box_alignment::neg_y_pos_x:
+ [[fallthrough]];
+ case box_alignment::neg_y_pos_z:
+ [[fallthrough]];
+ case box_alignment::neg_y_neg_x:
+ [[fallthrough]];
+ case box_alignment::neg_y_neg_z:
+ return box.dimension_y();
+
+ case box_alignment::pos_z_pos_x:
+ [[fallthrough]];
+ case box_alignment::pos_z_pos_y:
+ [[fallthrough]];
+ case box_alignment::pos_z_neg_x:
+ [[fallthrough]];
+ case box_alignment::pos_z_neg_y:
+ [[fallthrough]];
+
+ case box_alignment::neg_z_pos_x:
+ [[fallthrough]];
+ case box_alignment::neg_z_pos_y:
+ [[fallthrough]];
+ case box_alignment::neg_z_neg_x:
+ [[fallthrough]];
+ case box_alignment::neg_z_neg_y:
+ return box.dimension_z();
}
// *INDENT-ON*
ARMARX_TRACE;
@@ -193,55 +289,81 @@ namespace armarx
// *INDENT-OFF*
switch (align)
{
- case box_alignment::pos_y_pos_z: [[fallthrough]];
- case box_alignment::pos_y_neg_z: [[fallthrough]];
- case box_alignment::neg_y_pos_z: [[fallthrough]];
- case box_alignment::neg_y_neg_z: [[fallthrough]];
-
- case box_alignment::pos_z_pos_y: [[fallthrough]];
- case box_alignment::pos_z_neg_y: [[fallthrough]];
- case box_alignment::neg_z_pos_y: [[fallthrough]];
- case box_alignment::neg_z_neg_y: return box.dimension_x();
-
- case box_alignment::pos_x_pos_z: [[fallthrough]];
- case box_alignment::pos_x_neg_z: [[fallthrough]];
- case box_alignment::neg_x_pos_z: [[fallthrough]];
- case box_alignment::neg_x_neg_z: [[fallthrough]];
-
- case box_alignment::pos_z_pos_x: [[fallthrough]];
- case box_alignment::pos_z_neg_x: [[fallthrough]];
- case box_alignment::neg_z_pos_x: [[fallthrough]];
- case box_alignment::neg_z_neg_x: return box.dimension_y();
-
- case box_alignment::pos_x_pos_y: [[fallthrough]];
- case box_alignment::pos_x_neg_y: [[fallthrough]];
- case box_alignment::neg_x_pos_y: [[fallthrough]];
- case box_alignment::neg_x_neg_y: [[fallthrough]];
-
- case box_alignment::pos_y_pos_x: [[fallthrough]];
- case box_alignment::pos_y_neg_x: [[fallthrough]];
- case box_alignment::neg_y_pos_x: [[fallthrough]];
- case box_alignment::neg_y_neg_x: return box.dimension_z();
+ case box_alignment::pos_y_pos_z:
+ [[fallthrough]];
+ case box_alignment::pos_y_neg_z:
+ [[fallthrough]];
+ case box_alignment::neg_y_pos_z:
+ [[fallthrough]];
+ case box_alignment::neg_y_neg_z:
+ [[fallthrough]];
+
+ case box_alignment::pos_z_pos_y:
+ [[fallthrough]];
+ case box_alignment::pos_z_neg_y:
+ [[fallthrough]];
+ case box_alignment::neg_z_pos_y:
+ [[fallthrough]];
+ case box_alignment::neg_z_neg_y:
+ return box.dimension_x();
+
+ case box_alignment::pos_x_pos_z:
+ [[fallthrough]];
+ case box_alignment::pos_x_neg_z:
+ [[fallthrough]];
+ case box_alignment::neg_x_pos_z:
+ [[fallthrough]];
+ case box_alignment::neg_x_neg_z:
+ [[fallthrough]];
+
+ case box_alignment::pos_z_pos_x:
+ [[fallthrough]];
+ case box_alignment::pos_z_neg_x:
+ [[fallthrough]];
+ case box_alignment::neg_z_pos_x:
+ [[fallthrough]];
+ case box_alignment::neg_z_neg_x:
+ return box.dimension_y();
+
+ case box_alignment::pos_x_pos_y:
+ [[fallthrough]];
+ case box_alignment::pos_x_neg_y:
+ [[fallthrough]];
+ case box_alignment::neg_x_pos_y:
+ [[fallthrough]];
+ case box_alignment::neg_x_neg_y:
+ [[fallthrough]];
+
+ case box_alignment::pos_y_pos_x:
+ [[fallthrough]];
+ case box_alignment::pos_y_neg_x:
+ [[fallthrough]];
+ case box_alignment::neg_y_pos_x:
+ [[fallthrough]];
+ case box_alignment::neg_y_neg_x:
+ return box.dimension_z();
}
// *INDENT-ON*
ARMARX_TRACE;
throw std::logic_error{"reached unreachable code"};
}
- bool box_to_grasp_candidates::length_limit::in_limits(const box_to_grasp_candidates::box_t& box, box_to_grasp_candidates::box_alignment align) const
+ bool
+ box_to_grasp_candidates::length_limit::in_limits(
+ const box_to_grasp_candidates::box_t& box,
+ box_to_grasp_candidates::box_alignment align) const
{
const auto t_len = box_transverse_len(box, align);
const auto u_len = box_up_len(box, align);
const auto f_len = box_forward_len(box, align);
- return t_len < transverse_length_max &&
- t_len > transverse_length_min &&
- u_len < upward_length_max &&
- u_len > upward_length_min &&
- f_len < forward_length_max &&
- f_len > forward_length_min;
+ return t_len < transverse_length_max && t_len > transverse_length_min &&
+ u_len < upward_length_max && u_len > upward_length_min &&
+ f_len < forward_length_max && f_len > forward_length_min;
}
- std::string box_to_grasp_candidates::length_limit::limit_violation_string(const box_t& box, box_alignment align) const
+ std::string
+ box_to_grasp_candidates::length_limit::limit_violation_string(const box_t& box,
+ box_alignment align) const
{
const auto t_len = box_transverse_len(box, align);
const auto u_len = box_up_len(box, align);
@@ -249,7 +371,8 @@ namespace armarx
std::stringstream s;
if (!(t_len < transverse_length_max && t_len > transverse_length_min))
{
- s << "tr: " << t_len << " [" << transverse_length_min << ", " << transverse_length_max << "] ";
+ s << "tr: " << t_len << " [" << transverse_length_min << ", " << transverse_length_max
+ << "] ";
}
if (!(u_len < upward_length_max && u_len > upward_length_min))
{
@@ -263,30 +386,26 @@ namespace armarx
}
armarx::grasping::GraspCandidatePtr
- box_to_grasp_candidates::grasp::make_candidate(
- const std::string& provider_name
- ) const
+ box_to_grasp_candidates::grasp::make_candidate(const std::string& provider_name) const
{
ARMARX_CHECK_EXPRESSION(side && side->nh_arm_w_rob.getRobot());
- return make_candidate(provider_name, side->name,
- side->nh_arm_w_rob.getRobot()->getGlobalPose());
+ return make_candidate(
+ provider_name, side->name, side->nh_arm_w_rob.getRobot()->getGlobalPose());
}
+
armarx::grasping::GraspCandidatePtr
- box_to_grasp_candidates::grasp::make_candidate(
- const std::string& provider_name,
- const std::string& side_name,
- const VirtualRobot::RobotPtr& robot
- ) const
+ box_to_grasp_candidates::grasp::make_candidate(const std::string& provider_name,
+ const std::string& side_name,
+ const VirtualRobot::RobotPtr& robot) const
{
ARMARX_CHECK_NOT_NULL(robot);
return make_candidate(provider_name, side_name, robot->getGlobalPose());
}
+
armarx::grasping::GraspCandidatePtr
- box_to_grasp_candidates::grasp::make_candidate(
- const std::string& provider_name,
- const std::string& side_name,
- const Eigen::Matrix4f& global_pose
- ) const
+ box_to_grasp_candidates::grasp::make_candidate(const std::string& provider_name,
+ const std::string& side_name,
+ const Eigen::Matrix4f& global_pose) const
{
armarx::grasping::GraspCandidatePtr gc = new armarx::grasping::GraspCandidate;
gc->robotPose = new armarx::Pose(global_pose);
@@ -300,58 +419,61 @@ namespace armarx
return gc;
}
-}
+} // namespace armarx
namespace armarx
{
- box_to_grasp_candidates::box_to_grasp_candidates(armarx::RobotNameHelperPtr rnh, const VirtualRobot::RobotPtr& robot)
- : _rnh{std::move(rnh)}, _robot{robot}
+ box_to_grasp_candidates::box_to_grasp_candidates(armarx::RobotNameHelperPtr rnh,
+ const VirtualRobot::RobotPtr& robot) :
+ _rnh{std::move(rnh)}, _robot{robot}
{
ARMARX_CHECK_NOT_NULL(_robot);
}
- const box_to_grasp_candidates::side_data& box_to_grasp_candidates::side(const std::string& side) const
+ const box_to_grasp_candidates::side_data&
+ box_to_grasp_candidates::side(const std::string& side) const
{
return _sides.at(side);
}
- box_to_grasp_candidates::side_data& box_to_grasp_candidates::side(const std::string& side)
+ box_to_grasp_candidates::side_data&
+ box_to_grasp_candidates::side(const std::string& side)
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(_robot);
if (_sides.count(side))
{
- return _sides.at(side);
+ return _sides.at(side);
}
auto& data = _sides[side];
- data.name = side;
- data.nh_arm = _rnh->getArm(side);
+ data.name = side;
+ data.nh_arm = _rnh->getArm(side);
data.nh_arm_w_rob = data.nh_arm.addRobot(_robot);
return data;
}
- const std::map<std::string, box_to_grasp_candidates::side_data>& box_to_grasp_candidates::sides() const
+ const std::map<std::string, box_to_grasp_candidates::side_data>&
+ box_to_grasp_candidates::sides() const
{
return _sides;
}
- box_to_grasp_candidates::grasp box_to_grasp_candidates::center_grasp(
- const simox::OrientedBox<float>& box,
- const side_data& side,
- box_alignment align)
+ box_to_grasp_candidates::grasp
+ box_to_grasp_candidates::center_grasp(const simox::OrientedBox<float>& box,
+ const side_data& side,
+ box_alignment align)
{
ARMARX_TRACE;
- const auto g = center_grasp(
- box.center(),
+ const auto g = center_grasp(box.center(),
- box_up_axis(box, align),
- box_up_len(box, align),
- side.hand_up,
+ box_up_axis(box, align),
+ box_up_len(box, align),
+ side.hand_up,
- box_transverse_axis(box, align),
- side.hand_transverse,
+ box_transverse_axis(box, align),
+ side.hand_transverse,
- side.tcp_shift);
+ side.tcp_shift);
return {g.pose, g.approach, &side};
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.h b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.h
index dfd7e1030ba71ebd68b18cba6aa476c0bab51ebc..e8dc6947f911de87c06df9851e0104fc75301295 100644
--- a/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.h
+++ b/source/RobotAPI/libraries/GraspingUtility/box_to_grasp_candidates.h
@@ -2,8 +2,8 @@
#include <Eigen/Dense>
-#include <SimoxUtility/shapes/OrientedBox.h>
#include <SimoxUtility/meta/eigen/enable_if_compile_time_size.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
@@ -16,91 +16,108 @@ namespace armarx
using box_t = simox::OrientedBox<float>;
enum class box_alignment : std::uint_fast8_t
{
- pos_x_pos_y, pos_x_pos_z,
- pos_x_neg_y, pos_x_neg_z,
-
- neg_x_pos_y, neg_x_pos_z,
- neg_x_neg_y, neg_x_neg_z,
-
- pos_y_pos_x, pos_y_pos_z,
- pos_y_neg_x, pos_y_neg_z,
-
- neg_y_pos_x, neg_y_pos_z,
- neg_y_neg_x, neg_y_neg_z,
-
- pos_z_pos_x, pos_z_pos_y,
- pos_z_neg_x, pos_z_neg_y,
-
- neg_z_pos_x, neg_z_pos_y,
- neg_z_neg_x, neg_z_neg_y
+ pos_x_pos_y,
+ pos_x_pos_z,
+ pos_x_neg_y,
+ pos_x_neg_z,
+
+ neg_x_pos_y,
+ neg_x_pos_z,
+ neg_x_neg_y,
+ neg_x_neg_z,
+
+ pos_y_pos_x,
+ pos_y_pos_z,
+ pos_y_neg_x,
+ pos_y_neg_z,
+
+ neg_y_pos_x,
+ neg_y_pos_z,
+ neg_y_neg_x,
+ neg_y_neg_z,
+
+ pos_z_pos_x,
+ pos_z_pos_y,
+ pos_z_neg_x,
+ pos_z_neg_y,
+
+ neg_z_pos_x,
+ neg_z_pos_y,
+ neg_z_neg_x,
+ neg_z_neg_y
};
+
struct side_enabled_map
{
- bool& operator[](box_alignment al)
+ bool&
+ operator[](box_alignment al)
{
return enabled.at(static_cast<std::uint_fast8_t>(al));
}
- bool& operator[](std::uint_fast8_t al)
+
+ bool&
+ operator[](std::uint_fast8_t al)
{
return enabled.at(al);
}
- bool operator[](box_alignment al) const
+
+ bool
+ operator[](box_alignment al) const
{
return enabled.at(static_cast<std::uint_fast8_t>(al));
}
- bool operator[](std::uint_fast8_t al) const
+
+ bool
+ operator[](std::uint_fast8_t al) const
{
return enabled.at(al);
}
- std::array<bool, 24> enabled
- {
- 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0
- };
+
+ std::array<bool, 24> enabled{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
};
struct length_limit
{
- float transverse_length_min = 50;
+ float transverse_length_min = 50;
float transverse_length_max = 1500;
- float upward_length_min = 25;
- float upward_length_max = 500;
- float forward_length_min = 50;
- float forward_length_max = 150;
+ float upward_length_min = 25;
+ float upward_length_max = 500;
+ float forward_length_min = 50;
+ float forward_length_max = 150;
bool in_limits(const box_t& box, box_alignment align) const;
std::string limit_violation_string(const box_t& box, box_alignment align) const;
};
+
struct side_data
{
- std::string name;
- armarx::RobotNameHelper::Arm nh_arm;
+ std::string name;
+ armarx::RobotNameHelper::Arm nh_arm;
armarx::RobotNameHelper::RobotArm nh_arm_w_rob;
- Eigen::Vector3f tcp_shift = Eigen::Vector3f::Constant(std::nanf(""));
- Eigen::Vector3f hand_up = Eigen::Vector3f::Constant(std::nanf(""));
- Eigen::Vector3f hand_transverse = Eigen::Vector3f::Constant(std::nanf(""));
+ Eigen::Vector3f tcp_shift = Eigen::Vector3f::Constant(std::nanf(""));
+ Eigen::Vector3f hand_up = Eigen::Vector3f::Constant(std::nanf(""));
+ Eigen::Vector3f hand_transverse = Eigen::Vector3f::Constant(std::nanf(""));
};
+
struct grasp
{
- Eigen::Matrix4f pose = Eigen::Matrix4f::Constant(std::nanf(""));
+ Eigen::Matrix4f pose = Eigen::Matrix4f::Constant(std::nanf(""));
Eigen::Vector3f approach = Eigen::Vector3f::Constant(std::nanf(""));
- const side_data* side = nullptr;
-
- armarx::grasping::GraspCandidatePtr make_candidate(
- const std::string& provider_name
- ) const;
- armarx::grasping::GraspCandidatePtr make_candidate(
- const std::string& provider_name,
- const std::string& side_name,
- const VirtualRobot::RobotPtr& robot = nullptr
- ) const;
- armarx::grasping::GraspCandidatePtr make_candidate(
- const std::string& provider_name,
- const std::string& side_name,
- const Eigen::Matrix4f& global_pose
- ) const;
+ const side_data* side = nullptr;
+
+ armarx::grasping::GraspCandidatePtr
+ make_candidate(const std::string& provider_name) const;
+ armarx::grasping::GraspCandidatePtr
+ make_candidate(const std::string& provider_name,
+ const std::string& side_name,
+ const VirtualRobot::RobotPtr& robot = nullptr) const;
+ armarx::grasping::GraspCandidatePtr
+ make_candidate(const std::string& provider_name,
+ const std::string& side_name,
+ const Eigen::Matrix4f& global_pose) const;
};
+
public:
box_to_grasp_candidates(armarx::RobotNameHelperPtr rnh,
const VirtualRobot::RobotPtr& robot);
@@ -142,27 +159,25 @@ namespace armarx
auto consume_grasp,
const side_enabled_map& enabled);
- grasp center_grasp(const box_t& box,
- const side_data& side,
- box_alignment align);
-
- template<class D1, class D2, class D3>
- static
- std::enable_if_t <
- simox::meta::is_vec3_v<D1>&&
- simox::meta::is_vec3_v<D2>&&
- simox::meta::is_vec3_v<D3>, grasp >
- center_grasp(
- const Eigen::MatrixBase<D1>& center_pos,
- const Eigen::MatrixBase<D2>& up, float up_len, const Eigen::Vector3f& hand_up,
- const Eigen::MatrixBase<D3>& transverse, const Eigen::Vector3f& hand_transv,
- const Eigen::Vector3f& tcp_shift);
+ grasp center_grasp(const box_t& box, const side_data& side, box_alignment align);
+
+ template <class D1, class D2, class D3>
+ static std::enable_if_t<simox::meta::is_vec3_v<D1> && simox::meta::is_vec3_v<D2> &&
+ simox::meta::is_vec3_v<D3>,
+ grasp>
+ center_grasp(const Eigen::MatrixBase<D1>& center_pos,
+ const Eigen::MatrixBase<D2>& up,
+ float up_len,
+ const Eigen::Vector3f& hand_up,
+ const Eigen::MatrixBase<D3>& transverse,
+ const Eigen::Vector3f& hand_transv,
+ const Eigen::Vector3f& tcp_shift);
private:
- armarx::RobotNameHelperPtr _rnh;
- VirtualRobot::RobotPtr _robot;
+ armarx::RobotNameHelperPtr _rnh;
+ VirtualRobot::RobotPtr _robot;
std::map<std::string, side_data> _sides;
};
-}
+} // namespace armarx
#include "box_to_grasp_candidates.ipp"
diff --git a/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp b/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp
index f24952192f6174ece60745d1855f7985fcbc232e..27d96ff26fc5094b51899a4d5e6a3495a7d1153b 100644
--- a/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.cpp
@@ -1,40 +1,53 @@
+#include "grasp_candidate_drawer.h"
+
+#include <utility>
+
#include <SimoxUtility/math/pose/pose.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <utility>
-
-#include "grasp_candidate_drawer.h"
-
namespace armarx
{
- grasp_candidate_drawer::grasp_candidate_drawer(
- armarx::RobotNameHelperPtr rnh,
- const VirtualRobot::RobotPtr& robot)
- : rnh(std::move(rnh)), robot{robot}
- {}
+ grasp_candidate_drawer::grasp_candidate_drawer(armarx::RobotNameHelperPtr rnh,
+ const VirtualRobot::RobotPtr& robot) :
+ rnh(std::move(rnh)), robot{robot}
+ {
+ }
- void grasp_candidate_drawer::reset_colors()
+ void
+ grasp_candidate_drawer::reset_colors()
{
_color_offset = 0;
}
- void grasp_candidate_drawer::draw(const armarx::grasping::GraspCandidatePtr& gc, armarx::viz::Layer& l)
+ void
+ grasp_candidate_drawer::draw(const armarx::grasping::GraspCandidatePtr& gc,
+ armarx::viz::Layer& l)
{
draw(gc, l, simox::color::GlasbeyLUT::at(_color_offset, 255));
}
- void grasp_candidate_drawer::draw(const armarx::GraspCandidateHelper& gch, armarx::viz::Layer& l)
+
+ void
+ grasp_candidate_drawer::draw(const armarx::GraspCandidateHelper& gch, armarx::viz::Layer& l)
{
draw(gch, l, simox::color::GlasbeyLUT::at(_color_offset, 255));
}
- void grasp_candidate_drawer::draw(const armarx::grasping::GraspCandidatePtr& gc, armarx::viz::Layer& l, const armarx::viz::Color& color)
+
+ void
+ grasp_candidate_drawer::draw(const armarx::grasping::GraspCandidatePtr& gc,
+ armarx::viz::Layer& l,
+ const armarx::viz::Color& color)
{
ARMARX_CHECK_NOT_NULL(gc);
ARMARX_CHECK_NOT_NULL(robot);
draw(armarx::GraspCandidateHelper{gc, robot}, l, color);
}
- void grasp_candidate_drawer::draw(const armarx::GraspCandidateHelper& gch, armarx::viz::Layer& l, const armarx::viz::Color& color)
+
+ void
+ grasp_candidate_drawer::draw(const armarx::GraspCandidateHelper& gch,
+ armarx::viz::Layer& l,
+ const armarx::viz::Color& color)
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(gch.getGraspCandidate());
@@ -42,46 +55,68 @@ namespace armarx
const auto& s = side(gch);
const std::string name = "grasp_" + s.name + "_" + std::to_string(_color_offset++);
l.add(armarx::viz::Robot(name + "_robot")
- .file(s.hand_model_package, s.hand_model_path)
- .overrideColor(color)
- .pose(gch.getGraspPoseInGlobal() * s.tcp_2_hroot));
+ .file(s.hand_model_package, s.hand_model_path)
+ .overrideColor(color)
+ .pose(gch.getGraspPoseInGlobal() * s.tcp_2_hroot));
const Eigen::Vector3f targ = gch.getGraspPositionInGlobal();
const Eigen::Vector3f appr = gch.getApproachVector();
const Eigen::Vector3f from = targ - appr * length_approach;
l.add(armarx::viz::Arrow(name + "_approach")
- .color(color)
- .width(width_approach)
- .fromTo(from, targ));
+ .color(color)
+ .width(width_approach)
+ .fromTo(from, targ));
}
- void grasp_candidate_drawer::draw(
- const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
- float ax_len, float ax_width,
- const armarx::viz::Color& color_up, const armarx::viz::Color& color_trans)
+ void
+ grasp_candidate_drawer::draw(const box_to_grasp_candidates::side_data& side,
+ armarx::viz::Layer& l,
+ float ax_len,
+ float ax_width,
+ const armarx::viz::Color& color_up,
+ const armarx::viz::Color& color_trans)
{
draw(side.name, side, l, ax_len, ax_width, color_up, color_trans);
}
- void grasp_candidate_drawer::draw(
- const std::string& name,
- const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
- float ax_len, float ax_width,
- const armarx::viz::Color& color_up, const armarx::viz::Color& color_trans)
+
+ void
+ grasp_candidate_drawer::draw(const std::string& name,
+ const box_to_grasp_candidates::side_data& side,
+ armarx::viz::Layer& l,
+ float ax_len,
+ float ax_width,
+ const armarx::viz::Color& color_up,
+ const armarx::viz::Color& color_trans)
{
- draw(side, l, side.nh_arm_w_rob.getTCP()->getGlobalPose(),
- ax_len, ax_width, color_up, color_trans);
+ draw(side,
+ l,
+ side.nh_arm_w_rob.getTCP()->getGlobalPose(),
+ ax_len,
+ ax_width,
+ color_up,
+ color_trans);
}
- void grasp_candidate_drawer::draw(
- const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
- const Eigen::Matrix4f& gpose, float ax_len, float ax_width,
- const armarx::viz::Color& color_up, const armarx::viz::Color& color_trans)
+
+ void
+ grasp_candidate_drawer::draw(const box_to_grasp_candidates::side_data& side,
+ armarx::viz::Layer& l,
+ const Eigen::Matrix4f& gpose,
+ float ax_len,
+ float ax_width,
+ const armarx::viz::Color& color_up,
+ const armarx::viz::Color& color_trans)
{
draw("", side, l, gpose, ax_len, ax_width, color_up, color_trans);
}
- void grasp_candidate_drawer::draw(
- const std::string& name,
- const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
- const Eigen::Matrix4f& gpose, float ax_len, float ax_width,
- const armarx::viz::Color& color_up, const armarx::viz::Color& color_trans)
+
+ void
+ grasp_candidate_drawer::draw(const std::string& name,
+ const box_to_grasp_candidates::side_data& side,
+ armarx::viz::Layer& l,
+ const Eigen::Matrix4f& gpose,
+ float ax_len,
+ float ax_width,
+ const armarx::viz::Color& color_up,
+ const armarx::viz::Color& color_trans)
{
const Eigen::Vector3f hand_up =
simox::math::orientation(gpose) * side.hand_up.normalized() * ax_len;
@@ -92,13 +127,13 @@ namespace armarx
const Eigen::Vector3f tcp_pos = simox::math::position(gpose);
l.add(armarx::viz::Arrow(name + "hand_up")
- .color(color_up)
- .width(ax_width)
- .fromTo(tcp_pos, tcp_pos + hand_up));
+ .color(color_up)
+ .width(ax_width)
+ .fromTo(tcp_pos, tcp_pos + hand_up));
l.add(armarx::viz::Arrow(name + "transversal_outward")
- .color(color_trans)
- .width(ax_width)
- .fromTo(tcp_pos, tcp_pos + transversal_outward));
+ .color(color_trans)
+ .width(ax_width)
+ .fromTo(tcp_pos, tcp_pos + transversal_outward));
}
const grasp_candidate_drawer::side_data&
@@ -108,7 +143,7 @@ namespace armarx
const std::string& s = gch.getGraspCandidate()->side;
if (_sides.count(s))
{
- return _sides.at(s);
+ return _sides.at(s);
}
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
@@ -121,4 +156,4 @@ namespace armarx
data.tcp_2_hroot = nh_w_rob.getTcp2HandRootTransform();
return data;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h b/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h
index 1673d86a0957db9ffdb4216241a97ee85618608c..9628d018a3d1d06c540cb122e71c55162e719c80 100644
--- a/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h
+++ b/source/RobotAPI/libraries/GraspingUtility/grasp_candidate_drawer.h
@@ -2,11 +2,10 @@
#include <Eigen/Dense>
-#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include "GraspCandidateHelper.h"
-
#include "box_to_grasp_candidates.h"
namespace armarx
@@ -21,36 +20,51 @@ namespace armarx
std::string hand_model_path;
Eigen::Matrix4f tcp_2_hroot;
};
+
public:
- grasp_candidate_drawer(armarx::RobotNameHelperPtr rnh,
- const VirtualRobot::RobotPtr& robot);
+ grasp_candidate_drawer(armarx::RobotNameHelperPtr rnh, const VirtualRobot::RobotPtr& robot);
void reset_colors();
+
public:
void draw(const armarx::grasping::GraspCandidatePtr& gc, armarx::viz::Layer& l);
- void draw(const armarx::GraspCandidateHelper& gch, armarx::viz::Layer& l);
- void draw(const armarx::grasping::GraspCandidatePtr& gc, armarx::viz::Layer& l, const armarx::viz::Color& color);
- void draw(const armarx::GraspCandidateHelper& gch, armarx::viz::Layer& l, const armarx::viz::Color& color);
+ void draw(const armarx::GraspCandidateHelper& gch, armarx::viz::Layer& l);
+ void draw(const armarx::grasping::GraspCandidatePtr& gc,
+ armarx::viz::Layer& l,
+ const armarx::viz::Color& color);
+ void draw(const armarx::GraspCandidateHelper& gch,
+ armarx::viz::Layer& l,
+ const armarx::viz::Color& color);
+
public:
- void draw(const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
- float ax_len = 300, float ax_width = 5,
- const armarx::viz::Color& color_up = {255, 0, 0},
+ void draw(const box_to_grasp_candidates::side_data& side,
+ armarx::viz::Layer& l,
+ float ax_len = 300,
+ float ax_width = 5,
+ const armarx::viz::Color& color_up = {255, 0, 0},
const armarx::viz::Color& color_trans = {0, 255, 0});
void draw(const std::string& name,
- const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
- float ax_len = 300, float ax_width = 5,
- const armarx::viz::Color& color_up = {255, 0, 0},
+ const box_to_grasp_candidates::side_data& side,
+ armarx::viz::Layer& l,
+ float ax_len = 300,
+ float ax_width = 5,
+ const armarx::viz::Color& color_up = {255, 0, 0},
const armarx::viz::Color& color_trans = {0, 255, 0});
- void draw(const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
+ void draw(const box_to_grasp_candidates::side_data& side,
+ armarx::viz::Layer& l,
const Eigen::Matrix4f& gpose,
- float ax_len = 300, float ax_width = 5,
- const armarx::viz::Color& color_up = {255, 0, 0},
+ float ax_len = 300,
+ float ax_width = 5,
+ const armarx::viz::Color& color_up = {255, 0, 0},
const armarx::viz::Color& color_trans = {0, 255, 0});
void draw(const std::string& name,
- const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
+ const box_to_grasp_candidates::side_data& side,
+ armarx::viz::Layer& l,
const Eigen::Matrix4f& gpose,
- float ax_len = 300, float ax_width = 5,
- const armarx::viz::Color& color_up = {255, 0, 0},
+ float ax_len = 300,
+ float ax_width = 5,
+ const armarx::viz::Color& color_up = {255, 0, 0},
const armarx::viz::Color& color_trans = {0, 255, 0});
+
private:
const side_data& side(const armarx::GraspCandidateHelper& gch);
@@ -59,8 +73,9 @@ namespace armarx
VirtualRobot::RobotPtr robot;
float length_approach = 300;
float width_approach = 5;
+
private:
std::map<std::string, side_data> _sides;
std::size_t _color_offset = 0;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h b/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
index 57528416490797728886eb08a0ff7999bb4e4887..2838a6f8840cd4a873f18639ef7983bb4c0ec77c 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
@@ -140,8 +140,8 @@ namespace armarx::priorknowledge::core
virtual std::vector<FinderInfoType> findAll(const DatasetType& dataset) const = 0;
// Fix hidden virtual functions
- using Base::checkAll;
using Base::check;
+ using Base::checkAll;
using Base::find;
using Base::findAll;
};
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
index b8f0b9f6e0cf7a4ec7d64eab6a650c7afb90ad1b..8f36b3eb7867f41301ad029135efdb30cab329b8 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
@@ -22,6 +22,6 @@ namespace armarx::priorknowledge::util
const nlohmann::json&);
static std::vector<LocationAffordance> LoadLocationAffordances(const std::string& sourceId,
- const nlohmann::json&);
+ const nlohmann::json&);
};
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp
index 0329e3ba290bb0680b54a80272199fac52bacada..af944c162976fc535a4f7f5fe935b0d73d30199b 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp
@@ -12,7 +12,8 @@ namespace armarx::priorknowledge::util
std::vector<CommonPlace> ret;
if (not js.contains("common_places"))
{
- ARMARX_WARNING << "The common_places file (" << source << ") has the wrong structure. Missing key 'common_places'.";
+ ARMARX_WARNING << "The common_places file (" << source
+ << ") has the wrong structure. Missing key 'common_places'.";
return ret;
}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
index 8d5c5444aaf72999d8a10e6d84eb3d1026996ffa..7d3a336c3f7f605b97998ace262d73f2fac3bdaa 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
@@ -29,26 +29,32 @@ namespace armarx::priorknowledge::util::location
{
// Add global location to layer
std::smatch matches;
- if (std::regex_match(id, matches, pattern))
+ if (std::regex_match(id, matches, pattern))
{
const std::string& affordance_name = matches[1];
unique_affordances.insert(affordance_name);
- unsigned int index = std::distance(unique_affordances.begin(), unique_affordances.find(affordance_name))
- % simox::color::KellyLUT::KELLY_COLORS_COLOR_BLIND.size();
- layer.add(armarx::viz::Text(id + "_name").text(affordance_name)
- .color(simox::color::Color::black()).scale(10.0f).pose(locationGlobalPose));
- layer.add(armarx::viz::Box(id)
- .pose(locationGlobalPose)
- .size(extends)
- .color(simox::color::KellyLUT::KELLY_COLORS_COLOR_BLIND.at(index).with_alpha(0.5f)));
+ unsigned int index = std::distance(unique_affordances.begin(),
+ unique_affordances.find(affordance_name)) %
+ simox::color::KellyLUT::KELLY_COLORS_COLOR_BLIND.size();
+ layer.add(armarx::viz::Text(id + "_name")
+ .text(affordance_name)
+ .color(simox::color::Color::black())
+ .scale(10.0f)
+ .pose(locationGlobalPose));
+ layer.add(
+ armarx::viz::Box(id)
+ .pose(locationGlobalPose)
+ .size(extends)
+ .color(simox::color::KellyLUT::KELLY_COLORS_COLOR_BLIND.at(index).with_alpha(
+ 0.5f)));
}
- else
+ else
{
layer.add(armarx::viz::Text(id + "_name").text(id).pose(locationGlobalPose));
layer.add(armarx::viz::Box(id)
- .pose(locationGlobalPose)
- .size(extends)
- .color(this->settings.framedBoxedLocationColor));
+ .pose(locationGlobalPose)
+ .size(extends)
+ .color(this->settings.framedBoxedLocationColor));
}
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
index beb9ee13d8cbfc994cc8377ad43c7b11e851bcfc..7fa62982e15c4ace45c0dc25095fd634ac8e130c 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h
@@ -23,8 +23,7 @@ namespace armarx::plugins
std::string getStorageName();
};
-}
-
+} // namespace armarx::plugins
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -42,7 +41,8 @@ namespace armarx
armarx::viz::Client arviz;
- armarx::viz::Client& getArvizClient()
+ armarx::viz::Client&
+ getArvizClient()
{
return arviz;
}
@@ -50,11 +50,10 @@ namespace armarx
private:
armarx::plugins::ArVizComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx::plugins
{
// Legacy typedef.
using ArVizComponentPluginUser = armarx::ArVizComponentPluginUser;
-}
+} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
index 9d241c0c94832026b41a328aa215b7008e583db8..d7ed8da1ff83d2a5c85f7be2c25cddb1e6a00717 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.cpp
@@ -2,40 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void CartesianPositionControlComponentPlugin::preOnInitComponent()
+ void
+ CartesianPositionControlComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void CartesianPositionControlComponentPlugin::preOnConnectComponent()
+ void
+ CartesianPositionControlComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_cartesianPositionControl, PROPERTY_NAME);
}
- void CartesianPositionControlComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ CartesianPositionControlComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineRequiredProperty<std::string>(
- PROPERTY_NAME,
- "Name of the CartesianPositionControl");
+ PROPERTY_NAME, "Name of the CartesianPositionControl");
}
}
- CartesianPositionControlInterfacePrx CartesianPositionControlComponentPlugin::getCartesianPositionControl()
+ CartesianPositionControlInterfacePrx
+ CartesianPositionControlComponentPlugin::getCartesianPositionControl()
{
return _cartesianPositionControl;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -45,12 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- CartesianPositionControlInterfacePrx CartesianPositionControlComponentPluginUser::getCartesianPositionControl()
+ CartesianPositionControlInterfacePrx
+ CartesianPositionControlComponentPluginUser::getCartesianPositionControl()
{
return plugin->getCartesianPositionControl();
}
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
index 3a6dfebd06e39743a612ed9cfa3480fbd453d492..65e8d5b7d5eafdfbe04056e1716580731c837b45 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CartesianPositionControlComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotAPI/interface/components/CartesianPositionControlInterface.h>
+#include <RobotAPI/interface/components/CartesianPositionControlInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
CartesianPositionControlInterfacePrx _cartesianPositionControl;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,14 +47,14 @@ namespace armarx
armarx::plugins::CartesianPositionControlComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
namespace plugins
{
// Legacy typedef.
- using CartesianPositionControlComponentPluginUser = armarx::CartesianPositionControlComponentPluginUser;
- }
-}
+ using CartesianPositionControlComponentPluginUser =
+ armarx::CartesianPositionControlComponentPluginUser;
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp
index bd995800595c1715f71723f42a5c59dfee4dc063..bf328c2a1780fbfc51ae6f84f4df221fe116b63a 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.cpp
@@ -20,50 +20,54 @@
* GNU General Public License
*/
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/core/Component.h>
-
#include "DebugDrawerHelperComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx::plugins
{
- DebugDrawerHelperComponentPlugin::DebugDrawerHelperComponentPlugin(ManagedIceObject& parent, std::string pre) :
- ComponentPlugin(parent, pre),
- _pre{pre}
+ DebugDrawerHelperComponentPlugin::DebugDrawerHelperComponentPlugin(ManagedIceObject& parent,
+ std::string pre) :
+ ComponentPlugin(parent, pre), _pre{pre}
{
ARMARX_TRACE;
addPlugin(_robotStateComponentPlugin, pre);
addPluginDependency(_robotStateComponentPlugin);
}
- const DebugDrawerHelper& DebugDrawerHelperComponentPlugin::getDebugDrawerHelper() const
+ const DebugDrawerHelper&
+ DebugDrawerHelperComponentPlugin::getDebugDrawerHelper() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(_debugDrawerHelper);
return *_debugDrawerHelper;
}
- DebugDrawerHelper& DebugDrawerHelperComponentPlugin::getDebugDrawerHelper()
+ DebugDrawerHelper&
+ DebugDrawerHelperComponentPlugin::getDebugDrawerHelper()
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(_debugDrawerHelper);
return *_debugDrawerHelper;
}
- const DebugDrawerInterfacePrx& DebugDrawerHelperComponentPlugin::getDebugDrawerTopic() const
+ const DebugDrawerInterfacePrx&
+ DebugDrawerHelperComponentPlugin::getDebugDrawerTopic() const
{
ARMARX_TRACE;
return getDebugDrawerHelper().getDebugDrawer();
}
- std::unique_lock<std::recursive_mutex> DebugDrawerHelperComponentPlugin::getDebugDrawerHelperLock() const
+ std::unique_lock<std::recursive_mutex>
+ DebugDrawerHelperComponentPlugin::getDebugDrawerHelperLock() const
{
ARMARX_TRACE;
return std::unique_lock{_debugDrawerHelperMutex};
}
- std::string DebugDrawerHelperComponentPlugin::getDebugDrawerLayerName() const
+ std::string
+ DebugDrawerHelperComponentPlugin::getDebugDrawerLayerName() const
{
ARMARX_TRACE;
if (_pre.empty())
@@ -73,7 +77,8 @@ namespace armarx::plugins
return _pre + "_" + parent().getName();
}
- void DebugDrawerHelperComponentPlugin::preOnInitComponent()
+ void
+ DebugDrawerHelperComponentPlugin::preOnInitComponent()
{
ARMARX_TRACE;
if (!_debugDrawerTopic)
@@ -82,7 +87,8 @@ namespace armarx::plugins
}
}
- void DebugDrawerHelperComponentPlugin::preOnConnectComponent()
+ void
+ DebugDrawerHelperComponentPlugin::preOnConnectComponent()
{
ARMARX_TRACE;
if (!_debugDrawerTopic)
@@ -94,21 +100,19 @@ namespace armarx::plugins
const auto robotID = _pre + "_DebugDrawerHelperComponentPlugin_" + parent().getName();
if (!_robotStateComponentPlugin->hasRobot(robotID))
{
- _robotStateComponentPlugin->addRobot(robotID,
- VirtualRobot::RobotIO::eStructure);
+ _robotStateComponentPlugin->addRobot(robotID, VirtualRobot::RobotIO::eStructure);
}
const auto robot = _robotStateComponentPlugin->getRobot(robotID);
ARMARX_CHECK_NOT_NULL(_debugDrawerTopic);
ARMARX_CHECK_NOT_NULL(robot);
_debugDrawerHelper = std::make_unique<DebugDrawerHelper>(
- _debugDrawerTopic,
- getDebugDrawerLayerName(),
- robot);
+ _debugDrawerTopic, getDebugDrawerLayerName(), robot);
}
}
- void DebugDrawerHelperComponentPlugin::postOnDisconnectComponent()
+ void
+ DebugDrawerHelperComponentPlugin::postOnDisconnectComponent()
{
ARMARX_TRACE;
if (!_doNotClearLayersOnDisconnect)
@@ -119,30 +123,34 @@ namespace armarx::plugins
_debugDrawerHelper->cyclicCleanup();
_debugDrawerHelper->clearLayer();
}
- catch (...) {}
+ catch (...)
+ {
+ }
}
- _debugDrawerTopic = nullptr;
+ _debugDrawerTopic = nullptr;
}
- void DebugDrawerHelperComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ void
+ DebugDrawerHelperComponentPlugin::postCreatePropertyDefinitions(
+ PropertyDefinitionsPtr& properties)
{
ARMARX_TRACE;
if (!properties->hasDefinition(_propertyName))
{
properties->defineOptionalProperty<std::string>(
- _propertyName,
- "DebugDrawerUpdates",
- "Name of the debug drawer topic");
+ _propertyName, "DebugDrawerUpdates", "Name of the debug drawer topic");
}
}
- VirtualRobot::RobotPtr DebugDrawerHelperComponentPlugin::getDebugDrawerHelperRobot() const
+ VirtualRobot::RobotPtr
+ DebugDrawerHelperComponentPlugin::getDebugDrawerHelperRobot() const
{
ARMARX_TRACE;
return _debugDrawerHelper->getRobot();
}
- bool DebugDrawerHelperComponentPlugin::synchronizeDebugDrawerHelperRobot() const
+ bool
+ DebugDrawerHelperComponentPlugin::synchronizeDebugDrawerHelperRobot() const
{
ARMARX_TRACE;
const auto g = getDebugDrawerHelperLock();
@@ -150,28 +158,34 @@ namespace armarx::plugins
return _robotStateComponentPlugin->synchronizeLocalClone(getDebugDrawerHelperRobot());
}
- bool DebugDrawerHelperComponentPlugin::synchronizeDebugDrawerHelperRobot(Ice::Long timestamp) const
+ bool
+ DebugDrawerHelperComponentPlugin::synchronizeDebugDrawerHelperRobot(Ice::Long timestamp) const
{
ARMARX_TRACE;
const auto g = getDebugDrawerHelperLock();
ARMARX_CHECK_NOT_NULL(getDebugDrawerHelperRobot());
- return _robotStateComponentPlugin->synchronizeLocalClone(getDebugDrawerHelperRobot(), timestamp);
+ return _robotStateComponentPlugin->synchronizeLocalClone(getDebugDrawerHelperRobot(),
+ timestamp);
}
- bool DebugDrawerHelperComponentPlugin::synchronizeDebugDrawerHelperRobot(const RobotStateConfig& state) const
+ bool
+ DebugDrawerHelperComponentPlugin::synchronizeDebugDrawerHelperRobot(
+ const RobotStateConfig& state) const
{
ARMARX_TRACE;
const auto g = getDebugDrawerHelperLock();
ARMARX_CHECK_NOT_NULL(getDebugDrawerHelperRobot());
- return _robotStateComponentPlugin->synchronizeLocalClone(getDebugDrawerHelperRobot(), state);
+ return _robotStateComponentPlugin->synchronizeLocalClone(getDebugDrawerHelperRobot(),
+ state);
}
- void DebugDrawerHelperComponentPlugin::setClearLayersOnDisconnect(bool b)
+ void
+ DebugDrawerHelperComponentPlugin::setClearLayersOnDisconnect(bool b)
{
ARMARX_TRACE;
_doNotClearLayersOnDisconnect = !b;
}
-}
+} // namespace armarx::plugins
namespace armarx
{
@@ -181,66 +195,82 @@ namespace armarx
addPlugin(_debugDrawerHelperComponentPlugin);
}
- const DebugDrawerHelperComponentPluginUser::DebugDrawerHelperComponentPlugin& DebugDrawerHelperComponentPluginUser::getDebugDrawerHelperPlugin() const
+ const DebugDrawerHelperComponentPluginUser::DebugDrawerHelperComponentPlugin&
+ DebugDrawerHelperComponentPluginUser::getDebugDrawerHelperPlugin() const
{
ARMARX_TRACE;
return *_debugDrawerHelperComponentPlugin;
}
- DebugDrawerHelperComponentPluginUser::DebugDrawerHelperComponentPlugin& DebugDrawerHelperComponentPluginUser::getDebugDrawerHelperPlugin()
+ DebugDrawerHelperComponentPluginUser::DebugDrawerHelperComponentPlugin&
+ DebugDrawerHelperComponentPluginUser::getDebugDrawerHelperPlugin()
{
ARMARX_TRACE;
return *_debugDrawerHelperComponentPlugin;
}
- const DebugDrawerHelper& DebugDrawerHelperComponentPluginUser::getDebugDrawerHelper() const
+ const DebugDrawerHelper&
+ DebugDrawerHelperComponentPluginUser::getDebugDrawerHelper() const
{
ARMARX_TRACE;
return getDebugDrawerHelperPlugin().getDebugDrawerHelper();
}
- DebugDrawerHelper& DebugDrawerHelperComponentPluginUser::getDebugDrawerHelper()
+ DebugDrawerHelper&
+ DebugDrawerHelperComponentPluginUser::getDebugDrawerHelper()
{
ARMARX_TRACE;
return getDebugDrawerHelperPlugin().getDebugDrawerHelper();
}
- const DebugDrawerInterfacePrx& DebugDrawerHelperComponentPluginUser::getDebugDrawerTopic() const
+ const DebugDrawerInterfacePrx&
+ DebugDrawerHelperComponentPluginUser::getDebugDrawerTopic() const
{
ARMARX_TRACE;
return getDebugDrawerHelperPlugin().getDebugDrawerTopic();
}
- std::unique_lock<std::recursive_mutex> DebugDrawerHelperComponentPluginUser::getDebugDrawerHelperLock() const
+ std::unique_lock<std::recursive_mutex>
+ DebugDrawerHelperComponentPluginUser::getDebugDrawerHelperLock() const
{
ARMARX_TRACE;
return getDebugDrawerHelperPlugin().getDebugDrawerHelperLock();
}
- std::string DebugDrawerHelperComponentPluginUser::getDebugDrawerLayerName() const
+
+ std::string
+ DebugDrawerHelperComponentPluginUser::getDebugDrawerLayerName() const
{
ARMARX_TRACE;
return getDebugDrawerHelperPlugin().getDebugDrawerLayerName();
}
- VirtualRobot::RobotPtr DebugDrawerHelperComponentPluginUser::getDebugDrawerHelperRobot() const
+ VirtualRobot::RobotPtr
+ DebugDrawerHelperComponentPluginUser::getDebugDrawerHelperRobot() const
{
ARMARX_TRACE;
return getDebugDrawerHelperPlugin().getDebugDrawerHelperRobot();
}
- bool DebugDrawerHelperComponentPluginUser::synchronizeDebugDrawerHelperRobot() const
+ bool
+ DebugDrawerHelperComponentPluginUser::synchronizeDebugDrawerHelperRobot() const
{
ARMARX_TRACE;
return getDebugDrawerHelperPlugin().synchronizeDebugDrawerHelperRobot();
}
- bool DebugDrawerHelperComponentPluginUser::synchronizeDebugDrawerHelperRobot(Ice::Long timestamp) const
+
+ bool
+ DebugDrawerHelperComponentPluginUser::synchronizeDebugDrawerHelperRobot(
+ Ice::Long timestamp) const
{
ARMARX_TRACE;
return getDebugDrawerHelperPlugin().synchronizeDebugDrawerHelperRobot(timestamp);
}
- bool DebugDrawerHelperComponentPluginUser::synchronizeDebugDrawerHelperRobot(const RobotStateConfig& state) const
+
+ bool
+ DebugDrawerHelperComponentPluginUser::synchronizeDebugDrawerHelperRobot(
+ const RobotStateConfig& state) const
{
ARMARX_TRACE;
return getDebugDrawerHelperPlugin().synchronizeDebugDrawerHelperRobot(state);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h
index 59ed86bb95d3c66aa02c6ad935066e8d97fc53e3..204017335d8329d0a2d72967e06a311d3d157a0e 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/DebugDrawerHelperComponentPlugin.h
@@ -72,16 +72,15 @@ namespace armarx::plugins
//data
private:
using RobotStateComponentPlugin = armarx::plugins::RobotStateComponentPlugin;
- static constexpr auto _propertyName = "DebugDrawerTopicName";
- std::string _pre;
- mutable std::recursive_mutex _debugDrawerHelperMutex;
- std::unique_ptr<DebugDrawerHelper> _debugDrawerHelper;
- DebugDrawerInterfacePrx _debugDrawerTopic;
- RobotStateComponentPlugin* _robotStateComponentPlugin{nullptr};
- bool _doNotClearLayersOnDisconnect = false;
+ static constexpr auto _propertyName = "DebugDrawerTopicName";
+ std::string _pre;
+ mutable std::recursive_mutex _debugDrawerHelperMutex;
+ std::unique_ptr<DebugDrawerHelper> _debugDrawerHelper;
+ DebugDrawerInterfacePrx _debugDrawerTopic;
+ RobotStateComponentPlugin* _robotStateComponentPlugin{nullptr};
+ bool _doNotClearLayersOnDisconnect = false;
};
-}
-
+} // namespace armarx::plugins
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -91,8 +90,10 @@ namespace armarx
{
public:
using DebugDrawerHelperComponentPlugin = armarx::plugins::DebugDrawerHelperComponentPlugin;
+
private:
DebugDrawerHelperComponentPlugin* _debugDrawerHelperComponentPlugin{nullptr};
+
public:
DebugDrawerHelperComponentPluginUser();
@@ -115,4 +116,4 @@ namespace armarx
bool synchronizeDebugDrawerHelperRobot(Ice::Long timestamp) const;
bool synchronizeDebugDrawerHelperRobot(const RobotStateConfig& state) const;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
index 1ca9b39590f7f49df239495cce72356223b70514..7b12b9a8daf94773e58772337608288f057a8e6c 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
@@ -2,41 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void FrameTrackingComponentPlugin::preOnInitComponent()
+ void
+ FrameTrackingComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void FrameTrackingComponentPlugin::preOnConnectComponent()
+ void
+ FrameTrackingComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_frameTracking, PROPERTY_NAME);
}
- void FrameTrackingComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ FrameTrackingComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "FrameTracking",
- "Name of the FrameTracking");
+ PROPERTY_NAME, "FrameTracking", "Name of the FrameTracking");
}
}
- FrameTrackingInterfacePrx FrameTrackingComponentPlugin::getFrameTracking()
+ FrameTrackingInterfacePrx
+ FrameTrackingComponentPlugin::getFrameTracking()
{
return _frameTracking;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -46,12 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- FrameTrackingInterfacePrx FrameTrackingComponentPluginUser::getFrameTracking()
+ FrameTrackingInterfacePrx
+ FrameTrackingComponentPluginUser::getFrameTracking()
{
return plugin->getFrameTracking();
}
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
index 8364375ea697ba9129d3699edc00c0dede740810..8650993686e7be3a419be55e7120d5f6d353f2ee 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotAPI/interface/components/FrameTrackingInterface.h>
+#include <RobotAPI/interface/components/FrameTrackingInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
FrameTrackingInterfacePrx _frameTracking;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,8 +47,7 @@ namespace armarx
armarx::plugins::FrameTrackingComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
@@ -57,5 +55,5 @@ namespace armarx
{
// Legacy typedef.
using FrameTrackingComponentPluginUser = armarx::FrameTrackingComponentPluginUser;
- }
-}
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
index 6ab08acaee80d8896636f3c16c81bb38822365a5..7807672615d71af6515e1ab6e61ab708cc4b264f 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.cpp
@@ -1,20 +1,22 @@
+#include "GraspCandidateObserverComponentPlugin.h"
+
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <ArmarXCore/core/Component.h>
-#include "GraspCandidateObserverComponentPlugin.h"
-
namespace armarx
{
namespace plugins
{
- void GraspCandidateObserverComponentPlugin::preOnInitComponent()
+ void
+ GraspCandidateObserverComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
if (!_graspCandidateObserver && _graspCandidateObserverName.empty())
{
- parent<Component>().getProperty(_graspCandidateObserverName, makePropertyName(PROPERTY_NAME));
+ parent<Component>().getProperty(_graspCandidateObserverName,
+ makePropertyName(PROPERTY_NAME));
}
if (!_graspCandidateObserver)
@@ -23,7 +25,8 @@ namespace armarx
}
}
- void GraspCandidateObserverComponentPlugin::preOnConnectComponent()
+ void
+ GraspCandidateObserverComponentPlugin::preOnConnectComponent()
{
if (!_graspCandidateObserver)
{
@@ -31,33 +34,37 @@ namespace armarx
}
}
- void GraspCandidateObserverComponentPlugin::postOnDisconnectComponent()
+ void
+ GraspCandidateObserverComponentPlugin::postOnDisconnectComponent()
{
_graspCandidateObserver = nullptr;
}
- void GraspCandidateObserverComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ GraspCandidateObserverComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "GraspCandidateObserver",
- "Name of the GraspCandidateObserver");
+ PROPERTY_NAME, "GraspCandidateObserver", "Name of the GraspCandidateObserver");
}
}
- grasping::GraspCandidateObserverInterfacePrx GraspCandidateObserverComponentPlugin::getGraspCandidateObserver()
+ grasping::GraspCandidateObserverInterfacePrx
+ GraspCandidateObserverComponentPlugin::getGraspCandidateObserver()
{
return _graspCandidateObserver;
}
- const std::string& GraspCandidateObserverComponentPlugin::getGraspCandidateObserverName() const
+ const std::string&
+ GraspCandidateObserverComponentPlugin::getGraspCandidateObserverName() const
{
return _graspCandidateObserverName;
}
- grasping::GraspCandidateSeq GraspCandidateObserverComponentPlugin::getAllCandidates() const
+ grasping::GraspCandidateSeq
+ GraspCandidateObserverComponentPlugin::getAllCandidates() const
{
if (_graspCandidateObserver)
{
@@ -66,7 +73,8 @@ namespace armarx
return {};
}
- grasping::GraspCandidateSeq GraspCandidateObserverComponentPlugin::getCandidates() const
+ grasping::GraspCandidateSeq
+ GraspCandidateObserverComponentPlugin::getCandidates() const
{
if (!_graspCandidateObserver)
{
@@ -79,22 +87,29 @@ namespace armarx
return _graspCandidateObserver->getCandidatesByProviders(_upstream_providers);
}
- void GraspCandidateObserverComponentPlugin::setUpstreamGraspCandidateProviders(std::vector<std::string> pr)
+ void
+ GraspCandidateObserverComponentPlugin::setUpstreamGraspCandidateProviders(
+ std::vector<std::string> pr)
{
_upstream_providers = std::move(pr);
}
- void GraspCandidateObserverComponentPlugin::setUpstreamGraspCandidateProvidersFromCSV(const std::string& csv, const std::string& delim)
+ void
+ GraspCandidateObserverComponentPlugin::setUpstreamGraspCandidateProvidersFromCSV(
+ const std::string& csv,
+ const std::string& delim)
{
setUpstreamGraspCandidateProviders(simox::alg::split(csv, delim));
}
- void GraspCandidateObserverComponentPlugin::setGraspCandidateObserverName(const std::string& name)
+ void
+ GraspCandidateObserverComponentPlugin::setGraspCandidateObserverName(
+ const std::string& name)
{
_graspCandidateObserverName = name;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -103,30 +118,35 @@ namespace armarx
addPlugin(plugin);
}
- grasping::GraspCandidateObserverInterfacePrx GraspCandidateObserverComponentPluginUser::getGraspCandidateObserver()
+ grasping::GraspCandidateObserverInterfacePrx
+ GraspCandidateObserverComponentPluginUser::getGraspCandidateObserver()
{
return plugin->getGraspCandidateObserver();
}
- plugins::GraspCandidateObserverComponentPlugin& GraspCandidateObserverComponentPluginUser::getGraspCandidateObserverComponentPlugin()
+ plugins::GraspCandidateObserverComponentPlugin&
+ GraspCandidateObserverComponentPluginUser::getGraspCandidateObserverComponentPlugin()
{
ARMARX_CHECK_NOT_NULL(plugin);
return *plugin;
}
- const plugins::GraspCandidateObserverComponentPlugin& GraspCandidateObserverComponentPluginUser::getGraspCandidateObserverComponentPlugin() const
+ const plugins::GraspCandidateObserverComponentPlugin&
+ GraspCandidateObserverComponentPluginUser::getGraspCandidateObserverComponentPlugin() const
{
ARMARX_CHECK_NOT_NULL(plugin);
return *plugin;
}
- grasping::GraspCandidateSeq GraspCandidateObserverComponentPluginUser::getAllGraspCandidates() const
+ grasping::GraspCandidateSeq
+ GraspCandidateObserverComponentPluginUser::getAllGraspCandidates() const
{
return getGraspCandidateObserverComponentPlugin().getAllCandidates();
}
- grasping::GraspCandidateSeq GraspCandidateObserverComponentPluginUser::getGraspCandidates() const
+ grasping::GraspCandidateSeq
+ GraspCandidateObserverComponentPluginUser::getGraspCandidates() const
{
return getGraspCandidateObserverComponentPlugin().getCandidates();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
index a47f7877514702b1d4c719cb87947fea09ed5c90..c42be762556801236762f2eaa1ce45879c5ea583 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/GraspCandidateObserverComponentPlugin.h
@@ -1,6 +1,7 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
+
#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
namespace armarx
@@ -12,8 +13,8 @@ namespace armarx
public:
using ComponentPlugin::ComponentPlugin;
- void preOnInitComponent() override;
- void preOnConnectComponent() override;
+ void preOnInitComponent() override;
+ void preOnConnectComponent() override;
void postOnDisconnectComponent() override;
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
@@ -26,16 +27,17 @@ namespace armarx
grasping::GraspCandidateSeq getAllCandidates() const;
grasping::GraspCandidateSeq getCandidates() const;
void setUpstreamGraspCandidateProviders(std::vector<std::string> pr);
- void setUpstreamGraspCandidateProvidersFromCSV(const std::string& csv, const std::string& delim = ",;");
+ void setUpstreamGraspCandidateProvidersFromCSV(const std::string& csv,
+ const std::string& delim = ",;");
+
private:
static constexpr const char* PROPERTY_NAME = "GraspCandidateObserverName";
- std::string _graspCandidateObserverName;
+ std::string _graspCandidateObserverName;
grasping::GraspCandidateObserverInterfacePrx _graspCandidateObserver;
- std::vector<std::string> _upstream_providers;
+ std::vector<std::string> _upstream_providers;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -50,22 +52,25 @@ namespace armarx
GraspCandidateObserverComponentPluginUser();
grasping::GraspCandidateObserverInterfacePrx getGraspCandidateObserver();
- armarx::plugins::GraspCandidateObserverComponentPlugin& getGraspCandidateObserverComponentPlugin();
- const armarx::plugins::GraspCandidateObserverComponentPlugin& getGraspCandidateObserverComponentPlugin() const;
+ armarx::plugins::GraspCandidateObserverComponentPlugin&
+ getGraspCandidateObserverComponentPlugin();
+ const armarx::plugins::GraspCandidateObserverComponentPlugin&
+ getGraspCandidateObserverComponentPlugin() const;
grasping::GraspCandidateSeq getAllGraspCandidates() const;
grasping::GraspCandidateSeq getGraspCandidates() const;
+
private:
armarx::plugins::GraspCandidateObserverComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
namespace plugins
{
// Legacy typedef.
- using GraspCandidateObserverComponentPluginUser = armarx::GraspCandidateObserverComponentPluginUser;
- }
-}
+ using GraspCandidateObserverComponentPluginUser =
+ armarx::GraspCandidateObserverComponentPluginUser;
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
index df3ed14e186ef4bced826f2e4cc9e581529ae5af..a69e0e99489f150344bdf42353d99be35770263a 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
@@ -2,41 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void HandUnitComponentPlugin::preOnInitComponent()
+ void
+ HandUnitComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void HandUnitComponentPlugin::preOnConnectComponent()
+ void
+ HandUnitComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_handUnit, PROPERTY_NAME);
}
- void HandUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ HandUnitComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "HandUnit",
- "Name of the HandUnit");
+ PROPERTY_NAME, "HandUnit", "Name of the HandUnit");
}
}
- HandUnitInterfacePrx HandUnitComponentPlugin::getHandUnit()
+ HandUnitInterfacePrx
+ HandUnitComponentPlugin::getHandUnit()
{
return _handUnit;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -46,12 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- HandUnitInterfacePrx HandUnitComponentPluginUser::getHandUnit()
+ HandUnitInterfacePrx
+ HandUnitComponentPluginUser::getHandUnit()
{
return plugin->getHandUnit();
}
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
index 6cfe35ef754764c1c657ddefd7ec5bee811339c0..c11e131a2012b3f70c6e85f63dbbaf66a9a4a2e2 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotAPI/interface/units/HandUnitInterface.h>
+#include <RobotAPI/interface/units/HandUnitInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
HandUnitInterfacePrx _handUnit;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,8 +47,7 @@ namespace armarx
armarx::plugins::HandUnitComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
@@ -57,5 +55,5 @@ namespace armarx
{
// Legacy typedef.
using HandUnitComponentPluginUser = armarx::HandUnitComponentPluginUser;
- }
-}
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
index 9c1d995dee640e413d178d900df769068f1746df..5fa2a0e204271a65f444addac901b82fe6061242 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -139,8 +139,10 @@ namespace armarx::plugins
{
if (!properties->hasDefinition(makePropertyName(healthPropertyName)))
{
- properties->component(
- robotHealthComponentPrx, "RobotHealth", healthPropertyName, "Name of the robot health component.");
+ properties->component(robotHealthComponentPrx,
+ "RobotHealth",
+ healthPropertyName,
+ "Name of the robot health component.");
}
if (not properties->hasDefinition(makePropertyName(maximumCycleTimeWarningMSPropertyName)))
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
index 59dc918a1be3688d70795a5175c90ca28c5da099..2072e5ab01b35804323c33435def556250e7a648 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.cpp
@@ -2,41 +2,44 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void KinematicUnitComponentPlugin::preOnInitComponent()
+ void
+ KinematicUnitComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void KinematicUnitComponentPlugin::preOnConnectComponent()
+ void
+ KinematicUnitComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_kinematicUnit, PROPERTY_NAME);
}
- void KinematicUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ KinematicUnitComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
- properties->defineRequiredProperty<std::string>(
- PROPERTY_NAME,
- "Name of the KinematicUnit");
+ properties->defineRequiredProperty<std::string>(PROPERTY_NAME,
+ "Name of the KinematicUnit");
}
}
- KinematicUnitInterfacePrx KinematicUnitComponentPlugin::getKinematicUnit()
+ KinematicUnitInterfacePrx
+ KinematicUnitComponentPlugin::getKinematicUnit()
{
return _kinematicUnit;
}
- }
+ } // namespace plugins
-}
+} // namespace armarx
namespace armarx
{
@@ -46,14 +49,11 @@ namespace armarx
addPlugin(plugin);
}
- KinematicUnitInterfacePrx KinematicUnitComponentPluginUser::getKinematicUnit()
+ KinematicUnitInterfacePrx
+ KinematicUnitComponentPluginUser::getKinematicUnit()
{
return plugin->getKinematicUnit();
}
-
-
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
index 2b3486826b6b84883e28c86598f3e05ea04c2674..34ae3d7aed5e74a29c0e8a72a2f003911d6a5944 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/KinematicUnitComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
KinematicUnitInterfacePrx _kinematicUnit;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -49,8 +48,7 @@ namespace armarx
};
-}
-
+} // namespace armarx
namespace armarx
{
@@ -58,5 +56,5 @@ namespace armarx
{
// Legacy typedef.
using KinematicUnitComponentPluginUser = armarx::KinematicUnitComponentPluginUser;
- }
-}
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
index fd592a9250cb0bb99caef834243ff4cf5bbaab23..cd18502b45c688c487e16139dbb2441e88ffe9b4 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.cpp
@@ -2,41 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void NaturalIKComponentPlugin::preOnInitComponent()
+ void
+ NaturalIKComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void NaturalIKComponentPlugin::preOnConnectComponent()
+ void
+ NaturalIKComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_naturalIK, PROPERTY_NAME);
}
- void NaturalIKComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ NaturalIKComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "NaturalIK",
- "Name of the NaturalIK");
+ PROPERTY_NAME, "NaturalIK", "Name of the NaturalIK");
}
}
- NaturalIKInterfacePrx NaturalIKComponentPlugin::getNaturalIK()
+ NaturalIKInterfacePrx
+ NaturalIKComponentPlugin::getNaturalIK()
{
return _naturalIK;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -46,12 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- NaturalIKInterfacePrx NaturalIKComponentPluginUser::getNaturalIK()
+ NaturalIKInterfacePrx
+ NaturalIKComponentPluginUser::getNaturalIK()
{
return plugin->getNaturalIK();
}
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
index c7e1c9cf61c21bfa986b3a0c827a6b371aba3048..2718a246fdf44af9527a2e100f8a52ee6c37aa30 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/NaturalIKComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotAPI/interface/components/NaturalIKInterface.h>
+#include <RobotAPI/interface/components/NaturalIKInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
NaturalIKInterfacePrx _naturalIK;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,8 +47,7 @@ namespace armarx
armarx::plugins::NaturalIKComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
@@ -57,5 +55,5 @@ namespace armarx
{
// Legacy typedef.
using NaturalIKComponentPluginUser = armarx::NaturalIKComponentPluginUser;
- }
-}
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
index affec3b906414a2616c3e786aa0f88e6fa7b4d44..2070739f99bd9d39652cd4960a494ead4de63d6e 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.cpp
@@ -2,40 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void PlatformUnitComponentPlugin::preOnInitComponent()
+ void
+ PlatformUnitComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void PlatformUnitComponentPlugin::preOnConnectComponent()
+ void
+ PlatformUnitComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_platformUnit, PROPERTY_NAME);
}
- void PlatformUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ PlatformUnitComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
- properties->defineRequiredProperty<std::string>(
- PROPERTY_NAME,
- "Name of the PlatformUnit");
+ properties->defineRequiredProperty<std::string>(PROPERTY_NAME,
+ "Name of the PlatformUnit");
}
}
- PlatformUnitInterfacePrx PlatformUnitComponentPlugin::getPlatformUnit()
+ PlatformUnitInterfacePrx
+ PlatformUnitComponentPlugin::getPlatformUnit()
{
return _platformUnit;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -45,12 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- PlatformUnitInterfacePrx PlatformUnitComponentPluginUser::getPlatformUnit()
+ PlatformUnitInterfacePrx
+ PlatformUnitComponentPluginUser::getPlatformUnit()
{
return plugin->getPlatformUnit();
}
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
index 67b0edbc320bf724e321e2c32631b37ac5ebc92f..30a7638080e67aad18cf2eef762356f0e2184f7a 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/PlatformUnitComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
PlatformUnitInterfacePrx _platformUnit;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,8 +47,7 @@ namespace armarx
armarx::plugins::PlatformUnitComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
@@ -57,5 +55,5 @@ namespace armarx
{
// Legacy typedef.
using PlatformUnitComponentPluginUser = armarx::PlatformUnitComponentPluginUser;
- }
-}
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index 38b5f30cd0d88591fa518fcb11fab3df7357424e..f85755dc7f9ab2477d49263f74a9bdfb9a399873 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -20,39 +20,46 @@
* GNU General Public License
*/
-#include <ArmarXCore/core/Component.h>
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
+#include "RobotStateComponentPlugin.h"
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include "RobotStateComponentPlugin.h"
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
namespace armarx::plugins
{
- RobotNameHelperPtr RobotStateComponentPlugin::getRobotNameHelper() const
+ RobotNameHelperPtr
+ RobotStateComponentPlugin::getRobotNameHelper() const
{
ARMARX_CHECK_NOT_NULL(_nameHelper);
return _nameHelper;
}
- void RobotStateComponentPlugin::setRobotStateComponent(const RobotStateComponentInterfacePrx& rsc)
+
+ void
+ RobotStateComponentPlugin::setRobotStateComponent(const RobotStateComponentInterfacePrx& rsc)
{
ARMARX_CHECK_NOT_NULL(rsc);
ARMARX_CHECK_NULL(_robotStateComponent);
_robotStateComponent = rsc;
}
- bool RobotStateComponentPlugin::hasRobot(const std::string& id) const
+ bool
+ RobotStateComponentPlugin::hasRobot(const std::string& id) const
{
- std::lock_guard {_robotsMutex};
+ std::lock_guard{_robotsMutex};
return _robots.count(id);
}
- VirtualRobot::RobotPtr RobotStateComponentPlugin::addRobot(const std::string& id, const VirtualRobot::RobotPtr& robot, const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& node)
+ VirtualRobot::RobotPtr
+ RobotStateComponentPlugin::addRobot(const std::string& id,
+ const VirtualRobot::RobotPtr& robot,
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& node)
{
ARMARX_CHECK_NOT_NULL(robot);
if (rns)
@@ -63,9 +70,9 @@ namespace armarx::plugins
{
ARMARX_CHECK_EXPRESSION(robot == node->getRobot());
}
- std::lock_guard {_robotsMutex};
+ std::lock_guard{_robotsMutex};
ARMARX_CHECK_EXPRESSION(!hasRobot(id)) << "Robot with id '" << id << "' was already added";
- if (rns && ! node)
+ if (rns && !node)
{
_robots[id] = {robot, rns, rns->getTCP()};
}
@@ -76,7 +83,11 @@ namespace armarx::plugins
return robot;
}
- VirtualRobot::RobotPtr RobotStateComponentPlugin::addRobot(const std::string& id, const VirtualRobot::RobotPtr& robot, const std::string& rnsName, const std::string& nodeName)
+ VirtualRobot::RobotPtr
+ RobotStateComponentPlugin::addRobot(const std::string& id,
+ const VirtualRobot::RobotPtr& robot,
+ const std::string& rnsName,
+ const std::string& nodeName)
{
ARMARX_CHECK_NOT_NULL(robot) << "robot added with id '" << id << "' is null";
VirtualRobot::RobotNodeSetPtr rns;
@@ -100,39 +111,58 @@ namespace armarx::plugins
return addRobot(id, robot, rns, node);
}
- VirtualRobot::RobotPtr RobotStateComponentPlugin::addRobot(const std::string& id, VirtualRobot::RobotIO::RobotDescription loadMode, const std::string& rnsName, const std::string& nodeName)
+ VirtualRobot::RobotPtr
+ RobotStateComponentPlugin::addRobot(const std::string& id,
+ VirtualRobot::RobotIO::RobotDescription loadMode,
+ const std::string& rnsName,
+ const std::string& nodeName)
{
return addRobot(id,
RemoteRobot::createLocalCloneFromFile(_robotStateComponent, loadMode),
- rnsName, nodeName);
+ rnsName,
+ nodeName);
}
- VirtualRobot::RobotPtr RobotStateComponentPlugin::addRobot(const std::string& id, const std::string& filename, const Ice::StringSeq packages, VirtualRobot::RobotIO::RobotDescription loadMode, const std::string& rnsName, const std::string& nodeName)
+ VirtualRobot::RobotPtr
+ RobotStateComponentPlugin::addRobot(const std::string& id,
+ const std::string& filename,
+ const Ice::StringSeq packages,
+ VirtualRobot::RobotIO::RobotDescription loadMode,
+ const std::string& rnsName,
+ const std::string& nodeName)
{
- return addRobot(id,
- RemoteRobot::createLocalClone(_robotStateComponent, filename, packages, loadMode),
- rnsName, nodeName);
+ return addRobot(
+ id,
+ RemoteRobot::createLocalClone(_robotStateComponent, filename, packages, loadMode),
+ rnsName,
+ nodeName);
}
- VirtualRobot::RobotPtr RobotStateComponentPlugin::getRobot(const std::string& id) const
+ VirtualRobot::RobotPtr
+ RobotStateComponentPlugin::getRobot(const std::string& id) const
{
return getRobotData(id).robot;
}
- RobotStateComponentPlugin::RobotData RobotStateComponentPlugin::getRobotData(const std::string& id) const
+ RobotStateComponentPlugin::RobotData
+ RobotStateComponentPlugin::getRobotData(const std::string& id) const
{
- std::lock_guard {_robotsMutex};
+ std::lock_guard{_robotsMutex};
ARMARX_CHECK_EXPRESSION(hasRobot(id)) << "No robot with id '" << id << "' loaded";
return _robots.at(id);
}
- void RobotStateComponentPlugin::setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName)
+ void
+ RobotStateComponentPlugin::setRobotRNSAndNode(const std::string& id,
+ const std::string& rnsName,
+ const std::string& nodeName)
{
- std::lock_guard {_robotsMutex};
+ std::lock_guard{_robotsMutex};
ARMARX_CHECK_EXPRESSION(hasRobot(id)) << "No robot with id '" << id << "' loaded";
auto& r = _robots.at(id);
- ARMARX_CHECK_EXPRESSION(!rnsName.empty()) << "There was no robot node set specified! "
- << "Available RNS: " << r.robot->getRobotNodeSetNames();
+ ARMARX_CHECK_EXPRESSION(!rnsName.empty())
+ << "There was no robot node set specified! "
+ << "Available RNS: " << r.robot->getRobotNodeSetNames();
auto rns = r.robot->getRobotNodeSet(rnsName);
ARMARX_CHECK_NOT_NULL(rns) << "The robot has no node set with name '" << rnsName
<< "'. Available RNS: " << r.robot->getRobotNodeSetNames();
@@ -151,65 +181,83 @@ namespace armarx::plugins
}
}
- const RobotStateComponentInterfacePrx& RobotStateComponentPlugin::getRobotStateComponent() const
+ const RobotStateComponentInterfacePrx&
+ RobotStateComponentPlugin::getRobotStateComponent() const
{
return _robotStateComponent;
}
- bool RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const
+ bool
+ RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const
{
return RemoteRobot::synchronizeLocalClone(robot, _robotStateComponent);
}
- bool RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, Ice::Long timestamp) const
+ bool
+ RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot,
+ Ice::Long timestamp) const
{
- return RemoteRobot::synchronizeLocalCloneToTimestamp(robot, _robotStateComponent, timestamp);
+ return RemoteRobot::synchronizeLocalCloneToTimestamp(
+ robot, _robotStateComponent, timestamp);
}
- bool RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, const RobotStateConfig& state) const
+ bool
+ RobotStateComponentPlugin::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot,
+ const RobotStateConfig& state) const
{
return RemoteRobot::synchronizeLocalCloneToState(robot, state);
}
- bool RobotStateComponentPlugin::synchronizeLocalClone(const RobotData& rdata) const
+ bool
+ RobotStateComponentPlugin::synchronizeLocalClone(const RobotData& rdata) const
{
return synchronizeLocalClone(rdata.robot);
}
- bool RobotStateComponentPlugin::synchronizeLocalClone(const RobotData& rdata, Ice::Long timestamp) const
+ bool
+ RobotStateComponentPlugin::synchronizeLocalClone(const RobotData& rdata,
+ Ice::Long timestamp) const
{
return synchronizeLocalClone(rdata.robot, timestamp);
}
- bool RobotStateComponentPlugin::synchronizeLocalClone(const RobotData& rdata, const RobotStateConfig& state) const
+ bool
+ RobotStateComponentPlugin::synchronizeLocalClone(const RobotData& rdata,
+ const RobotStateConfig& state) const
{
return synchronizeLocalClone(rdata.robot, state);
}
- bool RobotStateComponentPlugin::synchronizeLocalClone(const std::string& id) const
+ bool
+ RobotStateComponentPlugin::synchronizeLocalClone(const std::string& id) const
{
return synchronizeLocalClone(getRobot(id));
}
- bool RobotStateComponentPlugin::synchronizeLocalClone(const std::string& id, Ice::Long timestamp) const
+ bool
+ RobotStateComponentPlugin::synchronizeLocalClone(const std::string& id,
+ Ice::Long timestamp) const
{
return synchronizeLocalClone(getRobot(id), timestamp);
}
- bool RobotStateComponentPlugin::synchronizeLocalClone(const std::string& id, const RobotStateConfig& state) const
+ bool
+ RobotStateComponentPlugin::synchronizeLocalClone(const std::string& id,
+ const RobotStateConfig& state) const
{
return synchronizeLocalClone(getRobot(id), state);
}
- SimpleDiffIK::Result RobotStateComponentPlugin::calculateRobotDiffIK(
- const std::string& id,
- const Eigen::Matrix4f& targetPose,
- const SimpleDiffIK::Parameters& params) const
+ SimpleDiffIK::Result
+ RobotStateComponentPlugin::calculateRobotDiffIK(const std::string& id,
+ const Eigen::Matrix4f& targetPose,
+ const SimpleDiffIK::Parameters& params) const
{
return getRobotData(id).calculateRobotDiffIK(targetPose, params);
}
- SimpleDiffIK::Reachability RobotStateComponentPlugin::calculateRobotReachability(
+ SimpleDiffIK::Reachability
+ RobotStateComponentPlugin::calculateRobotReachability(
const std::string& id,
const std::vector<Eigen::Matrix4f>& targets,
const Eigen::VectorXf& initialJV,
@@ -218,11 +266,13 @@ namespace armarx::plugins
return getRobotData(id).calculateRobotReachability(targets, initialJV, params);
}
- void RobotStateComponentPlugin::preOnInitComponent()
+ void
+ RobotStateComponentPlugin::preOnInitComponent()
{
if (!_deactivated && !_robotStateComponent && _robotStateComponentName.empty())
{
- parent<Component>().getProperty(_robotStateComponentName, makePropertyName(_propertyName));
+ parent<Component>().getProperty(_robotStateComponentName,
+ makePropertyName(_propertyName));
}
if (!_deactivated && !_robotStateComponent)
@@ -231,7 +281,8 @@ namespace armarx::plugins
}
}
- void RobotStateComponentPlugin::preOnConnectComponent()
+ void
+ RobotStateComponentPlugin::preOnConnectComponent()
{
if (!_deactivated && !_robotStateComponent)
{
@@ -243,64 +294,75 @@ namespace armarx::plugins
}
}
- void RobotStateComponentPlugin::postOnDisconnectComponent()
+ void
+ RobotStateComponentPlugin::postOnDisconnectComponent()
{
_robotStateComponent = nullptr;
}
- void RobotStateComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ void
+ RobotStateComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(makePropertyName(_propertyName)))
{
- properties->defineOptionalProperty<std::string>(
- makePropertyName(_propertyName),
- "RobotStateComponent",
- "Name of the robot state component");
+ properties->defineOptionalProperty<std::string>(makePropertyName(_propertyName),
+ "RobotStateComponent",
+ "Name of the robot state component");
}
}
- SimpleDiffIK::Result RobotStateComponentPlugin::RobotData::calculateRobotDiffIK(const Eigen::Matrix4f& targetPose, const SimpleDiffIK::Parameters& params) const
+ SimpleDiffIK::Result
+ RobotStateComponentPlugin::RobotData::calculateRobotDiffIK(
+ const Eigen::Matrix4f& targetPose,
+ const SimpleDiffIK::Parameters& params) const
{
ARMARX_CHECK_NOT_NULL(rns) << "No robot node set configured for the robot";
ARMARX_CHECK_NOT_NULL(node) << "No tcp configured for the robot";
return SimpleDiffIK::CalculateDiffIK(targetPose, rns, node, params);
}
- SimpleDiffIK::Reachability RobotStateComponentPlugin::RobotData::calculateRobotReachability(const std::vector<Eigen::Matrix4f>& targets, const Eigen::VectorXf& initialJV, const SimpleDiffIK::Parameters& params) const
+ SimpleDiffIK::Reachability
+ RobotStateComponentPlugin::RobotData::calculateRobotReachability(
+ const std::vector<Eigen::Matrix4f>& targets,
+ const Eigen::VectorXf& initialJV,
+ const SimpleDiffIK::Parameters& params) const
{
ARMARX_CHECK_NOT_NULL(rns) << "No robot node set configured for the robot";
ARMARX_CHECK_NOT_NULL(node) << "No tcp configured for the robot ";
return SimpleDiffIK::CalculateReachability(targets, initialJV, rns, node, params);
}
- void RobotStateComponentPlugin::setRobotStateComponentName(const std::string& name)
+ void
+ RobotStateComponentPlugin::setRobotStateComponentName(const std::string& name)
{
ARMARX_CHECK_NOT_EMPTY(name);
ARMARX_CHECK_EMPTY(_robotStateComponentName);
_robotStateComponentName = name;
}
- const std::string& RobotStateComponentPlugin::getRobotStateComponentName() const
+ const std::string&
+ RobotStateComponentPlugin::getRobotStateComponentName() const
{
return _robotStateComponentName;
}
- void RobotStateComponentPlugin::deactivate()
+ void
+ RobotStateComponentPlugin::deactivate()
{
_deactivated = true;
}
- Eigen::Matrix4f RobotStateComponentPlugin::transformFromTo(
- const std::string& from,
- const std::string& to,
- const VirtualRobot::RobotPtr& rob)
+ Eigen::Matrix4f
+ RobotStateComponentPlugin::transformFromTo(const std::string& from,
+ const std::string& to,
+ const VirtualRobot::RobotPtr& rob)
{
ARMARX_CHECK_NOT_NULL(rob);
armarx::FramedPose fp{Eigen::Matrix4f::Identity(), from, rob->getName()};
fp.changeFrame(rob, to);
return fp.toEigen();
}
-}
+} // namespace armarx::plugins
namespace armarx
{
@@ -308,123 +370,182 @@ namespace armarx
{
addPlugin(_robotStateComponentPlugin);
}
- const RobotStateComponentPluginUser::RobotStateComponentPlugin& RobotStateComponentPluginUser::getRobotStateComponentPlugin() const
+
+ const RobotStateComponentPluginUser::RobotStateComponentPlugin&
+ RobotStateComponentPluginUser::getRobotStateComponentPlugin() const
{
- return *_robotStateComponentPlugin;
+ return *_robotStateComponentPlugin;
}
- RobotStateComponentPluginUser::RobotStateComponentPlugin& RobotStateComponentPluginUser::getRobotStateComponentPlugin()
+ RobotStateComponentPluginUser::RobotStateComponentPlugin&
+ RobotStateComponentPluginUser::getRobotStateComponentPlugin()
{
- return *_robotStateComponentPlugin;
+ return *_robotStateComponentPlugin;
}
- const RobotStateComponentInterfacePrx& RobotStateComponentPluginUser::getRobotStateComponent() const
+ const RobotStateComponentInterfacePrx&
+ RobotStateComponentPluginUser::getRobotStateComponent() const
{
return getRobotStateComponentPlugin().getRobotStateComponent();
}
- RobotNameHelperPtr RobotStateComponentPluginUser::getRobotNameHelper() const
+
+ RobotNameHelperPtr
+ RobotStateComponentPluginUser::getRobotNameHelper() const
{
return getRobotStateComponentPlugin().getRobotNameHelper();
}
- bool RobotStateComponentPluginUser::hasRobot(const std::string& id) const
+ bool
+ RobotStateComponentPluginUser::hasRobot(const std::string& id) const
{
return getRobotStateComponentPlugin().hasRobot(id);
}
- VirtualRobot::RobotPtr RobotStateComponentPluginUser::addRobot(const std::string& id, const VirtualRobot::RobotPtr& robot, const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& node)
+ VirtualRobot::RobotPtr
+ RobotStateComponentPluginUser::addRobot(const std::string& id,
+ const VirtualRobot::RobotPtr& robot,
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& node)
{
return getRobotStateComponentPlugin().addRobot(id, robot, rns, node);
}
- VirtualRobot::RobotPtr RobotStateComponentPluginUser::addRobot(const std::string& id, const VirtualRobot::RobotPtr& robot, const std::string& rnsName, const std::string& nodeName)
+ VirtualRobot::RobotPtr
+ RobotStateComponentPluginUser::addRobot(const std::string& id,
+ const VirtualRobot::RobotPtr& robot,
+ const std::string& rnsName,
+ const std::string& nodeName)
{
return getRobotStateComponentPlugin().addRobot(id, robot, rnsName, nodeName);
}
- VirtualRobot::RobotPtr RobotStateComponentPluginUser::addRobot(const std::string& id, VirtualRobot::RobotIO::RobotDescription loadMode, const std::string& rnsName, const std::string& nodeName)
+ VirtualRobot::RobotPtr
+ RobotStateComponentPluginUser::addRobot(const std::string& id,
+ VirtualRobot::RobotIO::RobotDescription loadMode,
+ const std::string& rnsName,
+ const std::string& nodeName)
{
return getRobotStateComponentPlugin().addRobot(id, loadMode, rnsName, nodeName);
}
- VirtualRobot::RobotPtr RobotStateComponentPluginUser::addRobot(const std::string& id, const std::string& filename, const Ice::StringSeq packages, VirtualRobot::RobotIO::RobotDescription loadMode, const std::string& rnsName, const std::string& nodeName)
+ VirtualRobot::RobotPtr
+ RobotStateComponentPluginUser::addRobot(const std::string& id,
+ const std::string& filename,
+ const Ice::StringSeq packages,
+ VirtualRobot::RobotIO::RobotDescription loadMode,
+ const std::string& rnsName,
+ const std::string& nodeName)
{
- return getRobotStateComponentPlugin().addRobot(id, filename, packages, loadMode, rnsName, nodeName);
+ return getRobotStateComponentPlugin().addRobot(
+ id, filename, packages, loadMode, rnsName, nodeName);
}
- VirtualRobot::RobotPtr RobotStateComponentPluginUser::getRobot(const std::string& id) const
+ VirtualRobot::RobotPtr
+ RobotStateComponentPluginUser::getRobot(const std::string& id) const
{
return getRobotStateComponentPlugin().getRobot(id);
}
- plugins::RobotStateComponentPlugin::RobotData RobotStateComponentPluginUser::getRobotData(const std::string& id) const
+ plugins::RobotStateComponentPlugin::RobotData
+ RobotStateComponentPluginUser::getRobotData(const std::string& id) const
{
return getRobotStateComponentPlugin().getRobotData(id);
}
- void RobotStateComponentPluginUser::setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName)
+ void
+ RobotStateComponentPluginUser::setRobotRNSAndNode(const std::string& id,
+ const std::string& rnsName,
+ const std::string& nodeName)
{
getRobotStateComponentPlugin().setRobotRNSAndNode(id, rnsName, nodeName);
}
- RobotStateComponentInterfacePrx RobotStateComponentPluginUser::getRobotStateComponent()
+ RobotStateComponentInterfacePrx
+ RobotStateComponentPluginUser::getRobotStateComponent()
{
return getRobotStateComponentPlugin().getRobotStateComponent();
}
- bool RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const
+ bool
+ RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(robot);
}
- bool RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, Ice::Long timestamp) const
+ bool
+ RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot,
+ Ice::Long timestamp) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(robot, timestamp);
}
- bool RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, const RobotStateConfig& state) const
+ bool
+ RobotStateComponentPluginUser::synchronizeLocalClone(const VirtualRobot::RobotPtr& robot,
+ const RobotStateConfig& state) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(robot, state);
}
- bool RobotStateComponentPluginUser::synchronizeLocalClone(const plugins::RobotStateComponentPlugin::RobotData& rdata) const
+ bool
+ RobotStateComponentPluginUser::synchronizeLocalClone(
+ const plugins::RobotStateComponentPlugin::RobotData& rdata) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(rdata);
}
- bool RobotStateComponentPluginUser::synchronizeLocalClone(const plugins::RobotStateComponentPlugin::RobotData& rdata, Ice::Long timestamp) const
+ bool
+ RobotStateComponentPluginUser::synchronizeLocalClone(
+ const plugins::RobotStateComponentPlugin::RobotData& rdata,
+ Ice::Long timestamp) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(rdata, timestamp);
}
- bool RobotStateComponentPluginUser::synchronizeLocalClone(const plugins::RobotStateComponentPlugin::RobotData& rdata, const RobotStateConfig& state) const
+ bool
+ RobotStateComponentPluginUser::synchronizeLocalClone(
+ const plugins::RobotStateComponentPlugin::RobotData& rdata,
+ const RobotStateConfig& state) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(rdata, state);
}
- bool RobotStateComponentPluginUser::synchronizeLocalClone(const std::string& id) const
+ bool
+ RobotStateComponentPluginUser::synchronizeLocalClone(const std::string& id) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(id);
}
- bool RobotStateComponentPluginUser::synchronizeLocalClone(const std::string& id, Ice::Long timestamp) const
+ bool
+ RobotStateComponentPluginUser::synchronizeLocalClone(const std::string& id,
+ Ice::Long timestamp) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(id, timestamp);
}
- bool RobotStateComponentPluginUser::synchronizeLocalClone(const std::string& id, const RobotStateConfig& state) const
+ bool
+ RobotStateComponentPluginUser::synchronizeLocalClone(const std::string& id,
+ const RobotStateConfig& state) const
{
return getRobotStateComponentPlugin().synchronizeLocalClone(id, state);
}
- SimpleDiffIK::Result RobotStateComponentPluginUser::calculateRobotDiffIK(const std::string& id, const Eigen::Matrix4f& targetPose, const SimpleDiffIK::Parameters& params)
+ SimpleDiffIK::Result
+ RobotStateComponentPluginUser::calculateRobotDiffIK(const std::string& id,
+ const Eigen::Matrix4f& targetPose,
+ const SimpleDiffIK::Parameters& params)
{
return getRobotStateComponentPlugin().calculateRobotDiffIK(id, targetPose, params);
}
- SimpleDiffIK::Reachability RobotStateComponentPluginUser::calculateRobotReachability(const std::string& id, const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, const SimpleDiffIK::Parameters& params)
+ SimpleDiffIK::Reachability
+ RobotStateComponentPluginUser::calculateRobotReachability(
+ const std::string& id,
+ const std::vector<Eigen::Matrix4f> targets,
+ const Eigen::VectorXf& initialJV,
+ const SimpleDiffIK::Parameters& params)
{
- return getRobotStateComponentPlugin().calculateRobotReachability(id, targets, initialJV, params);
+ return getRobotStateComponentPlugin().calculateRobotReachability(
+ id, targets, initialJV, params);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index c385948dbea9cb9cf87755874eed4b9c847d3285..f34ffa1c75cf67457f1b64d365a72173d6a66ee0 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -29,11 +29,10 @@
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
-
namespace armarx::plugins
{
/**
@@ -63,32 +62,30 @@ namespace armarx::plugins
public:
bool hasRobot(const std::string& id) const;
- VirtualRobot::RobotPtr addRobot(
- const std::string& id,
- const VirtualRobot::RobotPtr& robot,
- const VirtualRobot::RobotNodeSetPtr& rns = {},
- const VirtualRobot::RobotNodePtr& node = {});
- VirtualRobot::RobotPtr addRobot(
- const std::string& id,
- const VirtualRobot::RobotPtr& robot,
- const std::string& rnsName,
- const std::string& nodeName = "");
- VirtualRobot::RobotPtr addRobot(
- const std::string& id,
- VirtualRobot::RobotIO::RobotDescription loadMode,
- const std::string& rnsName = "",
- const std::string& nodeName = "");
- VirtualRobot::RobotPtr addRobot(
- const std::string& id,
- const std::string& filename,
- const Ice::StringSeq packages,
- VirtualRobot::RobotIO::RobotDescription loadMode,
- const std::string& rnsName = "",
- const std::string& nodeName = "");
+ VirtualRobot::RobotPtr addRobot(const std::string& id,
+ const VirtualRobot::RobotPtr& robot,
+ const VirtualRobot::RobotNodeSetPtr& rns = {},
+ const VirtualRobot::RobotNodePtr& node = {});
+ VirtualRobot::RobotPtr addRobot(const std::string& id,
+ const VirtualRobot::RobotPtr& robot,
+ const std::string& rnsName,
+ const std::string& nodeName = "");
+ VirtualRobot::RobotPtr addRobot(const std::string& id,
+ VirtualRobot::RobotIO::RobotDescription loadMode,
+ const std::string& rnsName = "",
+ const std::string& nodeName = "");
+ VirtualRobot::RobotPtr addRobot(const std::string& id,
+ const std::string& filename,
+ const Ice::StringSeq packages,
+ VirtualRobot::RobotIO::RobotDescription loadMode,
+ const std::string& rnsName = "",
+ const std::string& nodeName = "");
VirtualRobot::RobotPtr getRobot(const std::string& id) const;
RobotData getRobotData(const std::string& id) const;
- void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
+ void setRobotRNSAndNode(const std::string& id,
+ const std::string& rnsName,
+ const std::string& nodeName);
//querry
public:
@@ -102,7 +99,8 @@ namespace armarx::plugins
public:
bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, Ice::Long timestamp) const;
- bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, const RobotStateConfig& state) const;
+ bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot,
+ const RobotStateConfig& state) const;
bool synchronizeLocalClone(const RobotData& rdata) const;
bool synchronizeLocalClone(const RobotData& rdata, Ice::Long timestamp) const;
@@ -114,15 +112,15 @@ namespace armarx::plugins
//diffik
public:
- SimpleDiffIK::Result calculateRobotDiffIK(
- const std::string& id,
- const Eigen::Matrix4f& targetPose,
- const SimpleDiffIK::Parameters& params = {}) const;
- SimpleDiffIK::Reachability calculateRobotReachability(
- const std::string& id,
- const std::vector<Eigen::Matrix4f>& targets,
- const Eigen::VectorXf& initialJV,
- const SimpleDiffIK::Parameters& params = {}) const;
+ SimpleDiffIK::Result
+ calculateRobotDiffIK(const std::string& id,
+ const Eigen::Matrix4f& targetPose,
+ const SimpleDiffIK::Parameters& params = {}) const;
+ SimpleDiffIK::Reachability
+ calculateRobotReachability(const std::string& id,
+ const std::vector<Eigen::Matrix4f>& targets,
+ const Eigen::VectorXf& initialJV,
+ const SimpleDiffIK::Parameters& params = {}) const;
//hooks
protected:
@@ -135,30 +133,30 @@ namespace armarx::plugins
public:
struct RobotData
{
- VirtualRobot::RobotPtr robot;
+ VirtualRobot::RobotPtr robot;
VirtualRobot::RobotNodeSetPtr rns;
- VirtualRobot::RobotNodePtr node;
-
- SimpleDiffIK::Result calculateRobotDiffIK(
- const Eigen::Matrix4f& targetPose,
- const SimpleDiffIK::Parameters& params = {}) const;
- SimpleDiffIK::Reachability calculateRobotReachability(
- const std::vector<Eigen::Matrix4f>& targets,
- const Eigen::VectorXf& initialJV,
- const SimpleDiffIK::Parameters& params = {}) const;
+ VirtualRobot::RobotNodePtr node;
+
+ SimpleDiffIK::Result
+ calculateRobotDiffIK(const Eigen::Matrix4f& targetPose,
+ const SimpleDiffIK::Parameters& params = {}) const;
+ SimpleDiffIK::Reachability
+ calculateRobotReachability(const std::vector<Eigen::Matrix4f>& targets,
+ const Eigen::VectorXf& initialJV,
+ const SimpleDiffIK::Parameters& params = {}) const;
};
+
//data
private:
- static constexpr auto _propertyName = "RemoteStateComponentName";
- std::string _robotStateComponentName;
- RobotStateComponentInterfacePrx _robotStateComponent;
- mutable std::recursive_mutex _robotsMutex;
+ static constexpr auto _propertyName = "RemoteStateComponentName";
+ std::string _robotStateComponentName;
+ RobotStateComponentInterfacePrx _robotStateComponent;
+ mutable std::recursive_mutex _robotsMutex;
std::map<std::string, RobotData> _robots;
- RobotNameHelperPtr _nameHelper;
- bool _deactivated = false;
+ RobotNameHelperPtr _nameHelper;
+ bool _deactivated = false;
};
-}
-
+} // namespace armarx::plugins
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -168,8 +166,10 @@ namespace armarx
{
public:
using RobotStateComponentPlugin = armarx::plugins::RobotStateComponentPlugin;
+
private:
RobotStateComponentPlugin* _robotStateComponentPlugin{nullptr};
+
public:
RobotStateComponentPluginUser();
@@ -183,31 +183,28 @@ namespace armarx
public:
bool hasRobot(const std::string& id) const;
- VirtualRobot::RobotPtr addRobot(
- const std::string& id,
- const VirtualRobot::RobotPtr& robot,
- const VirtualRobot::RobotNodeSetPtr& rns = {},
- const VirtualRobot::RobotNodePtr& node = {});
- VirtualRobot::RobotPtr addRobot(
- const std::string& id,
- const VirtualRobot::RobotPtr& robot,
- const std::string& rnsName,
- const std::string& nodeName = "");
- VirtualRobot::RobotPtr addRobot(
- const std::string& id,
- VirtualRobot::RobotIO::RobotDescription loadMode,
- const std::string& rnsName = "",
- const std::string& nodeName = "");
- VirtualRobot::RobotPtr addRobot(
- const std::string& id,
- const std::string& filename,
- const Ice::StringSeq packages,
- VirtualRobot::RobotIO::RobotDescription loadMode,
- const std::string& rnsName = "",
- const std::string& nodeName = "");
-
- template<class...Ts>
- VirtualRobot::RobotPtr addOrGetRobot(const std::string& id, Ts&& ...ts)
+ VirtualRobot::RobotPtr addRobot(const std::string& id,
+ const VirtualRobot::RobotPtr& robot,
+ const VirtualRobot::RobotNodeSetPtr& rns = {},
+ const VirtualRobot::RobotNodePtr& node = {});
+ VirtualRobot::RobotPtr addRobot(const std::string& id,
+ const VirtualRobot::RobotPtr& robot,
+ const std::string& rnsName,
+ const std::string& nodeName = "");
+ VirtualRobot::RobotPtr addRobot(const std::string& id,
+ VirtualRobot::RobotIO::RobotDescription loadMode,
+ const std::string& rnsName = "",
+ const std::string& nodeName = "");
+ VirtualRobot::RobotPtr addRobot(const std::string& id,
+ const std::string& filename,
+ const Ice::StringSeq packages,
+ VirtualRobot::RobotIO::RobotDescription loadMode,
+ const std::string& rnsName = "",
+ const std::string& nodeName = "");
+
+ template <class... Ts>
+ VirtualRobot::RobotPtr
+ addOrGetRobot(const std::string& id, Ts&&... ts)
{
if (hasRobot(id))
{
@@ -218,7 +215,9 @@ namespace armarx
VirtualRobot::RobotPtr getRobot(const std::string& id) const;
RobotStateComponentPlugin::RobotData getRobotData(const std::string& id) const;
- void setRobotRNSAndNode(const std::string& id, const std::string& rnsName, const std::string& nodeName);
+ void setRobotRNSAndNode(const std::string& id,
+ const std::string& rnsName,
+ const std::string& nodeName);
RobotStateComponentInterfacePrx getRobotStateComponent();
@@ -226,11 +225,14 @@ namespace armarx
public:
bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot) const;
bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, Ice::Long timestamp) const;
- bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot, const RobotStateConfig& state) const;
+ bool synchronizeLocalClone(const VirtualRobot::RobotPtr& robot,
+ const RobotStateConfig& state) const;
bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata) const;
- bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata, Ice::Long timestamp) const;
- bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata, const RobotStateConfig& state) const;
+ bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata,
+ Ice::Long timestamp) const;
+ bool synchronizeLocalClone(const RobotStateComponentPlugin::RobotData& rdata,
+ const RobotStateConfig& state) const;
bool synchronizeLocalClone(const std::string& id) const;
bool synchronizeLocalClone(const std::string& id, Ice::Long timestamp) const;
@@ -238,14 +240,13 @@ namespace armarx
//diffik
public:
- SimpleDiffIK::Result calculateRobotDiffIK(
- const std::string& id,
- const Eigen::Matrix4f& targetPose,
- const SimpleDiffIK::Parameters& params = {});
- SimpleDiffIK::Reachability calculateRobotReachability(
- const std::string& id,
- const std::vector<Eigen::Matrix4f> targets,
- const Eigen::VectorXf& initialJV,
- const SimpleDiffIK::Parameters& params = {});
+ SimpleDiffIK::Result calculateRobotDiffIK(const std::string& id,
+ const Eigen::Matrix4f& targetPose,
+ const SimpleDiffIK::Parameters& params = {});
+ SimpleDiffIK::Reachability
+ calculateRobotReachability(const std::string& id,
+ const std::vector<Eigen::Matrix4f> targets,
+ const Eigen::VectorXf& initialJV,
+ const SimpleDiffIK::Parameters& params = {});
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
index 0b5805c209fe40ab96eaafbeb2f573ca91a9ba9e..c3b8fdbca4538702b22bc30921617f02a04d9446 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.cpp
@@ -2,41 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void RobotUnitObserverComponentPlugin::preOnInitComponent()
+ void
+ RobotUnitObserverComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void RobotUnitObserverComponentPlugin::preOnConnectComponent()
+ void
+ RobotUnitObserverComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_robotUnitObserver, PROPERTY_NAME);
}
- void RobotUnitObserverComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ RobotUnitObserverComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "RobotUnitObserver",
- "Name of the RobotUnitObserver");
+ PROPERTY_NAME, "RobotUnitObserver", "Name of the RobotUnitObserver");
}
}
- ObserverInterfacePrx RobotUnitObserverComponentPlugin::getRobotUnitObserver()
+ ObserverInterfacePrx
+ RobotUnitObserverComponentPlugin::getRobotUnitObserver()
{
return _robotUnitObserver;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -46,12 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- ObserverInterfacePrx RobotUnitObserverComponentPluginUser::getRobotUnitObserver()
+ ObserverInterfacePrx
+ RobotUnitObserverComponentPluginUser::getRobotUnitObserver()
{
return plugin->getRobotUnitObserver();
}
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
index 27b70a5064ad2e6be651e04b2cffe5f7d2d0360f..7e1b012bc658e3f8be5c4c2332f94d1e6ec32b69 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitObserverComponentPlugin.h
@@ -3,7 +3,6 @@
#include <ArmarXCore/core/ComponentPlugin.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
-
namespace armarx
{
namespace plugins
@@ -27,9 +26,8 @@ namespace armarx
ObserverInterfacePrx _robotUnitObserver;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,8 +46,7 @@ namespace armarx
armarx::plugins::RobotUnitObserverComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
@@ -57,5 +54,5 @@ namespace armarx
{
// Legacy typedef.
using RobotUnitObserverComponentPluginUser = armarx::RobotUnitObserverComponentPluginUser;
- }
-}
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
index 28a6479b0ab5f844e5a2e68c14fdf84df66e3c67..5daad05626b1ee2a7559443776d8fdf3a42cb819 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.cpp
@@ -2,41 +2,43 @@
#include <ArmarXCore/core/Component.h>
-
namespace armarx
{
namespace plugins
{
- void TrajectoryPlayerComponentPlugin::preOnInitComponent()
+ void
+ TrajectoryPlayerComponentPlugin::preOnInitComponent()
{
parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
}
- void TrajectoryPlayerComponentPlugin::preOnConnectComponent()
+ void
+ TrajectoryPlayerComponentPlugin::preOnConnectComponent()
{
parent<Component>().getProxyFromProperty(_trajectoryPlayer, PROPERTY_NAME);
}
- void TrajectoryPlayerComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ TrajectoryPlayerComponentPlugin::postCreatePropertyDefinitions(
+ armarx::PropertyDefinitionsPtr& properties)
{
if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineOptionalProperty<std::string>(
- PROPERTY_NAME,
- "TrajectoryPlayer",
- "Name of the TrajectoryPlayer");
+ PROPERTY_NAME, "TrajectoryPlayer", "Name of the TrajectoryPlayer");
}
}
- TrajectoryPlayerInterfacePrx TrajectoryPlayerComponentPlugin::getTrajectoryPlayer()
+ TrajectoryPlayerInterfacePrx
+ TrajectoryPlayerComponentPlugin::getTrajectoryPlayer()
{
return _trajectoryPlayer;
}
- }
-}
+ } // namespace plugins
+} // namespace armarx
namespace armarx
{
@@ -46,12 +48,11 @@ namespace armarx
addPlugin(plugin);
}
- TrajectoryPlayerInterfacePrx TrajectoryPlayerComponentPluginUser::getTrajectoryPlayer()
+ TrajectoryPlayerInterfacePrx
+ TrajectoryPlayerComponentPluginUser::getTrajectoryPlayer()
{
return plugin->getTrajectoryPlayer();
}
-}
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
index 28d57ca0488a8318ca154add6ec749b5380f8e50..3317a82b7041416a55545bbd0ae8f4be61e3d7b9 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/TrajectoryPlayerComponentPlugin.h
@@ -1,8 +1,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
namespace armarx
{
@@ -27,9 +27,8 @@ namespace armarx
TrajectoryPlayerInterfacePrx _trajectoryPlayer;
};
- }
-}
-
+ } // namespace plugins
+} // namespace armarx
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -48,8 +47,7 @@ namespace armarx
armarx::plugins::TrajectoryPlayerComponentPlugin* plugin = nullptr;
};
-}
-
+} // namespace armarx
namespace armarx
{
@@ -57,5 +55,5 @@ namespace armarx
{
// Legacy typedef.
using TrajectoryPlayerComponentPluginUser = armarx::TrajectoryPlayerComponentPluginUser;
- }
-}
+ } // namespace plugins
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/test/RobotAPIComponentPluginsTest.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/test/RobotAPIComponentPluginsTest.cpp
index 3da86c4385f54f08b572e40992dab9b50b4c5999..9012139fa192ba7d753e359d1e110b4d5c5226d2 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/test/RobotAPIComponentPluginsTest.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/test/RobotAPIComponentPluginsTest.cpp
@@ -25,11 +25,12 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include "../RobotStateComponentPlugin.h"
-#include "../DebugDrawerHelperComponentPlugin.h"
#include <iostream>
+#include "../DebugDrawerHelperComponentPlugin.h"
+#include "../RobotStateComponentPlugin.h"
+
BOOST_AUTO_TEST_CASE(testExample)
{
diff --git a/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.cpp b/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.cpp
index e95378770f5328ba3669887e59df6715c8e962d3..912c6a889e758c07d43d850ac75e062184113489 100644
--- a/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.cpp
+++ b/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.cpp
@@ -22,13 +22,14 @@
#include "RobotAPIVariantWidget.h"
+#include <QFormLayout>
#include <QLabel>
#include <QString>
-#include <QVBoxLayout>
-#include <QFormLayout>
#include <QTableWidget>
+#include <QVBoxLayout>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include <RobotAPI/interface/core/FramedPoseBase.h>
#include <RobotAPI/interface/core/LinkedPoseBase.h>
#include <RobotAPI/interface/core/OrientedPoint.h>
@@ -36,12 +37,14 @@
namespace armarx::detail
{
- RobotAPIVariantWidgetDummySymbol::RobotAPIVariantWidgetDummySymbol(int) {}
-}
+ RobotAPIVariantWidgetDummySymbol::RobotAPIVariantWidgetDummySymbol(int)
+ {
+ }
+} // namespace armarx::detail
namespace armarx::VariantDataWidgets
{
- class Vector2BaseWidget: public VariantDataWidgetBase
+ class Vector2BaseWidget : public VariantDataWidgetBase
{
public:
Vector2BaseWidget(const VariantDataPtr& v)
@@ -55,23 +58,27 @@ namespace armarx::VariantDataWidgets
l->addRow("Y", labelY);
update(v);
}
- void update(const VariantDataPtr& p) override
+
+ void
+ update(const VariantDataPtr& p) override
{
Vector2BasePtr v = Vector2BasePtr::dynamicCast(p);
ARMARX_CHECK_EXPRESSION(v);
labelX->setText(QString::number(v->x));
labelY->setText(QString::number(v->y));
}
+
private:
QLabel* labelX;
QLabel* labelY;
};
- VariantDataWidgetFactoryRegistration<Vector2BaseWidget> registerVector2BaseWidget {Vector2Base::ice_staticId()};
- class Vector3BaseWidget: public VariantDataWidgetBase
+ VariantDataWidgetFactoryRegistration<Vector2BaseWidget> registerVector2BaseWidget{
+ Vector2Base::ice_staticId()};
+
+ class Vector3BaseWidget : public VariantDataWidgetBase
{
public:
-
Vector3BaseWidget(const VariantDataPtr& v)
{
auto l = new QFormLayout;
@@ -85,7 +92,9 @@ namespace armarx::VariantDataWidgets
l->addRow("Z", labelZ);
update(v);
}
- void update(const VariantDataPtr& p) override
+
+ void
+ update(const VariantDataPtr& p) override
{
Vector3BasePtr v = Vector3BasePtr::dynamicCast(p);
ARMARX_CHECK_EXPRESSION(v);
@@ -93,17 +102,19 @@ namespace armarx::VariantDataWidgets
labelY->setText(QString::number(v->y));
labelZ->setText(QString::number(v->z));
}
+
private:
QLabel* labelX;
QLabel* labelY;
QLabel* labelZ;
};
- VariantDataWidgetFactoryRegistration<Vector3BaseWidget> registerVector3BaseWidget {Vector3Base::ice_staticId()};
- class FramedPositionBaseWidget: public VariantDataWidgetBase
+ VariantDataWidgetFactoryRegistration<Vector3BaseWidget> registerVector3BaseWidget{
+ Vector3Base::ice_staticId()};
+
+ class FramedPositionBaseWidget : public VariantDataWidgetBase
{
public:
-
FramedPositionBaseWidget(const VariantDataPtr& v)
{
auto l = new QFormLayout;
@@ -121,7 +132,9 @@ namespace armarx::VariantDataWidgets
l->addRow("Z", labelZ);
update(v);
}
- void update(const VariantDataPtr& p) override
+
+ void
+ update(const VariantDataPtr& p) override
{
FramedPositionBasePtr v = FramedPositionBasePtr::dynamicCast(p);
ARMARX_CHECK_EXPRESSION(v);
@@ -131,6 +144,7 @@ namespace armarx::VariantDataWidgets
labelY->setText(QString::number(v->y));
labelZ->setText(QString::number(v->z));
}
+
private:
QLabel* labelAg;
QLabel* labelFr;
@@ -138,12 +152,13 @@ namespace armarx::VariantDataWidgets
QLabel* labelY;
QLabel* labelZ;
};
- VariantDataWidgetFactoryRegistration<FramedPositionBaseWidget> registerFramedPositionBaseWidget {FramedPositionBase::ice_staticId()};
- class FramedDirectionBaseWidget: public VariantDataWidgetBase
+ VariantDataWidgetFactoryRegistration<FramedPositionBaseWidget> registerFramedPositionBaseWidget{
+ FramedPositionBase::ice_staticId()};
+
+ class FramedDirectionBaseWidget : public VariantDataWidgetBase
{
public:
-
FramedDirectionBaseWidget(const VariantDataPtr& v)
{
auto l = new QFormLayout;
@@ -161,7 +176,9 @@ namespace armarx::VariantDataWidgets
l->addRow("Z", labelZ);
update(v);
}
- void update(const VariantDataPtr& p) override
+
+ void
+ update(const VariantDataPtr& p) override
{
FramedDirectionBasePtr v = FramedDirectionBasePtr::dynamicCast(p);
ARMARX_CHECK_EXPRESSION(v);
@@ -171,6 +188,7 @@ namespace armarx::VariantDataWidgets
labelY->setText(QString::number(v->y));
labelZ->setText(QString::number(v->z));
}
+
private:
QLabel* labelAg;
QLabel* labelFr;
@@ -178,12 +196,13 @@ namespace armarx::VariantDataWidgets
QLabel* labelY;
QLabel* labelZ;
};
- VariantDataWidgetFactoryRegistration<FramedDirectionBaseWidget> registerFramedDirectionBaseWidget {FramedDirectionBase::ice_staticId()};
- class OrientedPointBaseWidget: public VariantDataWidgetBase
+ VariantDataWidgetFactoryRegistration<FramedDirectionBaseWidget>
+ registerFramedDirectionBaseWidget{FramedDirectionBase::ice_staticId()};
+
+ class OrientedPointBaseWidget : public VariantDataWidgetBase
{
public:
-
OrientedPointBaseWidget(const VariantDataPtr& v)
{
auto l = new QFormLayout;
@@ -203,7 +222,9 @@ namespace armarx::VariantDataWidgets
l->addRow("NZ", labelNZ);
update(v);
}
- void update(const VariantDataPtr& p) override
+
+ void
+ update(const VariantDataPtr& p) override
{
OrientedPointBasePtr v = OrientedPointBasePtr::dynamicCast(p);
ARMARX_CHECK_EXPRESSION(v);
@@ -214,6 +235,7 @@ namespace armarx::VariantDataWidgets
labelNY->setText(QString::number(v->ny));
labelNZ->setText(QString::number(v->nz));
}
+
private:
QLabel* labelPX;
QLabel* labelPY;
@@ -222,12 +244,13 @@ namespace armarx::VariantDataWidgets
QLabel* labelNY;
QLabel* labelNZ;
};
- VariantDataWidgetFactoryRegistration<OrientedPointBaseWidget> registerOrientedPointBaseWidget {OrientedPointBase::ice_staticId()};
- class FramedOrientedPointBaseWidget: public VariantDataWidgetBase
+ VariantDataWidgetFactoryRegistration<OrientedPointBaseWidget> registerOrientedPointBaseWidget{
+ OrientedPointBase::ice_staticId()};
+
+ class FramedOrientedPointBaseWidget : public VariantDataWidgetBase
{
public:
-
FramedOrientedPointBaseWidget(const VariantDataPtr& v)
{
auto l = new QFormLayout;
@@ -251,7 +274,9 @@ namespace armarx::VariantDataWidgets
l->addRow("NZ", labelNZ);
update(v);
}
- void update(const VariantDataPtr& p) override
+
+ void
+ update(const VariantDataPtr& p) override
{
FramedOrientedPointBasePtr v = FramedOrientedPointBasePtr::dynamicCast(p);
ARMARX_CHECK_EXPRESSION(v);
@@ -264,6 +289,7 @@ namespace armarx::VariantDataWidgets
labelNY->setText(QString::number(v->ny));
labelNZ->setText(QString::number(v->nz));
}
+
private:
QLabel* labelAg;
QLabel* labelFr;
@@ -274,12 +300,13 @@ namespace armarx::VariantDataWidgets
QLabel* labelNY;
QLabel* labelNZ;
};
- VariantDataWidgetFactoryRegistration<FramedOrientedPointBaseWidget> registerFramedOrientedPointBaseWidget {FramedOrientedPointBase::ice_staticId()};
- class QuaternionBaseWidget: public VariantDataWidgetBase
+ VariantDataWidgetFactoryRegistration<FramedOrientedPointBaseWidget>
+ registerFramedOrientedPointBaseWidget{FramedOrientedPointBase::ice_staticId()};
+
+ class QuaternionBaseWidget : public VariantDataWidgetBase
{
public:
-
QuaternionBaseWidget(const VariantDataPtr& v)
{
auto l = new QFormLayout;
@@ -295,7 +322,9 @@ namespace armarx::VariantDataWidgets
l->addRow("QZ", labelQZ);
update(v);
}
- void update(const VariantDataPtr& p) override
+
+ void
+ update(const VariantDataPtr& p) override
{
QuaternionBasePtr v = QuaternionBasePtr::dynamicCast(p);
ARMARX_CHECK_EXPRESSION(v);
@@ -304,18 +333,20 @@ namespace armarx::VariantDataWidgets
labelQY->setText(QString::number(v->qy));
labelQZ->setText(QString::number(v->qz));
}
+
private:
QLabel* labelQW;
QLabel* labelQX;
QLabel* labelQY;
QLabel* labelQZ;
};
- VariantDataWidgetFactoryRegistration<QuaternionBaseWidget> registerQuaternionBaseWidget {QuaternionBase::ice_staticId()};
- class FramedOrientationBaseWidget: public VariantDataWidgetBase
+ VariantDataWidgetFactoryRegistration<QuaternionBaseWidget> registerQuaternionBaseWidget{
+ QuaternionBase::ice_staticId()};
+
+ class FramedOrientationBaseWidget : public VariantDataWidgetBase
{
public:
-
FramedOrientationBaseWidget(const VariantDataPtr& v)
{
auto l = new QFormLayout;
@@ -335,7 +366,9 @@ namespace armarx::VariantDataWidgets
l->addRow("QZ", labelQZ);
update(v);
}
- void update(const VariantDataPtr& p) override
+
+ void
+ update(const VariantDataPtr& p) override
{
FramedOrientationBasePtr v = FramedOrientationBasePtr::dynamicCast(p);
ARMARX_CHECK_EXPRESSION(v);
@@ -346,6 +379,7 @@ namespace armarx::VariantDataWidgets
labelQY->setText(QString::number(v->qy));
labelQZ->setText(QString::number(v->qz));
}
+
private:
QLabel* labelAg;
QLabel* labelFr;
@@ -354,12 +388,13 @@ namespace armarx::VariantDataWidgets
QLabel* labelQY;
QLabel* labelQZ;
};
- VariantDataWidgetFactoryRegistration<FramedOrientationBaseWidget> registerFramedOrientationBaseWidget {FramedOrientationBase::ice_staticId()};
- class PoseBaseWidget: public VariantDataWidgetBase
+ VariantDataWidgetFactoryRegistration<FramedOrientationBaseWidget>
+ registerFramedOrientationBaseWidget{FramedOrientationBase::ice_staticId()};
+
+ class PoseBaseWidget : public VariantDataWidgetBase
{
public:
-
PoseBaseWidget(const VariantDataPtr& v)
{
auto l = new QFormLayout;
@@ -381,7 +416,9 @@ namespace armarx::VariantDataWidgets
l->addRow("QZ", labelQZ);
update(v);
}
- void update(const VariantDataPtr& p) override
+
+ void
+ update(const VariantDataPtr& p) override
{
PoseBasePtr v = PoseBasePtr::dynamicCast(p);
ARMARX_CHECK_EXPRESSION(v);
@@ -395,6 +432,7 @@ namespace armarx::VariantDataWidgets
labelQY->setText(QString::number(v->orientation->qy));
labelQZ->setText(QString::number(v->orientation->qz));
}
+
private:
QLabel* labelX;
QLabel* labelY;
@@ -404,12 +442,13 @@ namespace armarx::VariantDataWidgets
QLabel* labelQY;
QLabel* labelQZ;
};
- VariantDataWidgetFactoryRegistration<PoseBaseWidget> registerPoseBaseWidget {PoseBase::ice_staticId()};
- class FramedPoseBaseWidget: public VariantDataWidgetBase
+ VariantDataWidgetFactoryRegistration<PoseBaseWidget> registerPoseBaseWidget{
+ PoseBase::ice_staticId()};
+
+ class FramedPoseBaseWidget : public VariantDataWidgetBase
{
public:
-
FramedPoseBaseWidget(const VariantDataPtr& v)
{
auto l = new QFormLayout;
@@ -435,7 +474,9 @@ namespace armarx::VariantDataWidgets
l->addRow("QZ", labelQZ);
update(v);
}
- void update(const VariantDataPtr& p) override
+
+ void
+ update(const VariantDataPtr& p) override
{
FramedPoseBasePtr v = FramedPoseBasePtr::dynamicCast(p);
ARMARX_CHECK_EXPRESSION(v);
@@ -451,6 +492,7 @@ namespace armarx::VariantDataWidgets
labelQY->setText(QString::number(v->orientation->qy));
labelQZ->setText(QString::number(v->orientation->qz));
}
+
private:
QLabel* labelAg;
QLabel* labelFr;
@@ -462,5 +504,7 @@ namespace armarx::VariantDataWidgets
QLabel* labelQY;
QLabel* labelQZ;
};
- VariantDataWidgetFactoryRegistration<FramedPoseBaseWidget> registerFramedPoseBaseWidget {FramedPoseBase::ice_staticId()};
-}
+
+ VariantDataWidgetFactoryRegistration<FramedPoseBaseWidget> registerFramedPoseBaseWidget{
+ FramedPoseBase::ice_staticId()};
+} // namespace armarx::VariantDataWidgets
diff --git a/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.h b/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.h
index 55819d35dfe50eb40fae29f1ecd995cc25048c30..c4c5fc5708e1ccaecd13371af4f5eb2282e92ef4 100644
--- a/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.h
+++ b/source/RobotAPI/libraries/RobotAPIVariantWidget/RobotAPIVariantWidget.h
@@ -30,10 +30,12 @@ namespace armarx::detail
{
RobotAPIVariantWidgetDummySymbol(int);
};
+
const RobotAPIVariantWidgetDummySymbol robotAPIVariantWidgetDummySymbolInstanceGlobal(1);
- inline std::size_t robotAPIVariantWidgetDummySymbolFunction()
+
+ inline std::size_t
+ robotAPIVariantWidgetDummySymbolFunction()
{
return sizeof(robotAPIVariantWidgetDummySymbolInstanceGlobal);
}
-}
-
+} // namespace armarx::detail
diff --git a/source/RobotAPI/libraries/RobotAPIVariantWidget/test/RobotAPIVariantWidgetTest.cpp b/source/RobotAPI/libraries/RobotAPIVariantWidget/test/RobotAPIVariantWidgetTest.cpp
index 29434013e569e395822acadea3bcfd9251fb50d4..e6d1198c4e174082e04bcff1bc5af6dc23e83f93 100644
--- a/source/RobotAPI/libraries/RobotAPIVariantWidget/test/RobotAPIVariantWidgetTest.cpp
+++ b/source/RobotAPI/libraries/RobotAPIVariantWidget/test/RobotAPIVariantWidgetTest.cpp
@@ -24,9 +24,10 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
#include "../RobotAPIVariantWidget.h"
+#include <RobotAPI/Test.h>
+
#include <iostream>
BOOST_AUTO_TEST_CASE(testExample)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
index de446c057a89d629f695db0d145369463b9597d9..ce110f9d41a510e3af4199520cbf52a419370670 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
@@ -23,38 +23,47 @@
#include "ForceTorqueHelper.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
namespace armarx
{
- ForceTorqueHelper::ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx& forceTorqueObserver, const std::string& FTDatefieldName)
+ ForceTorqueHelper::ForceTorqueHelper(
+ const ForceTorqueUnitObserverInterfacePrx& forceTorqueObserver,
+ const std::string& FTDatefieldName)
{
- forceDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getForceDatafield(FTDatefieldName));
- torqueDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getTorqueDatafield(FTDatefieldName));
+ forceDf =
+ DatafieldRefPtr::dynamicCast(forceTorqueObserver->getForceDatafield(FTDatefieldName));
+ torqueDf =
+ DatafieldRefPtr::dynamicCast(forceTorqueObserver->getTorqueDatafield(FTDatefieldName));
setZero();
}
- Eigen::Vector3f ForceTorqueHelper::getForce()
+ Eigen::Vector3f
+ ForceTorqueHelper::getForce()
{
return forceDf->getDataField()->get<FramedDirection>()->toEigen() - initialForce;
}
- FramedDirection ForceTorqueHelper::getFramedForce()
+ FramedDirection
+ ForceTorqueHelper::getFramedForce()
{
const auto force = forceDf->getDataField()->get<FramedDirection>();
return FramedDirection(force->toEigen() - initialForce, force->getFrame(), force->agent);
}
- Eigen::Vector3f ForceTorqueHelper::getTorque()
+ Eigen::Vector3f
+ ForceTorqueHelper::getTorque()
{
return torqueDf->getDataField()->get<FramedDirection>()->toEigen() - initialTorque;
}
- void ForceTorqueHelper::setZero()
+ void
+ ForceTorqueHelper::setZero()
{
initialForce = forceDf->getDataField()->get<FramedDirection>()->toEigen();
initialTorque = torqueDf->getDataField()->get<FramedDirection>()->toEigen();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
index 57f437a4caeee7b07d6f3d851c876346da08b87b..ab5922ef796d3468770febdce41d22123073accf 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
@@ -27,9 +27,8 @@
#include <Eigen/Dense>
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/interface/units/ForceTorqueUnit.h>
-
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx
{
@@ -42,7 +41,8 @@ namespace armarx
class ForceTorqueHelper
{
public:
- ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx& forceTorqueObserver, const std::string& FTDatefieldName);
+ ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx& forceTorqueObserver,
+ const std::string& FTDatefieldName);
Eigen::Vector3f getForce();
FramedDirection getFramedForce();
@@ -56,4 +56,4 @@ namespace armarx
Eigen::Vector3f initialForce;
Eigen::Vector3f initialTorque;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp
index cf01c7cafe69e21cb5967cf5ff6dd0762986b817..3168de692a093cdf824ee037d8d9bbc72bca7643 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp
@@ -25,30 +25,34 @@
namespace armarx
{
- KinematicUnitHelper::KinematicUnitHelper(const KinematicUnitInterfacePrx& kinUnit)
- : kinUnit(kinUnit)
+ KinematicUnitHelper::KinematicUnitHelper(const KinematicUnitInterfacePrx& kinUnit) :
+ kinUnit(kinUnit)
{
}
- void KinematicUnitHelper::setJointAngles(const NameValueMap& jointAngles)
+ void
+ KinematicUnitHelper::setJointAngles(const NameValueMap& jointAngles)
{
kinUnit->switchControlMode(MakeControlModes(jointAngles, ePositionControl));
kinUnit->setJointAngles(jointAngles);
}
- void KinematicUnitHelper::setJointVelocities(const NameValueMap& jointVelocities)
+ void
+ KinematicUnitHelper::setJointVelocities(const NameValueMap& jointVelocities)
{
kinUnit->switchControlMode(MakeControlModes(jointVelocities, eVelocityControl));
kinUnit->setJointVelocities(jointVelocities);
}
- void KinematicUnitHelper::setJointTorques(const NameValueMap& jointTorques)
+ void
+ KinematicUnitHelper::setJointTorques(const NameValueMap& jointTorques)
{
kinUnit->switchControlMode(MakeControlModes(jointTorques, eTorqueControl));
kinUnit->setJointTorques(jointTorques);
}
- NameControlModeMap KinematicUnitHelper::MakeControlModes(const NameValueMap& jointValues, ControlMode controlMode)
+ NameControlModeMap
+ KinematicUnitHelper::MakeControlModes(const NameValueMap& jointValues, ControlMode controlMode)
{
NameControlModeMap cm;
for (const auto& pair : jointValues)
@@ -57,5 +61,4 @@ namespace armarx
}
return cm;
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h
index 0434f7a2b5c2959464815fac2a802a113b05077f..dcefa7c62aa8a28450f2da6ed4634dfe8fd33bee 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h
@@ -24,6 +24,7 @@
#pragma once
#include <memory>
+
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
namespace armarx
@@ -39,8 +40,11 @@ namespace armarx
void setJointAngles(const NameValueMap& jointAngles);
void setJointVelocities(const NameValueMap& jointVelocities);
void setJointTorques(const NameValueMap& jointTorques);
- static NameControlModeMap MakeControlModes(const NameValueMap& jointValues, ControlMode controlMode);
- static std::string ControlModeToString(ControlMode controlMode)
+ static NameControlModeMap MakeControlModes(const NameValueMap& jointValues,
+ ControlMode controlMode);
+
+ static std::string
+ ControlModeToString(ControlMode controlMode)
{
std::string state;
switch (controlMode)
@@ -77,7 +81,8 @@ namespace armarx
}
return state;
}
+
private:
KinematicUnitInterfacePrx kinUnit;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
index 16308fdfe7da160f2691d894c24d5f6a33bc84ce..ca568c78023cc9a50cd38bad693c02a1c026f1b4 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
@@ -28,7 +28,6 @@
#include <SimoxUtility/math.h>
#include <VirtualRobot/Nodes/RobotNode.h>
-
armarx::ObstacleAvoidingPlatformUnitHelper::ObstacleAvoidingPlatformUnitHelper(
armarx::PlatformUnitInterfacePrx platform_unit,
VirtualRobot::RobotPtr robot) :
@@ -37,34 +36,27 @@ armarx::ObstacleAvoidingPlatformUnitHelper::ObstacleAvoidingPlatformUnitHelper(
// pass
}
-
armarx::ObstacleAvoidingPlatformUnitHelper::ObstacleAvoidingPlatformUnitHelper(
armarx::PlatformUnitInterfacePrx platform_unit,
VirtualRobot::RobotPtr robot,
const Config& cfg) :
- m_platform_unit{platform_unit},
- m_robot{robot},
- m_cfg{cfg}
+ m_platform_unit{platform_unit}, m_robot{robot}, m_cfg{cfg}
{
// pass
}
-
armarx::ObstacleAvoidingPlatformUnitHelper::~ObstacleAvoidingPlatformUnitHelper()
{
m_platform_unit->stopPlatform();
}
-
void
-armarx::ObstacleAvoidingPlatformUnitHelper::setTarget(
- const Eigen::Vector2f& target_pos,
- const float target_ori)
+armarx::ObstacleAvoidingPlatformUnitHelper::setTarget(const Eigen::Vector2f& target_pos,
+ const float target_ori)
{
setTarget(Target{target_pos, target_ori});
}
-
void
armarx::ObstacleAvoidingPlatformUnitHelper::setTarget(const Target& target)
{
@@ -74,15 +66,12 @@ armarx::ObstacleAvoidingPlatformUnitHelper::setTarget(const Target& target)
m_waypoint_changed = true;
}
-
armarx::ObstacleAvoidingPlatformUnitHelper::Target
-armarx::ObstacleAvoidingPlatformUnitHelper::getCurrentTarget()
-const
+armarx::ObstacleAvoidingPlatformUnitHelper::getCurrentTarget() const
{
return m_waypoints.at(m_current_waypoint_index);
}
-
void
armarx::ObstacleAvoidingPlatformUnitHelper::update()
{
@@ -113,14 +102,13 @@ armarx::ObstacleAvoidingPlatformUnitHelper::update()
}
}
-
void
-armarx::ObstacleAvoidingPlatformUnitHelper::addWaypoint(const Eigen::Vector2f& waypoint_pos, float waypoint_ori)
+armarx::ObstacleAvoidingPlatformUnitHelper::addWaypoint(const Eigen::Vector2f& waypoint_pos,
+ float waypoint_ori)
{
addWaypoint(Target{waypoint_pos, waypoint_ori});
}
-
void
armarx::ObstacleAvoidingPlatformUnitHelper::setWaypoints(const std::vector<Target>& waypoints)
{
@@ -129,73 +117,59 @@ armarx::ObstacleAvoidingPlatformUnitHelper::setWaypoints(const std::vector<Targe
m_waypoint_changed = true;
}
-
void
armarx::ObstacleAvoidingPlatformUnitHelper::addWaypoint(const Target& waypoint)
{
m_waypoints.push_back(waypoint);
}
-
bool
-armarx::ObstacleAvoidingPlatformUnitHelper::isLastWaypoint()
-const
+armarx::ObstacleAvoidingPlatformUnitHelper::isLastWaypoint() const
{
return m_current_waypoint_index + 1 >= m_waypoints.size();
}
-
bool
-armarx::ObstacleAvoidingPlatformUnitHelper::isCurrentTargetNear()
-const
+armarx::ObstacleAvoidingPlatformUnitHelper::isCurrentTargetNear() const
{
- return getPositionError() < m_cfg.pos_near_threshold and getOrientationError() < m_cfg.ori_near_threshold;
+ return getPositionError() < m_cfg.pos_near_threshold and
+ getOrientationError() < m_cfg.ori_near_threshold;
}
-
bool
-armarx::ObstacleAvoidingPlatformUnitHelper::isCurrentTargetReached()
-const
+armarx::ObstacleAvoidingPlatformUnitHelper::isCurrentTargetReached() const
{
- return getPositionError() < m_cfg.pos_reached_threshold and getOrientationError() < m_cfg.ori_reached_threshold;
+ return getPositionError() < m_cfg.pos_reached_threshold and
+ getOrientationError() < m_cfg.ori_reached_threshold;
}
-
bool
-armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetNear()
-const
+armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetNear() const
{
return isLastWaypoint() and isCurrentTargetNear();
}
-
bool
-armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetReached()
-const
+armarx::ObstacleAvoidingPlatformUnitHelper::isFinalTargetReached() const
{
return isLastWaypoint() and isCurrentTargetReached();
}
-
void
armarx::ObstacleAvoidingPlatformUnitHelper::setMaxVelocities(float max_vel, float max_angular_vel)
{
m_platform_unit->setMaxVelocities(max_vel, max_angular_vel);
}
-
float
-armarx::ObstacleAvoidingPlatformUnitHelper::getPositionError()
-const
+armarx::ObstacleAvoidingPlatformUnitHelper::getPositionError() const
{
const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
return (getCurrentTarget().pos - agent_pos).norm();
}
-
float
-armarx::ObstacleAvoidingPlatformUnitHelper::getOrientationError()
-const
+armarx::ObstacleAvoidingPlatformUnitHelper::getOrientationError() const
{
using namespace simox::math;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
index d372ee8da0badc3114a8b60971e88a950e970c54..836f908f0a1d1e0a9c4bc883f85dc932e711168b 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
@@ -33,7 +33,6 @@
// RobotAPI
#include <RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.h>
-
namespace armarx
{
@@ -41,13 +40,12 @@ namespace armarx
{
public:
-
struct Config
{
- float pos_reached_threshold = 10; // [mm]
- float ori_reached_threshold = 0.1; // [rad]
- float pos_near_threshold = 50; // [mm]
- float ori_near_threshold = 0.2; // [rad]
+ float pos_reached_threshold = 10; // [mm]
+ float ori_reached_threshold = 0.1; // [rad]
+ float pos_near_threshold = 50; // [mm]
+ float ori_near_threshold = 0.2; // [rad]
};
struct Target
@@ -57,73 +55,46 @@ namespace armarx
};
public:
+ ObstacleAvoidingPlatformUnitHelper(armarx::PlatformUnitInterfacePrx platform_unit,
+ VirtualRobot::RobotPtr robot);
- ObstacleAvoidingPlatformUnitHelper(
- armarx::PlatformUnitInterfacePrx platform_unit,
- VirtualRobot::RobotPtr robot);
-
- ObstacleAvoidingPlatformUnitHelper(
- armarx::PlatformUnitInterfacePrx platform_unit,
- VirtualRobot::RobotPtr robot,
- const Config& cfg);
+ ObstacleAvoidingPlatformUnitHelper(armarx::PlatformUnitInterfacePrx platform_unit,
+ VirtualRobot::RobotPtr robot,
+ const Config& cfg);
- virtual
- ~ObstacleAvoidingPlatformUnitHelper();
+ virtual ~ObstacleAvoidingPlatformUnitHelper();
- void
- setTarget(const Eigen::Vector2f& target_pos, float target_ori);
+ void setTarget(const Eigen::Vector2f& target_pos, float target_ori);
- void
- setTarget(const Target& target);
+ void setTarget(const Target& target);
- Target
- getCurrentTarget() const;
+ Target getCurrentTarget() const;
- void
- update();
+ void update();
- void
- setWaypoints(const std::vector<Target>& waypoints);
+ void setWaypoints(const std::vector<Target>& waypoints);
- void
- addWaypoint(const Eigen::Vector2f& waypoint_pos, float waypoint_ori);
+ void addWaypoint(const Eigen::Vector2f& waypoint_pos, float waypoint_ori);
- void
- addWaypoint(const Target& waypoint);
+ void addWaypoint(const Target& waypoint);
- bool
- isLastWaypoint()
- const;
+ bool isLastWaypoint() const;
- bool
- isCurrentTargetNear()
- const;
+ bool isCurrentTargetNear() const;
- bool
- isCurrentTargetReached()
- const;
+ bool isCurrentTargetReached() const;
- bool
- isFinalTargetNear()
- const;
+ bool isFinalTargetNear() const;
- bool
- isFinalTargetReached()
- const;
+ bool isFinalTargetReached() const;
- void
- setMaxVelocities(float max_vel, float max_angular_vel);
+ void setMaxVelocities(float max_vel, float max_angular_vel);
- float
- getPositionError()
- const;
+ float getPositionError() const;
- float
- getOrientationError()
- const;
+ float getOrientationError() const;
private:
-
armarx::PlatformUnitInterfacePrx m_platform_unit;
VirtualRobot::RobotPtr m_robot;
@@ -133,7 +104,6 @@ namespace armarx
bool m_waypoint_changed = false;
Config m_cfg;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index 7fbe6b3f88261e2815f1b8ca11a397c00968424d..8494ebb3d512bac664cb96870e692b85487e1105 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -23,31 +23,47 @@
#include "PositionControllerHelper.h"
-#include <ArmarXCore/core/time/TimeUtil.h>
-
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
+#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
-#include <VirtualRobot/MathTools.h>
-#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
namespace armarx
{
- PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target)
- : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
+ PositionControllerHelper::PositionControllerHelper(
+ const VirtualRobot::RobotNodePtr& tcp,
+ const VelocityControllerHelperPtr& velocityControllerHelper,
+ const Eigen::Matrix4f& target) :
+ posController(tcp),
+ velocityControllerHelper(velocityControllerHelper),
+ currentWaypointIndex(0)
{
waypoints.push_back(target);
}
- PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints)
- : posController(tcp), velocityControllerHelper(velocityControllerHelper), waypoints(waypoints), currentWaypointIndex(0)
+ PositionControllerHelper::PositionControllerHelper(
+ const VirtualRobot::RobotNodePtr& tcp,
+ const VelocityControllerHelperPtr& velocityControllerHelper,
+ const std::vector<Eigen::Matrix4f>& waypoints) :
+ posController(tcp),
+ velocityControllerHelper(velocityControllerHelper),
+ waypoints(waypoints),
+ currentWaypointIndex(0)
{
}
- PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints)
- : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
+ PositionControllerHelper::PositionControllerHelper(
+ const VirtualRobot::RobotNodePtr& tcp,
+ const VelocityControllerHelperPtr& velocityControllerHelper,
+ const std::vector<PosePtr>& waypoints) :
+ posController(tcp),
+ velocityControllerHelper(velocityControllerHelper),
+ currentWaypointIndex(0)
{
for (const PosePtr& pose : waypoints)
{
@@ -55,7 +71,8 @@ namespace armarx
}
}
- void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBase &config)
+ void
+ PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBase& config)
{
thresholdOrientationNear = config.thresholdOrientationNear;
thresholdOrientationReached = config.thresholdOrientationReached;
@@ -67,18 +84,21 @@ namespace armarx
posController.maxPosVel = config.maxPosVel;
}
- void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBasePtr& config)
+ void
+ PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBasePtr& config)
{
readConfig(*config);
}
- void PositionControllerHelper::update()
+ void
+ PositionControllerHelper::update()
{
updateRead();
updateWrite();
}
- void PositionControllerHelper::updateRead()
+ void
+ PositionControllerHelper::updateRead()
{
if (!isLastWaypoint() && isCurrentTargetNear())
{
@@ -86,7 +106,8 @@ namespace armarx
}
}
- void PositionControllerHelper::updateWrite()
+ void
+ PositionControllerHelper::updateWrite()
{
auto target = getCurrentTarget();
Eigen::VectorXf cv = posController.calculate(target, VirtualRobot::IKSolver::All);
@@ -97,13 +118,15 @@ namespace armarx
}
}
- void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
+ void
+ PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
{
this->waypoints = waypoints;
currentWaypointIndex = 0;
}
- void PositionControllerHelper::setNewWaypoints(const std::vector<PosePtr>& waypoints)
+ void
+ PositionControllerHelper::setNewWaypoints(const std::vector<PosePtr>& waypoints)
{
this->waypoints.clear();
for (const PosePtr& pose : waypoints)
@@ -113,35 +136,42 @@ namespace armarx
currentWaypointIndex = 0;
}
- void PositionControllerHelper::addWaypoint(const Eigen::Matrix4f& waypoint)
+ void
+ PositionControllerHelper::addWaypoint(const Eigen::Matrix4f& waypoint)
{
this->waypoints.push_back(waypoint);
}
- void PositionControllerHelper::setTarget(const Eigen::Matrix4f& target)
+ void
+ PositionControllerHelper::setTarget(const Eigen::Matrix4f& target)
{
waypoints.clear();
waypoints.push_back(target);
currentWaypointIndex = 0;
}
- void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
+ void
+ PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
{
this->feedForwardVelocity = feedForwardVelocity;
}
- void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
+ void
+ PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos,
+ const Eigen::Vector3f& feedForwardVelocityOri)
{
feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
}
- void PositionControllerHelper::clearFeedForwardVelocity()
+ void
+ PositionControllerHelper::clearFeedForwardVelocity()
{
feedForwardVelocity = Eigen::Vector6f::Zero();
}
- void PositionControllerHelper::immediateHardStop(bool clearTargets)
+ void
+ PositionControllerHelper::immediateHardStop(bool clearTargets)
{
velocityControllerHelper->controller->immediateHardStop();
if (clearTargets)
@@ -150,52 +180,64 @@ namespace armarx
}
}
- float PositionControllerHelper::getPositionError() const
+ float
+ PositionControllerHelper::getPositionError() const
{
return posController.getPositionError(getCurrentTarget());
}
- float PositionControllerHelper::getOrientationError() const
+ float
+ PositionControllerHelper::getOrientationError() const
{
return posController.getOrientationError(getCurrentTarget());
}
- bool PositionControllerHelper::isCurrentTargetReached() const
+ bool
+ PositionControllerHelper::isCurrentTargetReached() const
{
- return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+ return getPositionError() < thresholdPositionReached &&
+ getOrientationError() < thresholdOrientationReached;
}
- bool PositionControllerHelper::isCurrentTargetNear() const
+ bool
+ PositionControllerHelper::isCurrentTargetNear() const
{
- return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+ return getPositionError() < thresholdPositionNear &&
+ getOrientationError() < thresholdOrientationNear;
}
- bool PositionControllerHelper::isFinalTargetReached() const
+ bool
+ PositionControllerHelper::isFinalTargetReached() const
{
return isLastWaypoint() && isCurrentTargetReached();
}
- bool PositionControllerHelper::isFinalTargetNear() const
+ bool
+ PositionControllerHelper::isFinalTargetNear() const
{
return isLastWaypoint() && isCurrentTargetNear();
}
- bool PositionControllerHelper::isLastWaypoint() const
+ bool
+ PositionControllerHelper::isLastWaypoint() const
{
return currentWaypointIndex + 1 >= waypoints.size();
}
- const Eigen::Matrix4f& PositionControllerHelper::getCurrentTarget() const
+ const Eigen::Matrix4f&
+ PositionControllerHelper::getCurrentTarget() const
{
return waypoints.at(currentWaypointIndex);
}
- const Eigen::Vector3f PositionControllerHelper::getCurrentTargetPosition() const
+ const Eigen::Vector3f
+ PositionControllerHelper::getCurrentTargetPosition() const
{
return math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
}
- size_t PositionControllerHelper::skipToClosestWaypoint(float rad2mmFactor)
+ size_t
+ PositionControllerHelper::skipToClosestWaypoint(float rad2mmFactor)
{
float dist = FLT_MAX;
size_t minIndex = 0;
@@ -214,25 +256,34 @@ namespace armarx
return currentWaypointIndex;
}
- void PositionControllerHelper::setNullSpaceControl(bool enabled)
+ void
+ PositionControllerHelper::setNullSpaceControl(bool enabled)
{
velocityControllerHelper->setNullSpaceControl(enabled);
}
- std::string PositionControllerHelper::getStatusText()
+ std::string
+ PositionControllerHelper::getStatusText()
{
std::stringstream ss;
ss.precision(2);
- ss << std::fixed << "Waypoint: " << (currentWaypointIndex + 1) << "/" << waypoints.size() << " distance: " << getPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
+ ss << std::fixed << "Waypoint: " << (currentWaypointIndex + 1) << "/" << waypoints.size()
+ << " distance: " << getPositionError() << " mm "
+ << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
return ss.str();
}
- bool PositionControllerHelper::OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::Matrix4f& target, const NullspaceOptimizationArgs& args)
+ bool
+ PositionControllerHelper::OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp,
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::Matrix4f& target,
+ const NullspaceOptimizationArgs& args)
{
CartesianPositionController posHelper(tcp);
CartesianVelocityControllerPtr cartesianVelocityController;
- cartesianVelocityController.reset(new CartesianVelocityController(rns, tcp, args.invJacMethod));
+ cartesianVelocityController.reset(
+ new CartesianVelocityController(rns, tcp, args.invJacMethod));
float errorOriInitial = posHelper.getOrientationError(target);
float errorPosInitial = posHelper.getPositionError(target);
@@ -240,18 +291,21 @@ namespace armarx
float stepLength = args.stepLength;
float eps = args.eps;
- std::vector<float> initialJointAngles = rns->getJointValues();
+ std::vector<float> initialJointAngles = rns->getJointValues();
for (int i = 0; i < args.loops; i++)
{
Eigen::VectorXf tcpVelocities = posHelper.calculate(target, args.cartesianSelection);
- Eigen::VectorXf nullspaceVel = cartesianVelocityController->calculateJointLimitAvoidance(); // calculateNullspaceVelocity(tcpVelocities, 1.0f, vrmode);
+ Eigen::VectorXf nullspaceVel =
+ cartesianVelocityController
+ ->calculateJointLimitAvoidance(); // calculateNullspaceVelocity(tcpVelocities, 1.0f, vrmode);
float nullspaceLen = nullspaceVel.norm();
if (nullspaceLen > stepLength)
{
nullspaceVel = nullspaceVel / nullspaceLen * stepLength;
}
- Eigen::VectorXf jointVelocities = cartesianVelocityController->calculate(tcpVelocities, nullspaceVel, args.cartesianSelection);
+ Eigen::VectorXf jointVelocities = cartesianVelocityController->calculate(
+ tcpVelocities, nullspaceVel, args.cartesianSelection);
//jointVelocities = jointVelocities * stepLength;
float len = jointVelocities.norm();
@@ -262,7 +316,8 @@ namespace armarx
for (size_t n = 0; n < rns->getSize(); n++)
{
- rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jointVelocities(n));
+ rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() +
+ jointVelocities(n));
}
if (len < eps)
{
@@ -272,7 +327,8 @@ namespace armarx
float errorOriAfter = posHelper.getOrientationError(target);
float errorPosAfter = posHelper.getPositionError(target);
- if (errorOriAfter < errorOriInitial + 1.f / 180 * M_PI && errorPosAfter < errorPosInitial + 1.f)
+ if (errorOriAfter < errorOriInitial + 1.f / 180 * M_PI &&
+ errorPosAfter < errorPosInitial + 1.f)
{
return true;
}
@@ -282,4 +338,4 @@ namespace armarx
return false;
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index 279242ca5638f3643674bdcc13cabcba593b24d7..33f196b9edd6474d3373d9f5816684bdbcd7de4f 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -25,20 +25,19 @@
#include <Eigen/Dense>
-#include "VelocityControllerHelper.h"
-
-#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/IK/JacobiProvider.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include "VelocityControllerHelper.h"
+
namespace Eigen
{
using Vector6f = Matrix<float, 6, 1>;
}
-
namespace armarx
{
@@ -48,9 +47,15 @@ namespace armarx
class PositionControllerHelper
{
public:
- PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target);
- PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints);
- PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints);
+ PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp,
+ const VelocityControllerHelperPtr& velocityControllerHelper,
+ const Eigen::Matrix4f& target);
+ PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp,
+ const VelocityControllerHelperPtr& velocityControllerHelper,
+ const std::vector<Eigen::Matrix4f>& waypoints);
+ PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp,
+ const VelocityControllerHelperPtr& velocityControllerHelper,
+ const std::vector<PosePtr>& waypoints);
void readConfig(const CartesianPositionControllerConfigBase& config);
void readConfig(const CartesianPositionControllerConfigBasePtr& config);
@@ -68,7 +73,8 @@ namespace armarx
void addWaypoint(const Eigen::Matrix4f& waypoint);
void setTarget(const Eigen::Matrix4f& target);
void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
- void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
+ void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos,
+ const Eigen::Vector3f& feedForwardVelocityOri);
void clearFeedForwardVelocity();
void immediateHardStop(bool clearTargets = true);
@@ -94,14 +100,19 @@ namespace armarx
struct NullspaceOptimizationArgs
{
- NullspaceOptimizationArgs() {}
+ NullspaceOptimizationArgs()
+ {
+ }
+
int loops = 100;
float stepLength = 0.05f;
float eps = 0.001;
float maxOriErrorIncrease = 1.f / 180 * M_PI;
float maxPoseErrorIncrease = 1;
- VirtualRobot::IKSolver::CartesianSelection cartesianSelection = VirtualRobot::IKSolver::All;
- VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVD;
+ VirtualRobot::IKSolver::CartesianSelection cartesianSelection =
+ VirtualRobot::IKSolver::All;
+ VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod =
+ VirtualRobot::JacobiProvider::eSVD;
};
/**
@@ -112,7 +123,11 @@ namespace armarx
* @param args
* @return
*/
- static bool OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::Matrix4f& target, const NullspaceOptimizationArgs& args = NullspaceOptimizationArgs());
+ static bool
+ OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp,
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::Matrix4f& target,
+ const NullspaceOptimizationArgs& args = NullspaceOptimizationArgs());
CartesianPositionController posController;
VelocityControllerHelperPtr velocityControllerHelper;
@@ -127,4 +142,4 @@ namespace armarx
Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
bool autoClearFeedForward = true;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 602cda54231704749972b62390cf0323efe043b2..3a9aef8fe7ba9aff0966eb1acf1161554f8aed7d 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -31,20 +31,22 @@
#include <ArmarXCore/util/CPPUtility/trace.h>
// #include <ArmarXCore/core/system/ArmarXDataPath.cpp>
+#include <algorithm>
+
+#include <Eigen/Dense>
+
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
-#include <Eigen/Dense>
-#include <algorithm>
-
namespace armarx
{
- void RobotNameHelper::writeRobotInfoNode(const RobotInfoNodePtr& n,
- std::ostream& str,
- std::size_t indent,
- char indentChar)
+ void
+ RobotNameHelper::writeRobotInfoNode(const RobotInfoNodePtr& n,
+ std::ostream& str,
+ std::size_t indent,
+ char indentChar)
{
const std::string ind(4 * indent, indentChar);
if (!n)
@@ -58,12 +60,14 @@ namespace armarx
writeRobotInfoNode(c, str, indent + 1);
}
}
- const RobotInfoNodePtr& RobotNameHelper::getRobotInfoNodePtr() const
+
+ const RobotInfoNodePtr&
+ RobotNameHelper::getRobotInfoNodePtr() const
{
return robotInfo;
}
- const std::string RobotNameHelper::LocationLeft = "Left";
+ const std::string RobotNameHelper::LocationLeft = "Left";
const std::string RobotNameHelper::LocationRight = "Right";
RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo,
@@ -80,7 +84,8 @@ namespace armarx
profiles.emplace_back(""); // for matching the default/root
}
- std::string RobotNameHelper::select(const std::string& path) const
+ std::string
+ RobotNameHelper::select(const std::string& path) const
{
ARMARX_TRACE;
Node node = root;
@@ -98,26 +103,28 @@ namespace armarx
return node.value();
}
- RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo,
- const StatechartProfilePtr& profile)
+ RobotNameHelperPtr
+ RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
{
return RobotNameHelperPtr(new RobotNameHelper(robotInfo, profile));
}
- RobotNameHelperPtr RobotNameHelper::Create(const std::string& robotXmlFilename,
- const StatechartProfilePtr& profile)
+ RobotNameHelperPtr
+ RobotNameHelper::Create(const std::string& robotXmlFilename,
+ const StatechartProfilePtr& profile)
{
- RapidXmlReaderPtr reader = RapidXmlReader::FromFile(robotXmlFilename);
+ RapidXmlReaderPtr reader = RapidXmlReader::FromFile(robotXmlFilename);
RapidXmlReaderNode robotNode = reader->getRoot("Robot");
return Create(readRobotInfo(robotNode.first_node("RobotInfo")), profile);
}
- RobotInfoNodePtr RobotNameHelper::readRobotInfo(const RapidXmlReaderNode& infoNode)
+ RobotInfoNodePtr
+ RobotNameHelper::readRobotInfo(const RapidXmlReaderNode& infoNode)
{
- const std::string name = infoNode.name();
+ const std::string name = infoNode.name();
const std::string profile = infoNode.attribute_value_or_default("profile", "");
- const std::string value = infoNode.attribute_value_or_default("value", "");
+ const std::string value = infoNode.attribute_value_or_default("value", "");
const auto nodes = infoNode.nodes();
@@ -127,15 +134,13 @@ namespace armarx
std::transform(nodes.begin(),
nodes.end(),
std::back_inserter(children),
- [](const auto & childNode)
- {
- return readRobotInfo(childNode);
- });
+ [](const auto& childNode) { return readRobotInfo(childNode); });
return new RobotInfoNode(name, profile, value, children);
}
- Arm RobotNameHelper::getArm(const std::string& side) const
+ Arm
+ RobotNameHelper::getArm(const std::string& side) const
{
return Arm(shared_from_this(), side);
}
@@ -146,21 +151,25 @@ namespace armarx
return RobotArm(Arm(shared_from_this(), side), robot);
}
- std::shared_ptr<const RobotNameHelper> Arm::getRobotNameHelper() const
+ std::shared_ptr<const RobotNameHelper>
+ Arm::getRobotNameHelper() const
{
return rnh;
}
- RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo) : robotInfo(robotInfo) {}
+ RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo) : robotInfo(robotInfo)
+ {
+ }
- std::string RobotNameHelper::Node::value()
+ std::string
+ RobotNameHelper::Node::value()
{
checkValid();
return robotInfo->value;
}
- RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name,
- const std::vector<std::string>& profiles)
+ RobotNameHelper::Node
+ RobotNameHelper::Node::select(const std::string& name, const std::vector<std::string>& profiles)
{
ARMARX_TRACE;
if (!isValid())
@@ -185,12 +194,14 @@ namespace armarx
return Node(nullptr);
}
- bool RobotNameHelper::Node::isValid()
+ bool
+ RobotNameHelper::Node::isValid()
{
return robotInfo ? true : false;
}
- void RobotNameHelper::Node::checkValid()
+ void
+ RobotNameHelper::Node::checkValid()
{
if (!isValid())
{
@@ -202,97 +213,113 @@ namespace armarx
}
}
- std::string Arm::getSide() const
+ std::string
+ Arm::getSide() const
{
return side;
}
- std::string Arm::getKinematicChain() const
+ std::string
+ Arm::getKinematicChain() const
{
ARMARX_TRACE;
return select("KinematicChain");
}
- std::string Arm::getTorsoKinematicChain() const
+ std::string
+ Arm::getTorsoKinematicChain() const
{
ARMARX_TRACE;
return select("TorsoKinematicChain");
}
- std::string Arm::getTCP() const
+ std::string
+ Arm::getTCP() const
{
ARMARX_TRACE;
return select("TCP");
}
- std::string Arm::getForceTorqueSensor() const
+ std::string
+ Arm::getForceTorqueSensor() const
{
ARMARX_TRACE;
return select("ForceTorqueSensor");
}
- std::string Arm::getForceTorqueSensorFrame() const
+ std::string
+ Arm::getForceTorqueSensorFrame() const
{
ARMARX_TRACE;
return select("ForceTorqueSensorFrame");
}
- std::string Arm::getEndEffector() const
+ std::string
+ Arm::getEndEffector() const
{
ARMARX_TRACE;
return select("EndEffector");
}
- std::string Arm::getMemoryHandName() const
+ std::string
+ Arm::getMemoryHandName() const
{
ARMARX_TRACE;
return select("MemoryHandName");
}
- std::string Arm::getHandControllerName() const
+ std::string
+ Arm::getHandControllerName() const
{
ARMARX_TRACE;
return select("HandControllerName");
}
- std::string Arm::getHandUnitName() const
+ std::string
+ Arm::getHandUnitName() const
{
ARMARX_TRACE;
return select("HandUnitName");
}
- std::string Arm::getHandRootNode() const
+ std::string
+ Arm::getHandRootNode() const
{
ARMARX_TRACE;
return select("HandRootNode");
}
- std::string Arm::getHandModelPath() const
+ std::string
+ Arm::getHandModelPath() const
{
ARMARX_TRACE;
return select("HandModelPath");
}
- std::string Arm::getAbsoluteHandModelPath() const
+ std::string
+ Arm::getAbsoluteHandModelPath() const
{
ArmarXDataPath::FindPackageAndAddDataPath(getHandModelPackage());
auto path = getHandModelPath();
return ArmarXDataPath::getAbsolutePath(path, path) ? path : "";
}
- std::string Arm::getHandModelPackage() const
+ std::string
+ Arm::getHandModelPackage() const
{
ARMARX_TRACE;
return select("HandModelPackage");
}
- std::string Arm::getPalmCollisionModel() const
+ std::string
+ Arm::getPalmCollisionModel() const
{
ARMARX_TRACE;
return select("PalmCollisionModel");
}
- std::string Arm::getFullHandCollisionModel() const
+ std::string
+ Arm::getFullHandCollisionModel() const
{
ARMARX_TRACE;
return select("FullHandCollisionModel");
@@ -305,53 +332,59 @@ namespace armarx
return RobotArm(*this, robot);
}
- Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh,
- const std::string& side) :
+ Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh, const std::string& side) :
rnh(rnh), side(side)
{
}
- std::string Arm::select(const std::string& path) const
+ std::string
+ Arm::select(const std::string& path) const
{
ARMARX_TRACE;
return rnh->select(side + "Arm/" + path);
}
- std::string RobotArm::getSide() const
+ std::string
+ RobotArm::getSide() const
{
ARMARX_TRACE;
return arm.getSide();
}
- VirtualRobot::RobotNodeSetPtr RobotArm::getKinematicChain() const
+ VirtualRobot::RobotNodeSetPtr
+ RobotArm::getKinematicChain() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNodeSet(arm.getKinematicChain());
}
- VirtualRobot::RobotNodeSetPtr RobotArm::getTorsoKinematicChain() const
+ VirtualRobot::RobotNodeSetPtr
+ RobotArm::getTorsoKinematicChain() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNodeSet(arm.getTorsoKinematicChain());
}
- VirtualRobot::RobotNodePtr RobotArm::getTCP() const
+ VirtualRobot::RobotNodePtr
+ RobotArm::getTCP() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNode(arm.getTCP());
}
- VirtualRobot::RobotNodePtr RobotArm::getPalmCollisionModel() const
+ VirtualRobot::RobotNodePtr
+ RobotArm::getPalmCollisionModel() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNode(arm.getPalmCollisionModel());
}
- VirtualRobot::TriMeshModelPtr RobotArm::getFullHandCollisionModel() const
+ VirtualRobot::TriMeshModelPtr
+ RobotArm::getFullHandCollisionModel() const
{
ARMARX_TRACE;
std::string abs;
@@ -360,14 +393,16 @@ namespace armarx
return VirtualRobot::TriMeshModel::FromFile(abs);
}
- VirtualRobot::RobotNodePtr RobotArm::getHandRootNode() const
+ VirtualRobot::RobotNodePtr
+ RobotArm::getHandRootNode() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNode(arm.getHandRootNode());
}
- Eigen::Matrix4f RobotArm::getTcp2HandRootTransform() const
+ Eigen::Matrix4f
+ RobotArm::getTcp2HandRootTransform() const
{
ARMARX_TRACE;
const auto tcp = getTCP();
@@ -377,23 +412,25 @@ namespace armarx
return tcp->getPoseInRootFrame().inverse() * hand->getPoseInRootFrame();
}
- const VirtualRobot::RobotPtr& RobotArm::getRobot() const
+ const VirtualRobot::RobotPtr&
+ RobotArm::getRobot() const
{
return robot;
}
- const Arm& RobotArm::getArm() const
+ const Arm&
+ RobotArm::getArm() const
{
return arm;
}
- RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot) :
- arm(arm), robot(robot)
+ RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot) : arm(arm), robot(robot)
{
ARMARX_CHECK_NOT_NULL(robot);
}
- const std::vector<std::string>& RobotNameHelper::getProfiles() const
+ const std::vector<std::string>&
+ RobotNameHelper::getProfiles() const
{
return profiles;
}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 86e01df9c63b146cf3d37d1b44768b4fa76b3166..3e9d53b928b8249e53bd352e5731e463cfc3c74f 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -23,10 +23,10 @@
#pragma once
-#include <RobotAPI/interface/core/RobotState.h>
-
#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/interface/core/RobotState.h>
+
namespace armarx
{
using RobotNameHelperPtr = std::shared_ptr<class RobotNameHelper>;
@@ -41,7 +41,6 @@ namespace armarx
friend class RobotNameHelper;
public:
-
std::string getSide() const;
std::string getKinematicChain() const;
@@ -93,7 +92,6 @@ namespace armarx
std::string select(const std::string& path) const;
private:
-
std::shared_ptr<const RobotNameHelper> rnh;
std::string side;
};
@@ -103,6 +101,7 @@ namespace armarx
friend class RobotNameHelper;
friend struct Arm;
+
public:
std::string getSide() const;
@@ -137,7 +136,6 @@ namespace armarx
RobotArm& operator=(const RobotArm&) = default;
private:
-
Arm arm;
VirtualRobot::RobotPtr robot;
};
@@ -145,10 +143,13 @@ namespace armarx
class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
{
public:
- static void
- writeRobotInfoNode(const RobotInfoNodePtr& n, std::ostream& str, std::size_t indent = 0, char indentChar = ' ');
+ static void writeRobotInfoNode(const RobotInfoNodePtr& n,
+ std::ostream& str,
+ std::size_t indent = 0,
+ char indentChar = ' ');
using Arm = armarx::Arm;
using RobotArm = armarx::RobotArm;
+
class Node
{
public:
@@ -171,12 +172,14 @@ namespace armarx
std::string select(const std::string& path) const;
- static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
+ static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo,
+ const StatechartProfilePtr& profile);
- static RobotNameHelperPtr
- Create(const std::string& robotXmlFilename, const StatechartProfilePtr& profile = nullptr);
+ static RobotNameHelperPtr Create(const std::string& robotXmlFilename,
+ const StatechartProfilePtr& profile = nullptr);
- RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile = nullptr);
+ RobotNameHelper(const RobotInfoNodePtr& robotInfo,
+ const StatechartProfilePtr& profile = nullptr);
RobotNameHelper(RobotNameHelper&&) = default;
@@ -195,10 +198,10 @@ namespace armarx
const RobotInfoNodePtr& getRobotInfoNodePtr() const;
private:
-
static RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
- RobotNameHelper& self()
+ RobotNameHelper&
+ self()
{
return *this;
}
@@ -207,4 +210,4 @@ namespace armarx
std::vector<std::string> profiles;
RobotInfoNodePtr robotInfo;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h
index c476160c84e8ea782ff55c10f4ca890560abe550..501052b91191e38e4063c134fe0311d85744f50b 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h
@@ -22,7 +22,6 @@
#pragma once
-
namespace armarx
{
/**
@@ -39,7 +38,6 @@ namespace armarx
class RobotStatechartHelpers
{
public:
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
index 4fcd3e452040b179aa92e8a3928d32d05abaaf60..b636e50efd835318cdff73dec549900841653911 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
@@ -27,21 +27,28 @@
namespace armarx
{
- VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, const std::string& nodeSetName, const std::string& controllerName)
- : robotUnit(robotUnit), controllerName(controllerName)
+ VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit,
+ const std::string& nodeSetName,
+ const std::string& controllerName) :
+ robotUnit(robotUnit), controllerName(controllerName)
{
- config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, "", CartesianSelectionMode::eAll, 500, 1, 2, 1, 2);
+ config = new NJointCartesianVelocityControllerWithRampConfig(
+ nodeSetName, "", CartesianSelectionMode::eAll, 500, 1, 2, 1, 2);
init();
}
- VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string& controllerName)
- : robotUnit(robotUnit), controllerName(controllerName)
+ VelocityControllerHelper::VelocityControllerHelper(
+ const RobotUnitInterfacePrx& robotUnit,
+ NJointCartesianVelocityControllerWithRampConfigPtr config,
+ const std::string& controllerName) :
+ robotUnit(robotUnit), controllerName(controllerName)
{
this->config = config;
init();
}
- void VelocityControllerHelper::init()
+ void
+ VelocityControllerHelper::init()
{
NJointControllerInterfacePrx ctrl = robotUnit->getNJointController(controllerName);
if (ctrl)
@@ -50,19 +57,22 @@ namespace armarx
}
else
{
- ctrl = robotUnit->createOrReplaceNJointController("NJointCartesianVelocityControllerWithRamp", controllerName, config);
+ ctrl = robotUnit->createOrReplaceNJointController(
+ "NJointCartesianVelocityControllerWithRamp", controllerName, config);
controllerCreated = true;
}
controller = NJointCartesianVelocityControllerWithRampInterfacePrx::checkedCast(ctrl);
controller->activateController();
}
- void VelocityControllerHelper::setTargetVelocity(const Eigen::Vector6f& cv)
+ void
+ VelocityControllerHelper::setTargetVelocity(const Eigen::Vector6f& cv)
{
controller->setTargetVelocity(cv(0), cv(1), cv(2), cv(3), cv(4), cv(5));
}
- void VelocityControllerHelper::setNullSpaceControl(bool enabled)
+ void
+ VelocityControllerHelper::setNullSpaceControl(bool enabled)
{
if (enabled)
{
@@ -76,7 +86,8 @@ namespace armarx
}
}
- void VelocityControllerHelper::cleanup()
+ void
+ VelocityControllerHelper::cleanup()
{
if (controllerCreated)
{
@@ -89,4 +100,4 @@ namespace armarx
controller->deactivateController();
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
index ab47ffaccc480f999f214df3f5dbb908549f1ad7..c4be7d69793725b4b6860b86904342815fa73ccb 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
@@ -25,11 +25,11 @@
#include <memory>
+#include <Eigen/Dense>
+
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include <Eigen/Dense>
-
namespace armarx
{
class VelocityControllerHelper;
@@ -38,8 +38,12 @@ namespace armarx
class VelocityControllerHelper
{
public:
- VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, const std::string& nodeSetName, const std::string& controllerName);
- VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string& controllerName);
+ VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit,
+ const std::string& nodeSetName,
+ const std::string& controllerName);
+ VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit,
+ NJointCartesianVelocityControllerWithRampConfigPtr config,
+ const std::string& controllerName);
void init();
@@ -57,4 +61,4 @@ namespace armarx
float initialKp;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
index fb834d52c07e86f500d05c097d5a5d19616ee2c5..693b5492b301335972cfad53b0298abec2a4c1ff 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -74,8 +74,7 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
static_assert(sizeof(std::uint64_t) == sizeof(msgSequenceNbr));
const auto seq = static_cast<std::uint64_t>(msgSequenceNbr);
std::lock_guard g{_data_mutex};
- ARMARX_VERBOSE << deactivateSpam()
- << "received " << data.size() << " timesteps";
+ ARMARX_VERBOSE << deactivateSpam() << "received " << data.size() << " timesteps";
_data[seq] = data;
}
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h
index 60a03b5c98109b657adddcd48c55731c84cb2f63..849c32afb7850af6d44fd1620ffeeedd2b477a2b 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h
@@ -33,9 +33,11 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
{
TYPEDEF_PTRS_HANDLE(Receiver);
}
+
namespace armarx
{
TYPEDEF_PTRS_SHARED(RobotUnitDataStreamingReceiver);
+
/**
* @defgroup Library-RobotUnitDataStreamingReceiver RobotUnitDataStreamingReceiver
* @ingroup RobotAPI
@@ -50,17 +52,17 @@ namespace armarx
class RobotUnitDataStreamingReceiver
{
public:
- using clock_t = std::chrono::high_resolution_clock;
+ using clock_t = std::chrono::high_resolution_clock;
using timestep_t = RobotUnitDataStreaming::TimeStep;
- using entry_t = RobotUnitDataStreaming::DataEntry;
+ using entry_t = RobotUnitDataStreaming::DataEntry;
- template<class T>
+ template <class T>
struct DataEntryReader
{
- DataEntryReader() = default;
- DataEntryReader(DataEntryReader&&) = default;
- DataEntryReader(const DataEntryReader&) = default;
- DataEntryReader& operator=(DataEntryReader&&) = default;
+ DataEntryReader() = default;
+ DataEntryReader(DataEntryReader&&) = default;
+ DataEntryReader(const DataEntryReader&) = default;
+ DataEntryReader& operator=(DataEntryReader&&) = default;
DataEntryReader& operator=(const DataEntryReader&) = default;
DataEntryReader(const RobotUnitDataStreaming::DataEntry& k);
@@ -70,94 +72,102 @@ namespace armarx
void set(const RobotUnitDataStreaming::DataEntry& k);
T get(const RobotUnitDataStreaming::TimeStep& t) const;
T operator()(const RobotUnitDataStreaming::TimeStep& t) const;
+
private:
RobotUnitDataStreaming::DataEntry _key;
};
- RobotUnitDataStreamingReceiver(
- const ManagedIceObjectPtr& obj,
- const RobotUnitInterfacePrx& ru,
- const RobotUnitDataStreaming::Config& cfg);
+ RobotUnitDataStreamingReceiver(const ManagedIceObjectPtr& obj,
+ const RobotUnitInterfacePrx& ru,
+ const RobotUnitDataStreaming::Config& cfg);
~RobotUnitDataStreamingReceiver();
std::deque<timestep_t>& getDataBuffer();
const RobotUnitDataStreaming::DataStreamingDescription& getDataDescription() const;
std::string getDataDescriptionString() const;
- template<class T>
+ template <class T>
DataEntryReader<T> getDataEntryReader(const std::string& name) const;
- template<class T>
+ template <class T>
void getDataEntryReader(DataEntryReader<T>& e, const std::string& name) const;
//statics
static void VisitEntry(auto&& f, const timestep_t& st, const entry_t& e);
- template<class T>
+ template <class T>
static T GetAs(const timestep_t& st, const entry_t& e);
- template<class T>
+ template <class T>
static RobotUnitDataStreaming::DataEntryType ExpectedDataEntryType();
static void VisitEntries(auto&& f, const timestep_t& st, const auto& cont);
- private:
- ManagedIceObjectPtr _obj;
- RobotUnitInterfacePrx _ru;
- detail::RobotUnitDataStreamingReceiver::ReceiverPtr _receiver;
- RobotUnitDataStreaming::ReceiverPrx _proxy;
- RobotUnitDataStreaming::DataStreamingDescription _description;
- std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> _tmp_data_buffer;
- std::uint64_t _tmp_data_buffer_seq_id = 0;
- std::deque<RobotUnitDataStreaming::TimeStep> _data_buffer;
- long _last_iteration_id = -1;
+ private:
+ ManagedIceObjectPtr _obj;
+ RobotUnitInterfacePrx _ru;
+ detail::RobotUnitDataStreamingReceiver::ReceiverPtr _receiver;
+ RobotUnitDataStreaming::ReceiverPrx _proxy;
+ RobotUnitDataStreaming::DataStreamingDescription _description;
+ std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> _tmp_data_buffer;
+ std::uint64_t _tmp_data_buffer_seq_id = 0;
+ std::deque<RobotUnitDataStreaming::TimeStep> _data_buffer;
+ long _last_iteration_id = -1;
};
-}
+} // namespace armarx
+
//impl
namespace armarx
{
- template<class T> inline
- RobotUnitDataStreamingReceiver::DataEntryReader<T>::DataEntryReader(const RobotUnitDataStreaming::DataEntry& k)
+ template <class T>
+ inline RobotUnitDataStreamingReceiver::DataEntryReader<T>::DataEntryReader(
+ const RobotUnitDataStreaming::DataEntry& k)
{
set(k);
}
- template<class T> inline
- void
- RobotUnitDataStreamingReceiver::DataEntryReader<T>::set(const RobotUnitDataStreaming::DataEntry& k)
+ template <class T>
+ inline void
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>::set(
+ const RobotUnitDataStreaming::DataEntry& k)
{
- ARMARX_CHECK_EQUAL(RobotUnitDataStreamingReceiver::ExpectedDataEntryType<T>(),
- k.type) << "the key references a value of the wrong type!";
+ ARMARX_CHECK_EQUAL(RobotUnitDataStreamingReceiver::ExpectedDataEntryType<T>(), k.type)
+ << "the key references a value of the wrong type!";
_key = k;
}
- template<class T> inline
- RobotUnitDataStreamingReceiver::DataEntryReader<T>&
- RobotUnitDataStreamingReceiver::DataEntryReader<T>::operator=(const RobotUnitDataStreaming::DataEntry& k)
+ template <class T>
+ inline RobotUnitDataStreamingReceiver::DataEntryReader<T>&
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>::operator=(
+ const RobotUnitDataStreaming::DataEntry& k)
{
set(k);
return *this;
}
- template<class T> inline
- T
- RobotUnitDataStreamingReceiver::DataEntryReader<T>::get(const RobotUnitDataStreaming::TimeStep& t) const
+ template <class T>
+ inline T
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>::get(
+ const RobotUnitDataStreaming::TimeStep& t) const
{
return RobotUnitDataStreamingReceiver::GetAs<T>(t, _key);
}
- template<class T> inline
- T
- RobotUnitDataStreamingReceiver::DataEntryReader<T>::operator()(const RobotUnitDataStreaming::TimeStep& t) const
+ template <class T>
+ inline T
+ RobotUnitDataStreamingReceiver::DataEntryReader<T>::operator()(
+ const RobotUnitDataStreaming::TimeStep& t) const
{
return get(t);
}
- template<class T> inline
- void
- RobotUnitDataStreamingReceiver::getDataEntryReader(DataEntryReader<T>& e, const std::string& name) const
+ template <class T>
+ inline void
+ RobotUnitDataStreamingReceiver::getDataEntryReader(DataEntryReader<T>& e,
+ const std::string& name) const
{
e = _description.entries.at(name);
}
- template<class T> inline
- RobotUnitDataStreamingReceiver::DataEntryReader<T>
+
+ template <class T>
+ inline RobotUnitDataStreamingReceiver::DataEntryReader<T>
RobotUnitDataStreamingReceiver::getDataEntryReader(const std::string& name) const
{
DataEntryReader<T> r;
@@ -169,56 +179,70 @@ namespace armarx
RobotUnitDataStreamingReceiver::VisitEntry(auto&& f, const timestep_t& st, const entry_t& e)
{
using enum_t = RobotUnitDataStreaming::DataEntryType;
- // *INDENT-OFF*
- switch (e.type)
- {
- case enum_t::NodeTypeBool : f(st.bools .at(e.index)); break;
- case enum_t::NodeTypeByte : f(st.bytes .at(e.index)); break;
- case enum_t::NodeTypeShort : f(st.shorts .at(e.index)); break;
- case enum_t::NodeTypeInt : f(st.ints .at(e.index)); break;
- case enum_t::NodeTypeLong : f(st.longs .at(e.index)); break;
- case enum_t::NodeTypeFloat : f(st.floats .at(e.index)); break;
- case enum_t::NodeTypeDouble: f(st.doubles.at(e.index)); break;
- };
- // *INDENT-ON*
+ // *INDENT-OFF*
+ switch (e.type)
+ {
+ case enum_t::NodeTypeBool:
+ f(st.bools.at(e.index));
+ break;
+ case enum_t::NodeTypeByte:
+ f(st.bytes.at(e.index));
+ break;
+ case enum_t::NodeTypeShort:
+ f(st.shorts.at(e.index));
+ break;
+ case enum_t::NodeTypeInt:
+ f(st.ints.at(e.index));
+ break;
+ case enum_t::NodeTypeLong:
+ f(st.longs.at(e.index));
+ break;
+ case enum_t::NodeTypeFloat:
+ f(st.floats.at(e.index));
+ break;
+ case enum_t::NodeTypeDouble:
+ f(st.doubles.at(e.index));
+ break;
+ };
+ // *INDENT-ON*
}
- template<class T> inline
- T
+ template <class T>
+ inline T
RobotUnitDataStreamingReceiver::GetAs(const timestep_t& st, const entry_t& e)
{
using enum_t = RobotUnitDataStreaming::DataEntryType;
- if constexpr(std::is_same_v<bool, T>)
+ if constexpr (std::is_same_v<bool, T>)
{
ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeBool);
- return st.bools .at(e.index);
+ return st.bools.at(e.index);
}
- else if constexpr(std::is_same_v<Ice::Byte, T>)
+ else if constexpr (std::is_same_v<Ice::Byte, T>)
{
ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeByte);
- return st.bytes .at(e.index);
+ return st.bytes.at(e.index);
}
- else if constexpr(std::is_same_v<Ice::Short, T>)
+ else if constexpr (std::is_same_v<Ice::Short, T>)
{
ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeShort);
- return st.shorts .at(e.index);
+ return st.shorts.at(e.index);
}
- else if constexpr(std::is_same_v<Ice::Int, T>)
+ else if constexpr (std::is_same_v<Ice::Int, T>)
{
ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeInt);
- return st.ints .at(e.index);
+ return st.ints.at(e.index);
}
- else if constexpr(std::is_same_v<Ice::Long, T>)
+ else if constexpr (std::is_same_v<Ice::Long, T>)
{
ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeLong);
- return st.longs .at(e.index);
+ return st.longs.at(e.index);
}
- else if constexpr(std::is_same_v<Ice::Float, T>)
+ else if constexpr (std::is_same_v<Ice::Float, T>)
{
ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeFloat);
- return st.floats .at(e.index);
+ return st.floats.at(e.index);
}
- else if constexpr(std::is_same_v<Ice::Double, T>)
+ else if constexpr (std::is_same_v<Ice::Double, T>)
{
ARMARX_CHECK_EQUAL(e.type, enum_t::NodeTypeDouble);
return st.doubles.at(e.index);
@@ -229,36 +253,36 @@ namespace armarx
}
}
- template<class T> inline
- RobotUnitDataStreaming::DataEntryType
+ template <class T>
+ inline RobotUnitDataStreaming::DataEntryType
RobotUnitDataStreamingReceiver::ExpectedDataEntryType()
{
using enum_t = RobotUnitDataStreaming::DataEntryType;
- if constexpr(std::is_same_v<bool, T>)
+ if constexpr (std::is_same_v<bool, T>)
{
return enum_t::NodeTypeBool;
}
- else if constexpr(std::is_same_v<Ice::Byte, T>)
+ else if constexpr (std::is_same_v<Ice::Byte, T>)
{
return enum_t::NodeTypeByte;
}
- else if constexpr(std::is_same_v<Ice::Short, T>)
+ else if constexpr (std::is_same_v<Ice::Short, T>)
{
return enum_t::NodeTypeShort;
}
- else if constexpr(std::is_same_v<Ice::Int, T>)
+ else if constexpr (std::is_same_v<Ice::Int, T>)
{
return enum_t::NodeTypeInt;
}
- else if constexpr(std::is_same_v<Ice::Long, T>)
+ else if constexpr (std::is_same_v<Ice::Long, T>)
{
return enum_t::NodeTypeLong;
}
- else if constexpr(std::is_same_v<Ice::Float, T>)
+ else if constexpr (std::is_same_v<Ice::Float, T>)
{
return enum_t::NodeTypeFloat;
}
- else if constexpr(std::is_same_v<Ice::Double, T>)
+ else if constexpr (std::is_same_v<Ice::Double, T>)
{
return enum_t::NodeTypeDouble;
}
@@ -276,4 +300,4 @@ namespace armarx
VisitEntry(f, st, e);
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/RobotUnitDataStreamingReceiverTest.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/RobotUnitDataStreamingReceiverTest.cpp
index 08ac432e58223900d4f06545c71130570bd89d65..4704c1ed4df9b8143604abb7e28b98b79889973f 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/RobotUnitDataStreamingReceiverTest.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/test/RobotUnitDataStreamingReceiverTest.cpp
@@ -24,9 +24,10 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
#include "../RobotUnitDataStreamingReceiver.h"
+#include <RobotAPI/Test.h>
+
#include <iostream>
BOOST_AUTO_TEST_CASE(testExample)
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp
index 11336c50bf43eff78222fb2deefe9aef9779635e..985ac5a575db178bd7a95eafd89c32df644f666a 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.cpp
@@ -25,13 +25,14 @@
namespace armarx
{
- SimpleJsonLogger::SimpleJsonLogger(const std::string& filename, bool append)
- : autoflush(true)
+ SimpleJsonLogger::SimpleJsonLogger(const std::string& filename, bool append) : autoflush(true)
{
- file.open(filename.c_str(), append ? std::ios_base::app : std::ios_base::out | std::ios_base::trunc);
+ file.open(filename.c_str(),
+ append ? std::ios_base::app : std::ios_base::out | std::ios_base::trunc);
}
- void SimpleJsonLogger::log(JsonDataPtr entry)
+ void
+ SimpleJsonLogger::log(JsonDataPtr entry)
{
file << entry->toJsonString() << "\n";
if (autoflush)
@@ -40,17 +41,20 @@ namespace armarx
}
}
- void SimpleJsonLogger::log(const SimpleJsonLoggerEntry& entry)
+ void
+ SimpleJsonLogger::log(const SimpleJsonLoggerEntry& entry)
{
log(entry.obj);
}
- void SimpleJsonLogger::log(SimpleJsonLoggerEntryPtr entry)
+ void
+ SimpleJsonLogger::log(SimpleJsonLoggerEntryPtr entry)
{
log(entry->obj);
}
- void SimpleJsonLogger::close()
+ void
+ SimpleJsonLogger::close()
{
file.flush();
file.close();
@@ -63,4 +67,4 @@ namespace armarx
file.close();
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h
index 51ede587cf70f32b9ff388c481e78caf7d507c33..97e924da23529f324120879738bb70c5c3b28715 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h
@@ -23,16 +23,16 @@
#pragma once
-#include "SimpleJsonLoggerEntry.h"
-
-#include <iostream>
#include <fstream>
+#include <iostream>
+
+#include <Eigen/Dense>
#include <ArmarXGui/libraries/StructuralJson/JsonArray.h>
#include <ArmarXGui/libraries/StructuralJson/JsonData.h>
#include <ArmarXGui/libraries/StructuralJson/JsonObject.h>
-#include <Eigen/Dense>
+#include "SimpleJsonLoggerEntry.h"
namespace armarx
{
@@ -53,5 +53,4 @@ namespace armarx
bool autoflush;
std::ofstream file;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
index ecf7f69cdc41df7319c29a9fac28aa0362724a8d..1d37039c7c6baa39e7b6030f742bd7a477c910d7 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.cpp
@@ -22,62 +22,71 @@
*/
#include "SimpleJsonLoggerEntry.h"
+
#include <IceUtil/Time.h>
namespace armarx
{
- SimpleJsonLoggerEntry::SimpleJsonLoggerEntry()
- : obj(new JsonObject)
+ SimpleJsonLoggerEntry::SimpleJsonLoggerEntry() : obj(new JsonObject)
{
}
- void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Vector3f& vec)
+ void
+ SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Vector3f& vec)
{
obj->add(key, ToArr((Eigen::VectorXf)vec));
}
- void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::VectorXf& vec)
+ void
+ SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::VectorXf& vec)
{
obj->add(key, ToArr(vec));
}
- void SimpleJsonLoggerEntry::AddAsObj(const std::string& key, const Eigen::Vector3f& vec)
+ void
+ SimpleJsonLoggerEntry::AddAsObj(const std::string& key, const Eigen::Vector3f& vec)
{
obj->add(key, ToObj(vec));
}
- void SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Matrix4f& mat)
+ void
+ SimpleJsonLoggerEntry::AddAsArr(const std::string& key, const Eigen::Matrix4f& mat)
{
obj->add(key, ToArr(mat));
}
- void SimpleJsonLoggerEntry::AddMatrix(const std::string& key, const Eigen::MatrixXf& mat)
+ void
+ SimpleJsonLoggerEntry::AddMatrix(const std::string& key, const Eigen::MatrixXf& mat)
{
obj->add(key, MatrixToArr(mat));
}
- void SimpleJsonLoggerEntry::Add(const std::string& key, const std::string& value)
+ void
+ SimpleJsonLoggerEntry::Add(const std::string& key, const std::string& value)
{
obj->add(key, JsonValue::Create(value));
}
- void SimpleJsonLoggerEntry::Add(const std::string& key, float value)
+ void
+ SimpleJsonLoggerEntry::Add(const std::string& key, float value)
{
obj->add(key, JsonValue::Create(value));
}
- void SimpleJsonLoggerEntry::Add(const std::string& key, const std::vector<float>& value)
+ void
+ SimpleJsonLoggerEntry::Add(const std::string& key, const std::vector<float>& value)
{
obj->add(key, ToArr(value));
}
- void SimpleJsonLoggerEntry::Add(const std::string& key, const std::map<std::string, float>& value)
+ void
+ SimpleJsonLoggerEntry::Add(const std::string& key, const std::map<std::string, float>& value)
{
obj->add(key, ToObj(value));
}
-
- JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const Eigen::VectorXf& vec)
+ JsonArrayPtr
+ SimpleJsonLoggerEntry::ToArr(const Eigen::VectorXf& vec)
{
JsonArrayPtr arr(new JsonArray);
for (int i = 0; i < vec.rows(); i++)
@@ -87,7 +96,8 @@ namespace armarx
return arr;
}
- JsonArrayPtr SimpleJsonLoggerEntry::ToArr(const std::vector<float>& vec)
+ JsonArrayPtr
+ SimpleJsonLoggerEntry::ToArr(const std::vector<float>& vec)
{
JsonArrayPtr arr(new JsonArray);
for (float v : vec)
@@ -97,7 +107,8 @@ namespace armarx
return arr;
}
- JsonObjectPtr SimpleJsonLoggerEntry::ToObj(Eigen::Vector3f vec)
+ JsonObjectPtr
+ SimpleJsonLoggerEntry::ToObj(Eigen::Vector3f vec)
{
JsonObjectPtr obj(new JsonObject);
obj->add("x", JsonValue::Create(vec(0)));
@@ -106,7 +117,8 @@ namespace armarx
return obj;
}
- JsonObjectPtr SimpleJsonLoggerEntry::ToObj(const std::map<std::string, float>& value)
+ JsonObjectPtr
+ SimpleJsonLoggerEntry::ToObj(const std::map<std::string, float>& value)
{
JsonObjectPtr obj(new JsonObject);
for (const auto& pair : value)
@@ -116,7 +128,8 @@ namespace armarx
return obj;
}
- JsonArrayPtr SimpleJsonLoggerEntry::ToArr(Eigen::Matrix4f mat)
+ JsonArrayPtr
+ SimpleJsonLoggerEntry::ToArr(Eigen::Matrix4f mat)
{
JsonArrayPtr arr(new JsonArray);
JsonArrayPtr row1(new JsonArray);
@@ -152,7 +165,8 @@ namespace armarx
return arr;
}
- JsonArrayPtr SimpleJsonLoggerEntry::MatrixToArr(Eigen::MatrixXf mat)
+ JsonArrayPtr
+ SimpleJsonLoggerEntry::MatrixToArr(Eigen::MatrixXf mat)
{
JsonArrayPtr arr(new JsonArray);
for (int i = 0; i < mat.rows(); i++)
@@ -167,9 +181,10 @@ namespace armarx
return arr;
}
- void SimpleJsonLoggerEntry::AddTimestamp()
+ void
+ SimpleJsonLoggerEntry::AddTimestamp()
{
IceUtil::Time now = IceUtil::Time::now();
obj->add("timestamp", JsonValue::Create(now.toDateTime()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
index 117379e35616569b6b6e50b9f8d99dacaa181201..9034c7eaec22ca977971a3d9dd9b8bacd5eb0a34 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h
@@ -23,10 +23,12 @@
#pragma once
-#include <ArmarXGui/libraries/StructuralJson/JsonObject.h>
-#include <Eigen/Dense>
#include <map>
+#include <Eigen/Dense>
+
+#include <ArmarXGui/libraries/StructuralJson/JsonObject.h>
+
namespace armarx
{
class SimpleJsonLoggerEntry;
@@ -58,5 +60,4 @@ namespace armarx
JsonObjectPtr obj;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/SimpleJsonLogger/test/SimpleJsonLoggerTest.cpp b/source/RobotAPI/libraries/SimpleJsonLogger/test/SimpleJsonLoggerTest.cpp
index 15c777b73509c2ab2ccc7cd2ecc415819bb8152d..ca32d2b92696e8a3f2c94357b1bd34e70bc053c2 100644
--- a/source/RobotAPI/libraries/SimpleJsonLogger/test/SimpleJsonLoggerTest.cpp
+++ b/source/RobotAPI/libraries/SimpleJsonLogger/test/SimpleJsonLoggerTest.cpp
@@ -24,11 +24,12 @@
#define ARMARX_BOOST_TEST
-#include <ReactiveGrasping/Test.h>
#include "../SimpleJsonLogger.h"
#include <iostream>
+#include <ReactiveGrasping/Test.h>
+
BOOST_AUTO_TEST_CASE(testExample)
{
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp b/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp
index 88258722f663c186ae3aabe393bcf710c11c12c4..a5d81241f73b95aed689a21589c4704dd8456a0c 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp
@@ -1,16 +1,17 @@
#include "Track.h"
-
namespace armarx::trajectory
{
template <>
- void Track<VariantValue>::checkValueType(const VariantValue& value)
+ void
+ Track<VariantValue>::checkValueType(const VariantValue& value)
{
if (!empty() && value.index() != keyframes.front().value.index())
{
- throw error::WrongValueTypeInKeyframe(id, value.index(), keyframes.front().value.index());
+ throw error::WrongValueTypeInKeyframe(
+ id, value.index(), keyframes.front().value.index());
}
}
-}
+} // namespace armarx::trajectory
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Track.h b/source/RobotAPI/libraries/SimpleTrajectory/Track.h
index 6658b8df8856589c99fbe3cdfe3cfb2bc0f292ce..6062d1d788d8c5ae5f1c82f2f02354b75da1fd84 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/Track.h
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Track.h
@@ -3,7 +3,6 @@
#include "VariantValue.h"
#include "interpolate/linear.h"
-
namespace armarx::trajectory
{
@@ -15,16 +14,16 @@ namespace armarx::trajectory
{
/// Constructor.
Keyframe(float time, const ValueT& value) : time(time), value(value)
- {}
+ {
+ }
- float time; ///< The time on the timeline.
+ float time; ///< The time on the timeline.
ValueT value; ///< The value.
};
/// A keyframe with of type TValue.
using VariantKeyframe = Keyframe<VariantValue>;
-
/**
* @brief A track represents the timeline of a single value, identified by a track ID.
* A track is comprised of a sequence of keyframes and used to call a
@@ -40,17 +39,15 @@ namespace armarx::trajectory
struct KeyframeProxy;
public:
-
/// Construct a track with given ID (and no update function).
- Track(const TrackID& id) :
- id(id)
- {}
+ Track(const TrackID& id) : id(id)
+ {
+ }
/// Construct a track with given ID and update function.
- Track(const TrackID& id, UpdateFunc updateFunc) :
- id(id), updateFunc(updateFunc)
- {}
-
+ Track(const TrackID& id, UpdateFunc updateFunc) : id(id), updateFunc(updateFunc)
+ {
+ }
/// Indicate whether this track does not contain any keyframes.
bool empty() const;
@@ -85,7 +82,6 @@ namespace armarx::trajectory
private:
-
/// Sort the keyframes.
void sortKeyframes();
@@ -105,7 +101,6 @@ namespace armarx::trajectory
public:
-
/// A proxy allowing for adding keyframes by: `track[time] = value;`
struct KeyframeProxy
{
@@ -113,9 +108,9 @@ namespace armarx::trajectory
/// Get the value at time.
operator ValueT() const;
/// Add a keyframe on assignment.
- void operator= (const ValueT& value);
+ void operator=(const ValueT& value);
/// Add a keyframe on assignment.
- void operator= (const KeyframeProxy& value);
+ void operator=(const KeyframeProxy& value);
/// Get the value at time.
ValueT value() const;
@@ -128,26 +123,28 @@ namespace armarx::trajectory
Track& track;
float time;
};
-
};
/// A track with value type TValue.
using VariantTrack = Track<VariantValue>;
template <typename V>
- bool Track<V>::empty() const
+ bool
+ Track<V>::empty() const
{
return keyframes.empty();
}
- template<typename V>
- void Track<V>::clear()
+ template <typename V>
+ void
+ Track<V>::clear()
{
keyframes.clear();
}
template <typename V>
- void Track<V>::addKeyframe(const Keyframe<V>& keyframe)
+ void
+ Track<V>::addKeyframe(const Keyframe<V>& keyframe)
{
checkValueType(keyframe.value);
keyframes.push_back(keyframe);
@@ -155,31 +152,35 @@ namespace armarx::trajectory
}
template <typename V>
- void Track<V>::addKeyframe(float time, const V& value)
+ void
+ Track<V>::addKeyframe(float time, const V& value)
{
addKeyframe(Keyframe<V>(time, value));
}
- template<typename V>
- auto Track<V>::operator[](float time) -> KeyframeProxy
+ template <typename V>
+ auto
+ Track<V>::operator[](float time) -> KeyframeProxy
{
return KeyframeProxy{*this, time};
}
template <typename V>
- void Track<V>::sortKeyframes()
+ void
+ Track<V>::sortKeyframes()
{
- std::sort(keyframes.begin(), keyframes.end(),
+ std::sort(keyframes.begin(),
+ keyframes.end(),
[](const Keyframe<V>& lhs, const Keyframe<V>& rhs)
- {
- return lhs.time < rhs.time;
- });
+ { return lhs.time < rhs.time; });
}
/// In general, do nothing.
template <typename V>
- void Track<V>::checkValueType(const V&)
- {}
+ void
+ Track<V>::checkValueType(const V&)
+ {
+ }
/**
* If the track is not empty, check whether the given type of the given
@@ -190,9 +191,9 @@ namespace armarx::trajectory
template <>
void Track<VariantValue>::checkValueType(const VariantValue& value);
-
template <typename V>
- V Track<V>::at(float time) const
+ V
+ Track<V>::at(float time) const
{
if (empty())
{
@@ -230,9 +231,9 @@ namespace armarx::trajectory
return interpolate::linear<V>(t, kf1.value, kf2.value);
}
-
template <typename V>
- void Track<V>::update(float time, bool ignoreIfEmpty)
+ void
+ Track<V>::update(float time, bool ignoreIfEmpty)
{
if (updateFunc && !(ignoreIfEmpty && empty()))
{
@@ -240,11 +241,10 @@ namespace armarx::trajectory
}
}
-
- template<typename V>
- Track<V>::KeyframeProxy::KeyframeProxy(Track& track, float time) :
- track(track), time(time)
- {}
+ template <typename V>
+ Track<V>::KeyframeProxy::KeyframeProxy(Track& track, float time) : track(track), time(time)
+ {
+ }
template <typename V>
Track<V>::KeyframeProxy::operator V() const
@@ -253,25 +253,29 @@ namespace armarx::trajectory
}
template <typename V>
- void Track<V>::KeyframeProxy::operator=(const V& value)
+ void
+ Track<V>::KeyframeProxy::operator=(const V& value)
{
track.addKeyframe(time, value);
}
template <typename V>
- void Track<V>::KeyframeProxy::operator=(const KeyframeProxy& other)
+ void
+ Track<V>::KeyframeProxy::operator=(const KeyframeProxy& other)
{
track.addKeyframe(time, other.value());
}
template <typename V>
- auto Track<V>::KeyframeProxy::value() const -> V
+ auto
+ Track<V>::KeyframeProxy::value() const -> V
{
return track.at(time);
}
template <typename ValueT>
- std::ostream& operator<<(std::ostream& os, const Track<ValueT>& track)
+ std::ostream&
+ operator<<(std::ostream& os, const Track<ValueT>& track)
{
os << "Track '" << track.id << "' with " << track.keyframes.size() << " keyframes: [";
for (const Keyframe<ValueT>& kf : track.keyframes)
@@ -281,4 +285,4 @@ namespace armarx::trajectory
return os << "]";
}
-}
+} // namespace armarx::trajectory
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp
index 3dad904583eca19ac645d364ec045851f1045805..f67b08e8c96cd965575729cf29a56fbb9398e930 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp
@@ -3,36 +3,42 @@
using namespace armarx::trajectory;
-
Trajectory::Trajectory()
-{}
+{
+}
-void Trajectory::clear()
+void
+Trajectory::clear()
{
tracks.clear();
}
-VariantTrack& Trajectory::addTrack(const TrackID& id)
+VariantTrack&
+Trajectory::addTrack(const TrackID& id)
{
- return tracks.emplace(id, VariantTrack {id}).first->second;
+ return tracks.emplace(id, VariantTrack{id}).first->second;
}
-VariantTrack& Trajectory::addTrack(const TrackID& id, const VariantTrack::UpdateFunc& updateFunc)
+VariantTrack&
+Trajectory::addTrack(const TrackID& id, const VariantTrack::UpdateFunc& updateFunc)
{
- return tracks.emplace(id, VariantTrack {id, updateFunc}).first->second;
+ return tracks.emplace(id, VariantTrack{id, updateFunc}).first->second;
}
-void Trajectory::addKeyframe(const TrackID& id, const VariantKeyframe& keyframe)
+void
+Trajectory::addKeyframe(const TrackID& id, const VariantKeyframe& keyframe)
{
(*this)[id].addKeyframe(keyframe);
}
-void Trajectory::addKeyframe(const TrackID& id, float time, const VariantValue& value)
+void
+Trajectory::addKeyframe(const TrackID& id, float time, const VariantValue& value)
{
- addKeyframe(id, VariantKeyframe {time, value});
+ addKeyframe(id, VariantKeyframe{time, value});
}
-void Trajectory::update(float time, bool ignoreEmptyTracks)
+void
+Trajectory::update(float time, bool ignoreEmptyTracks)
{
for (auto& it : tracks)
{
@@ -40,7 +46,8 @@ void Trajectory::update(float time, bool ignoreEmptyTracks)
}
}
-VariantTrack& Trajectory::operator[](const TrackID& id)
+VariantTrack&
+Trajectory::operator[](const TrackID& id)
{
try
{
@@ -52,7 +59,8 @@ VariantTrack& Trajectory::operator[](const TrackID& id)
}
}
-const VariantTrack& Trajectory::operator[](const TrackID& id) const
+const VariantTrack&
+Trajectory::operator[](const TrackID& id) const
{
try
{
@@ -64,7 +72,8 @@ const VariantTrack& Trajectory::operator[](const TrackID& id) const
}
}
-std::ostream& armarx::trajectory::operator<<(std::ostream& os, const Trajectory& trajectory)
+std::ostream&
+armarx::trajectory::operator<<(std::ostream& os, const Trajectory& trajectory)
{
os << "Trajectory with " << trajectory.tracks.size() << " tracks: ";
for (const auto& [name, track] : trajectory.tracks)
@@ -74,32 +83,27 @@ std::ostream& armarx::trajectory::operator<<(std::ostream& os, const Trajectory&
return os;
}
-
namespace armarx
{
- auto trajectory::toUpdateFunc(std::function<void (float)> func) -> VariantTrack::UpdateFunc
+ auto
+ trajectory::toUpdateFunc(std::function<void(float)> func) -> VariantTrack::UpdateFunc
{
- return [func](VariantValue value)
- {
- func(std::get<float>(value));
- };
+ return [func](VariantValue value) { func(std::get<float>(value)); };
}
- auto trajectory::toUpdateFunc(std::function<void (const Eigen::MatrixXf&)> func) -> VariantTrack::UpdateFunc
+ auto
+ trajectory::toUpdateFunc(std::function<void(const Eigen::MatrixXf&)> func)
+ -> VariantTrack::UpdateFunc
{
- return [func](VariantValue value)
- {
- func(std::get<Eigen::MatrixXf>(value));
- };
+ return [func](VariantValue value) { func(std::get<Eigen::MatrixXf>(value)); };
}
- auto trajectory::toUpdateFunc(std::function<void (const Eigen::Quaternionf&)> func) -> VariantTrack::UpdateFunc
+ auto
+ trajectory::toUpdateFunc(std::function<void(const Eigen::Quaternionf&)> func)
+ -> VariantTrack::UpdateFunc
{
- return [func](VariantValue value)
- {
- func(std::get<Eigen::Quaternionf>(value));
- };
+ return [func](VariantValue value) { func(std::get<Eigen::Quaternionf>(value)); };
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h
index f79148ab16b1d8ca6a0cc4d2ccc33ed92ba44ffe..6d68d712ee9ad89ba4817074f0531a5636963c11 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h
@@ -4,7 +4,6 @@
#include "Track.h"
-
namespace armarx::trajectory
{
@@ -21,7 +20,6 @@ namespace armarx::trajectory
class Trajectory
{
public:
-
/// Constructor.
Trajectory();
@@ -72,26 +70,20 @@ namespace armarx::trajectory
private:
-
/// The tracks.
std::map<TrackID, VariantTrack> tracks;
-
};
-
std::ostream& operator<<(std::ostream& os, const Trajectory& trajectory);
-
// UPDATE FUNCTION HELPERS
/// Get an update function for value assignments.
template <typename ValueT>
- VariantTrack::UpdateFunc updateValue(ValueT& v)
+ VariantTrack::UpdateFunc
+ updateValue(ValueT& v)
{
- return [&v](VariantValue value)
- {
- v = std::get<ValueT>(value);
- };
+ return [&v](VariantValue value) { v = std::get<ValueT>(value); };
}
/// Wrap the function in a Track::UpdateFunc.
@@ -101,5 +93,4 @@ namespace armarx::trajectory
/// Wrap the function in a Track::UpdateFunc.
VariantTrack::UpdateFunc toUpdateFunc(std::function<void(const Eigen::Quaternionf&)> func);
-}
-
+} // namespace armarx::trajectory
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h b/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h
index ec4f5086b8944f12ad22ad096ba8cd9253941a2d..4c9c694e428a7125cd9fc3bf4e57f0ff11e27597 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h
+++ b/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h
@@ -1,10 +1,9 @@
#pragma once
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
#include <variant>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
namespace armarx::trajectory
{
@@ -15,4 +14,4 @@ namespace armarx::trajectory
/// ID of tracks.
using TrackID = std::string;
-}
+} // namespace armarx::trajectory
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
index 8af3f984e0005d1e3f6b8ce1271135d0662fb57d..c10c72d2d00ac6f9622e765bf9d980eabc9619eb 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
@@ -3,18 +3,20 @@
namespace armarx::trajectory::error
{
TrajectoryException::TrajectoryException(const std::string& msg) : std::logic_error(msg)
- {}
-
+ {
+ }
InterpolateDifferentTypesError::InterpolateDifferentTypesError() :
TrajectoryException("Interpolating between two different types.")
- {}
-
+ {
+ }
NoTrackWithID::NoTrackWithID(const TrackID& id) : TrajectoryException(makeMsg(id))
- {}
+ {
+ }
- std::string NoTrackWithID::makeMsg(const TrackID& id)
+ std::string
+ NoTrackWithID::makeMsg(const TrackID& id)
{
std::stringstream ss;
ss << "No track with ID '" << id << "'. \n"
@@ -22,19 +24,23 @@ namespace armarx::trajectory::error
return ss.str();
}
-
EmptyTrack::EmptyTrack(const TrackID& id) : TrajectoryException(makeMsg(id))
- {}
+ {
+ }
- std::string EmptyTrack::makeMsg(const TrackID& id)
+ std::string
+ EmptyTrack::makeMsg(const TrackID& id)
{
std::stringstream ss;
- ss << "Track with ID '" << id << "' is empty. \n"
- "Add a keyframe to track '" << id << "' before updating.";
+ ss << "Track with ID '" << id
+ << "' is empty. \n"
+ "Add a keyframe to track '"
+ << id << "' before updating.";
return ss.str();
}
- static std::string makeMsg(const TrackID& id, int typeIndex, int expectedTypeIndex)
+ static std::string
+ makeMsg(const TrackID& id, int typeIndex, int expectedTypeIndex)
{
std::stringstream ss;
ss << "Tried to add keyframe with value type '" << typeIndex << "' to non-empty track '"
@@ -43,9 +49,12 @@ namespace armarx::trajectory::error
return ss.str();
}
- WrongValueTypeInKeyframe::WrongValueTypeInKeyframe(const TrackID& trackID, int typeIndex, int expectedTypeIndex) :
+ WrongValueTypeInKeyframe::WrongValueTypeInKeyframe(const TrackID& trackID,
+ int typeIndex,
+ int expectedTypeIndex) :
TrajectoryException(makeMsg(trackID, typeIndex, expectedTypeIndex))
- {}
+ {
+ }
-}
+} // namespace armarx::trajectory::error
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h
index 131c5fea4a33436bb28c7f63188b9caf4a6719d2..0498e31ab00fd8d81da906b92d7d19cca752708c 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h
+++ b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h
@@ -5,7 +5,6 @@
#include "VariantValue.h" // for TrackID
-
namespace armarx::trajectory::error
{
@@ -15,32 +14,27 @@ namespace armarx::trajectory::error
TrajectoryException(const std::string& msg);
};
-
struct InterpolateDifferentTypesError : public TrajectoryException
{
InterpolateDifferentTypesError();
};
-
struct NoTrackWithID : public TrajectoryException
{
NoTrackWithID(const TrackID& id);
static std::string makeMsg(const TrackID& id);
};
-
struct EmptyTrack : public TrajectoryException
{
EmptyTrack(const TrackID& id);
static std::string makeMsg(const TrackID& id);
};
-
struct WrongValueTypeInKeyframe : public TrajectoryException
{
WrongValueTypeInKeyframe(const TrackID& trackID, int typeIndex, int expectedTypeIndex);
};
-
-}
+} // namespace armarx::trajectory::error
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp
index 4736420d31260a9e25ad69b23f2e90a793c3535b..8559f307c5cb2ec14938faec872bb12b421c29e7 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp
@@ -1,14 +1,14 @@
#include "linear.h"
-
namespace armarx::trajectory
{
template <>
- VariantValue interpolate::linear<VariantValue>(float t, const VariantValue& lhs, const VariantValue& rhs)
+ VariantValue
+ interpolate::linear<VariantValue>(float t, const VariantValue& lhs, const VariantValue& rhs)
{
// dont use std::get
- return std::visit(Linear {t}, lhs, rhs);
+ return std::visit(Linear{t}, lhs, rhs);
}
-}
+} // namespace armarx::trajectory
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h
index 20fc87cc935a751fa2ce9a344f114276c8c1685e..6b6eadd622f6ec16867308e966f347a549c8232e 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h
+++ b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h
@@ -1,8 +1,7 @@
#pragma once
-#include "../exceptions.h"
#include "../VariantValue.h"
-
+#include "../exceptions.h"
namespace armarx::trajectory::interpolate
{
@@ -13,53 +12,56 @@ namespace armarx::trajectory::interpolate
class Linear
{
public:
-
using result_type = VariantValue; ///< Exposed result type.
public:
-
/**
* @brief Interpolator
* @param t in [0, 1], where `t = 0` for `lhs` and `t = 1` for `rhs`.
*/
- Linear(float t) : t(t) {}
+ Linear(float t) : t(t)
+ {
+ }
template <typename U, typename V>
- VariantValue operator()(const U&, const V&) const
+ VariantValue
+ operator()(const U&, const V&) const
{
throw error::InterpolateDifferentTypesError();
}
- VariantValue operator()(const float& lhs, const float& rhs) const
+ VariantValue
+ operator()(const float& lhs, const float& rhs) const
{
return (1 - t) * lhs + t * rhs;
}
- VariantValue operator()(const Eigen::MatrixXf& lhs, const Eigen::MatrixXf& rhs) const
+ VariantValue
+ operator()(const Eigen::MatrixXf& lhs, const Eigen::MatrixXf& rhs) const
{
return (1 - t) * lhs + t * rhs;
}
- VariantValue operator()(const Eigen::Quaternionf& lhs, const Eigen::Quaternionf& rhs) const
+ VariantValue
+ operator()(const Eigen::Quaternionf& lhs, const Eigen::Quaternionf& rhs) const
{
return lhs.slerp(t, rhs);
}
private:
-
float t;
-
};
template <typename ReturnT>
- ReturnT linear(float t, const VariantValue& lhs, const VariantValue& rhs)
+ ReturnT
+ linear(float t, const VariantValue& lhs, const VariantValue& rhs)
{
- VariantValue result = std::visit(Linear {t}, lhs, rhs);
+ VariantValue result = std::visit(Linear{t}, lhs, rhs);
return std::get<ReturnT>(result);
}
template <>
VariantValue linear<VariantValue>(float t, const VariantValue& lhs, const VariantValue& rhs);
-}
+} // namespace armarx::trajectory::interpolate
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp
index 73afd8c5616497db533115bacbbe6a90ef92f1cf..de2a34215f877f63c71f5e86af355d191c32cb4d 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp
+++ b/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp
@@ -3,23 +3,24 @@
#define ARMARX_BOOST_TEST
-#include <iostream>
+#include "../Trajectory.h"
#include <RobotAPI/Test.h>
-#include "../Trajectory.h"
+
+#include <iostream>
using namespace armarx::trajectory;
namespace std
{
- std::ostream& operator<<(std::ostream& os, const std::type_info& rhs)
+ std::ostream&
+ operator<<(std::ostream& os, const std::type_info& rhs)
{
os << "TypeInfo (" << rhs.name() << ", " << rhs.hash_code() << ")";
return os;
}
-}
-
+} // namespace std
struct Fixture
{
@@ -36,12 +37,9 @@ struct Fixture
BOOST_FIXTURE_TEST_SUITE(TrajectoryTest, Fixture)
-
BOOST_AUTO_TEST_CASE(testNoTrackWithID)
{
- for (auto* id :
- { "id1", "id2"
- })
+ for (auto* id : {"id1", "id2"})
{
BOOST_CHECK_THROW(trajectory.addKeyframe(id, 0, 0.f), error::NoTrackWithID);
BOOST_CHECK_THROW(trajectory[id].addKeyframe(0, 0.f), error::NoTrackWithID);
@@ -59,7 +57,6 @@ BOOST_AUTO_TEST_CASE(testNoTrackWithID)
BOOST_CHECK_THROW(trajectory["id2"][0] = 0.f, error::NoTrackWithID);
}
-
BOOST_AUTO_TEST_CASE(testEmptyTrack)
{
trajectory.addTrack("float", updateValue(float_));
@@ -69,7 +66,7 @@ BOOST_AUTO_TEST_CASE(testEmptyTrack)
BOOST_CHECK_THROW(trajectory["float"].update(0), error::EmptyTrack);
BOOST_CHECK_THROW(trajectory.update(0), error::EmptyTrack);
- trajectory.addKeyframe("float", VariantKeyframe {0.f, 1.f});
+ trajectory.addKeyframe("float", VariantKeyframe{0.f, 1.f});
BOOST_CHECK_NO_THROW(trajectory["float"].update(0));
BOOST_CHECK_NO_THROW(trajectory.update(0));
@@ -78,7 +75,8 @@ BOOST_AUTO_TEST_CASE(testEmptyTrack)
}
template <typename Derived>
-std::ostream& operator<<(std::ostream& os, const Eigen::MatrixBase<Derived>& rhs)
+std::ostream&
+operator<<(std::ostream& os, const Eigen::MatrixBase<Derived>& rhs)
{
static const Eigen::IOFormat iof(5, 0, " ", "\n", "| ", " |", "");
os << rhs.format(iof);
@@ -86,24 +84,20 @@ std::ostream& operator<<(std::ostream& os, const Eigen::MatrixBase<Derived>& rhs
}
template <typename Derived>
-std::ostream& operator<<(std::ostream& os, const Eigen::QuaternionBase<Derived>& rhs)
+std::ostream&
+operator<<(std::ostream& os, const Eigen::QuaternionBase<Derived>& rhs)
{
static const Eigen::IOFormat iof(5, 0, " ", "\n", "| ", " |", "");
os << rhs.format(iof);
return os;
}
-
-
-
BOOST_AUTO_TEST_CASE(testDifferentTypes)
{
trajectory.addTrack("float", updateValue(float_));
- std::vector<VariantValue> values
- {
- 1.f, Eigen::MatrixXf::Zero(3, 3), Eigen::Quaternionf::Identity()
- };
+ std::vector<VariantValue> values{
+ 1.f, Eigen::MatrixXf::Zero(3, 3), Eigen::Quaternionf::Identity()};
for (std::size_t i = 1; i < values.size(); ++i)
{
@@ -123,7 +117,7 @@ BOOST_AUTO_TEST_CASE(testDifferentTypes)
BOOST_AUTO_TEST_CASE(testInterpolateFloat)
{
float f1 = -2;
- float f2 = 5;
+ float f2 = 5;
VariantValue v1(f1);
VariantValue v2(f2);
@@ -156,7 +150,8 @@ BOOST_AUTO_TEST_CASE(testInterpolateMatrix)
BOOST_AUTO_TEST_CASE(testInterpolateQuaternion)
{
Eigen::Quaternionf q1(Eigen::AngleAxisf(0.f, Eigen::Vector3f(1, 0, 0)));
- Eigen::Quaternionf q2(Eigen::AngleAxisf(2.f, Eigen::Vector3f(0, 1, 0)));;
+ Eigen::Quaternionf q2(Eigen::AngleAxisf(2.f, Eigen::Vector3f(0, 1, 0)));
+ ;
VariantValue v1(q1);
VariantValue v2(q2);
@@ -195,14 +190,12 @@ BOOST_AUTO_TEST_CASE(testInterpolateVector3)
BOOST_CHECK(mi.isApprox(vec2, ftol));
}
-
-
BOOST_AUTO_TEST_CASE(testTrajectoryFloat)
{
trajectory.addTrack("float", updateValue(float_));
trajectory.addKeyframe("float", 0.f, 0.f);
- trajectory.addKeyframe("float", VariantKeyframe {1.f, 1.f});
+ trajectory.addKeyframe("float", VariantKeyframe{1.f, 1.f});
int num = 10;
@@ -263,4 +256,3 @@ BOOST_AUTO_TEST_CASE(testTrajectoryMatrix)
}
BOOST_AUTO_TEST_SUITE_END()
-
diff --git a/source/RobotAPI/libraries/armem/client.h b/source/RobotAPI/libraries/armem/client.h
index fa61feb250a44f58dbc3c0d8b00a0c7ad67d8d31..57c27b840967fde4716955b77f9f09ed4cefec60 100644
--- a/source/RobotAPI/libraries/armem/client.h
+++ b/source/RobotAPI/libraries/armem/client.h
@@ -1,13 +1,12 @@
#pragma once
#include "client/MemoryNameSystem.h"
-#include "client/plugins.h"
#include "client/Query.h"
-#include "client/query/Builder.h"
-#include "client/query/query_fns.h"
#include "client/Reader.h"
#include "client/Writer.h"
-
+#include "client/plugins.h"
+#include "client/query/Builder.h"
+#include "client/query/query_fns.h"
namespace armarx::armem
{
@@ -20,4 +19,4 @@ namespace armarx::armem
using ClientQueryBuilder = client::query::Builder;
namespace client_query_fns = client::query_fns;
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
index f879ed5a349cfccb511315371aad7e72ab4363af..1021175d0a246e13315935371217b78d69374f3d 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -1,15 +1,13 @@
#include "MemoryNameSystem.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-
+#include <RobotAPI/libraries/armem/core/error.h>
namespace armarx::armem::client
{
@@ -18,22 +16,21 @@ namespace armarx::armem::client
{
}
-
- MemoryNameSystem::MemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component) :
- util::MemoryListener(component),
- mns(mns), component(component)
+ MemoryNameSystem::MemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns,
+ ManagedIceObject* component) :
+ util::MemoryListener(component), mns(mns), component(component)
{
}
-
- void MemoryNameSystem::initialize(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component)
+ void
+ MemoryNameSystem::initialize(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component)
{
this->mns = mns;
setComponent(component);
}
-
- void MemoryNameSystem::update()
+ void
+ MemoryNameSystem::update()
{
ARMARX_CHECK_NOT_NULL(mns);
@@ -59,7 +56,7 @@ namespace armarx::armem::client
else
{
// Compare ice identities, update if it changed.
- auto foo = [](auto & oldProxy, const auto & newProxy)
+ auto foo = [](auto& oldProxy, const auto& newProxy)
{
try
{
@@ -86,7 +83,6 @@ namespace armarx::armem::client
// if there is any exception, replace the old proxy by the new one
oldProxy = newProxy;
}
-
};
foo(it->second.reading, server.reading);
foo(it->second.writing, server.writing);
@@ -113,11 +109,10 @@ namespace armarx::armem::client
}
}
-
mns::dto::MemoryServerInterfaces
MemoryNameSystem::resolveServer(const MemoryID& memoryID)
{
- update();
+ update();
if (auto it = servers.find(memoryID.memoryName); it != servers.end())
{
return it->second;
@@ -128,8 +123,8 @@ namespace armarx::armem::client
}
}
-
- mns::dto::MemoryServerInterfaces MemoryNameSystem::waitForServer(const MemoryID& memoryID)
+ mns::dto::MemoryServerInterfaces
+ MemoryNameSystem::waitForServer(const MemoryID& memoryID)
{
update();
if (auto it = servers.find(memoryID.memoryName); it != servers.end())
@@ -156,19 +151,22 @@ namespace armarx::armem::client
}
}
- mns::dto::MemoryServerInterfaces MemoryNameSystem::useServer(const MemoryID& memoryID)
+ mns::dto::MemoryServerInterfaces
+ MemoryNameSystem::useServer(const MemoryID& memoryID)
{
ARMARX_CHECK_NOT_NULL(component)
- << "Owning component not set when using a memory server. \n"
- << "When calling `armem::mns::MemoryNameSystem::useServer()`, the owning component which should "
- << "receive the dependency to the memory server must be set beforehand. \n\n"
- << "Use `armem::mns::MemoryNameSystem::setComponent()` or pass the component on construction "
- << "before calling useServer().";
+ << "Owning component not set when using a memory server. \n"
+ << "When calling `armem::mns::MemoryNameSystem::useServer()`, the owning component "
+ "which should "
+ << "receive the dependency to the memory server must be set beforehand. \n\n"
+ << "Use `armem::mns::MemoryNameSystem::setComponent()` or pass the component on "
+ "construction "
+ << "before calling useServer().";
return useServer(memoryID, *component);
}
-
- mns::dto::MemoryServerInterfaces MemoryNameSystem::useServer(const MemoryID& memoryID, ManagedIceObject& component)
+ mns::dto::MemoryServerInterfaces
+ MemoryNameSystem::useServer(const MemoryID& memoryID, ManagedIceObject& component)
{
mns::dto::MemoryServerInterfaces server = waitForServer(memoryID);
// Add dependency.
@@ -177,52 +175,51 @@ namespace armarx::armem::client
return server;
}
-
- mns::dto::MemoryServerInterfaces MemoryNameSystem::useServer(const std::string& memoryName)
+ mns::dto::MemoryServerInterfaces
+ MemoryNameSystem::useServer(const std::string& memoryName)
{
return useServer(MemoryID().withMemoryName(memoryName));
}
-
- mns::dto::MemoryServerInterfaces MemoryNameSystem::useServer(const std::string& memoryName, ManagedIceObject& component)
+ mns::dto::MemoryServerInterfaces
+ MemoryNameSystem::useServer(const std::string& memoryName, ManagedIceObject& component)
{
return useServer(MemoryID().withMemoryName(memoryName), component);
}
-
- Reader MemoryNameSystem::getReader(const MemoryID& memoryID)
+ Reader
+ MemoryNameSystem::getReader(const MemoryID& memoryID)
{
auto server = resolveServer(memoryID);
return Reader(server.reading, server.prediction);
}
-
- Reader MemoryNameSystem::useReader(const MemoryID& memoryID)
+ Reader
+ MemoryNameSystem::useReader(const MemoryID& memoryID)
{
auto server = useServer(memoryID);
return Reader(server.reading, server.prediction);
}
-
- Reader MemoryNameSystem::useReader(const MemoryID& memoryID, ManagedIceObject& component)
+ Reader
+ MemoryNameSystem::useReader(const MemoryID& memoryID, ManagedIceObject& component)
{
auto server = useServer(memoryID, component);
return Reader(server.reading, server.prediction);
}
-
- Reader MemoryNameSystem::useReader(const std::string& memoryName)
+ Reader
+ MemoryNameSystem::useReader(const std::string& memoryName)
{
return useReader(MemoryID().withMemoryName(memoryName));
}
-
- Reader MemoryNameSystem::useReader(const std::string& memoryName, ManagedIceObject& component)
+ Reader
+ MemoryNameSystem::useReader(const std::string& memoryName, ManagedIceObject& component)
{
return useReader(MemoryID().withMemoryName(memoryName), component);
}
-
template <class ClientT>
std::map<std::string, ClientT>
MemoryNameSystem::_getAllClients(ClientFactory<ClientT>&& factory) const
@@ -238,8 +235,8 @@ namespace armarx::armem::client
return result;
}
-
- std::optional<Reader> readerFactory(const mns::dto::MemoryServerInterfaces& server)
+ std::optional<Reader>
+ readerFactory(const mns::dto::MemoryServerInterfaces& server)
{
if (auto read = server.reading)
{
@@ -255,8 +252,8 @@ namespace armarx::armem::client
return std::nullopt;
}
-
- std::optional<Writer> writerFactory(const mns::dto::MemoryServerInterfaces& server)
+ std::optional<Writer>
+ writerFactory(const mns::dto::MemoryServerInterfaces& server)
{
if (auto write = server.writing)
{
@@ -265,8 +262,8 @@ namespace armarx::armem::client
return std::nullopt;
}
-
- std::map<std::string, Reader> MemoryNameSystem::getAllReaders(bool update)
+ std::map<std::string, Reader>
+ MemoryNameSystem::getAllReaders(bool update)
{
if (update)
{
@@ -276,44 +273,44 @@ namespace armarx::armem::client
return _getAllClients<Reader>(readerFactory);
}
-
- std::map<std::string, Reader> MemoryNameSystem::getAllReaders() const
+ std::map<std::string, Reader>
+ MemoryNameSystem::getAllReaders() const
{
return _getAllClients<Reader>(readerFactory);
}
-
- Writer MemoryNameSystem::getWriter(const MemoryID& memoryID)
+ Writer
+ MemoryNameSystem::getWriter(const MemoryID& memoryID)
{
return Writer(resolveServer(memoryID).writing);
}
-
- Writer MemoryNameSystem::useWriter(const MemoryID& memoryID)
+ Writer
+ MemoryNameSystem::useWriter(const MemoryID& memoryID)
{
return Writer(useServer(memoryID).writing);
}
-
- Writer MemoryNameSystem::useWriter(const MemoryID& memoryID, ManagedIceObject& component)
+ Writer
+ MemoryNameSystem::useWriter(const MemoryID& memoryID, ManagedIceObject& component)
{
return Writer(useServer(memoryID, component).writing);
}
-
- Writer MemoryNameSystem::useWriter(const std::string& memoryName)
+ Writer
+ MemoryNameSystem::useWriter(const std::string& memoryName)
{
return useWriter(MemoryID().withMemoryName(memoryName));
}
-
- Writer MemoryNameSystem::useWriter(const std::string& memoryName, ManagedIceObject& component)
+ Writer
+ MemoryNameSystem::useWriter(const std::string& memoryName, ManagedIceObject& component)
{
return useWriter(MemoryID().withMemoryName(memoryName), component);
}
-
- std::map<std::string, Writer> MemoryNameSystem::getAllWriters(bool update)
+ std::map<std::string, Writer>
+ MemoryNameSystem::getAllWriters(bool update)
{
if (update)
{
@@ -322,14 +319,14 @@ namespace armarx::armem::client
return _getAllClients<Writer>(writerFactory);
}
-
- std::map<std::string, Writer> MemoryNameSystem::getAllWriters() const
+ std::map<std::string, Writer>
+ MemoryNameSystem::getAllWriters() const
{
return _getAllClients<Writer>(writerFactory);
}
-
- std::optional<wm::EntityInstance> MemoryNameSystem::resolveEntityInstance(const MemoryID& id)
+ std::optional<wm::EntityInstance>
+ MemoryNameSystem::resolveEntityInstance(const MemoryID& id)
{
auto result = resolveEntityInstances({id});
if (result.size() > 0)
@@ -342,8 +339,8 @@ namespace armarx::armem::client
}
}
-
- std::map<MemoryID, wm::EntityInstance> MemoryNameSystem::resolveEntityInstances(const std::vector<MemoryID>& ids)
+ std::map<MemoryID, wm::EntityInstance>
+ MemoryNameSystem::resolveEntityInstances(const std::vector<MemoryID>& ids)
{
std::stringstream errors;
int errorCounter = 0;
@@ -375,12 +372,14 @@ namespace armarx::armem::client
}
else if (id.hasEntityName())
{
- result[id] = queryResult.memory.getEntity(id).getLatestSnapshot().getInstance(0);
+ result[id] =
+ queryResult.memory.getEntity(id).getLatestSnapshot().getInstance(0);
}
else
{
std::stringstream ss;
- ss << "MemoryNameSystem::" << __FUNCTION__ << "requires IDs to be entity, snapshot or instance IDs,"
+ ss << "MemoryNameSystem::" << __FUNCTION__
+ << "requires IDs to be entity, snapshot or instance IDs,"
<< "but ID has no entity name.";
throw error::InvalidMemoryID(id, ss.str());
}
@@ -388,21 +387,25 @@ namespace armarx::armem::client
catch (const error::ArMemError& e)
{
errors << "\n#" << ++errorCounter << "\n"
- << "Failed to retrieve " << id << " from query result: \n" << e.what();
+ << "Failed to retrieve " << id << " from query result: \n"
+ << e.what();
}
}
}
else
{
errors << "\n# " << ++errorCounter << "\n"
- << "Failed to query '" << memoryName << "': \n" << queryResult.errorMessage;
+ << "Failed to query '" << memoryName << "': \n"
+ << queryResult.errorMessage;
}
}
if (errors.str().size() > 0)
{
- ARMARX_INFO << "MemoryNameSystem::" << __FUNCTION__ << ": The following errors may affect your result: "
- << "\n\n" << errors.str() << "\n\n"
+ ARMARX_INFO << "MemoryNameSystem::" << __FUNCTION__
+ << ": The following errors may affect your result: "
+ << "\n\n"
+ << errors.str() << "\n\n"
<< "When querying entity instances: \n- "
<< simox::alg::join(simox::alg::multi_to_string(ids), "\n- ");
}
@@ -410,24 +413,27 @@ namespace armarx::armem::client
return result;
}
-
- void MemoryNameSystem::registerServer(const MemoryID& memoryID, mns::dto::MemoryServerInterfaces server)
+ void
+ MemoryNameSystem::registerServer(const MemoryID& memoryID,
+ mns::dto::MemoryServerInterfaces server)
{
mns::dto::RegisterServerInput input;
input.name = memoryID.memoryName;
input.server = server;
- ARMARX_CHECK(server.reading or server.writing) << VAROUT(server.reading) << " | " << VAROUT(server.writing);
+ ARMARX_CHECK(server.reading or server.writing)
+ << VAROUT(server.reading) << " | " << VAROUT(server.writing);
ARMARX_CHECK_NOT_NULL(mns);
mns::dto::RegisterServerResult result = mns->registerServer(input);
if (!result.success)
{
- throw error::ServerRegistrationOrRemovalFailed("register", memoryID, result.errorMessage);
+ throw error::ServerRegistrationOrRemovalFailed(
+ "register", memoryID, result.errorMessage);
}
}
-
- void MemoryNameSystem::removeServer(const MemoryID& memoryID)
+ void
+ MemoryNameSystem::removeServer(const MemoryID& memoryID)
{
mns::dto::RemoveServerInput input;
input.name = memoryID.memoryName;
@@ -440,26 +446,23 @@ namespace armarx::armem::client
}
}
-
- mns::MemoryNameSystemInterfacePrx MemoryNameSystem::getMemoryNameSystem() const
+ mns::MemoryNameSystemInterfacePrx
+ MemoryNameSystem::getMemoryNameSystem() const
{
return mns;
}
-
- void MemoryNameSystem::getMemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns)
+ void
+ MemoryNameSystem::getMemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns)
{
this->mns = mns;
}
-
- void MemoryNameSystem::setComponent(ManagedIceObject* component)
+ void
+ MemoryNameSystem::setComponent(ManagedIceObject* component)
{
util::MemoryListener::setComponent(component);
this->component = component;
}
-}
-
-
-
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
index 50e90444e45b6f2a5cde22e0bc31837f946091ba..6172463d19b518fcb319c82651b3696038e18962 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
@@ -27,21 +27,20 @@
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
-
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/client/util/MemoryListener.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/mns/Registry.h>
-
namespace armarx
{
class ManagedIceObject;
}
+
namespace armarx::armem::client
{
class Reader;
class Writer;
-}
+} // namespace armarx::armem::client
namespace armarx::armem::wm
{
@@ -69,7 +68,6 @@ namespace armarx::armem::client
class MemoryNameSystem : public util::MemoryListener
{
public:
-
MemoryNameSystem();
/**
@@ -82,7 +80,8 @@ namespace armarx::armem::client
MemoryNameSystem(mns::MemoryNameSystemInterfacePrx mns,
ManagedIceObject* component = nullptr);
- void initialize(mns::MemoryNameSystemInterfacePrx mns, ManagedIceObject* component = nullptr);
+ void initialize(mns::MemoryNameSystemInterfacePrx mns,
+ ManagedIceObject* component = nullptr);
mns::MemoryNameSystemInterfacePrx getMemoryNameSystem() const;
@@ -179,7 +178,8 @@ namespace armarx::armem::client
*/
std::optional<wm::EntityInstance> resolveEntityInstance(const MemoryID& id);
- std::map<MemoryID, wm::EntityInstance> resolveEntityInstances(const std::vector<MemoryID>& ids);
+ std::map<MemoryID, wm::EntityInstance>
+ resolveEntityInstances(const std::vector<MemoryID>& ids);
/**
* @brief Resolve the given memory server for the given memory ID.
@@ -212,19 +212,17 @@ namespace armarx::armem::client
*/
void removeServer(const MemoryID& memoryID);
-
-
// Operators
/// Indicate whether the proxy is set.
- inline operator bool() const
+ inline
+ operator bool() const
{
return bool(mns);
}
private:
-
/**
* @brief Wait for the given memory server.
*
@@ -245,15 +243,17 @@ namespace armarx::armem::client
* @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
*/
mns::dto::MemoryServerInterfaces useServer(const MemoryID& memoryID);
- mns::dto::MemoryServerInterfaces useServer(const MemoryID& memoryID, ManagedIceObject& component);
+ mns::dto::MemoryServerInterfaces useServer(const MemoryID& memoryID,
+ ManagedIceObject& component);
mns::dto::MemoryServerInterfaces useServer(const std::string& memoryName);
- mns::dto::MemoryServerInterfaces useServer(const std::string& memoryName, ManagedIceObject& component);
+ mns::dto::MemoryServerInterfaces useServer(const std::string& memoryName,
+ ManagedIceObject& component);
private:
-
template <class ClientT>
- using ClientFactory = std::function<std::optional<ClientT>(const mns::dto::MemoryServerInterfaces& server)>;
+ using ClientFactory =
+ std::function<std::optional<ClientT>(const mns::dto::MemoryServerInterfaces& server)>;
template <class ClientT>
std::map<std::string, ClientT> _getAllClients(ClientFactory<ClientT>&& factory) const;
@@ -267,8 +267,7 @@ namespace armarx::armem::client
/// The registered memory servers.
std::map<std::string, mns::dto::MemoryServerInterfaces> servers;
-
};
-}
+} // namespace armarx::armem::client
diff --git a/source/RobotAPI/libraries/armem/client/Query.cpp b/source/RobotAPI/libraries/armem/client/Query.cpp
index b6a66fb7d7aa6b52a760e057167e2ee0cb2b0800..55543f67693bd566763a7914b44329afeb61cb54 100644
--- a/source/RobotAPI/libraries/armem/client/Query.cpp
+++ b/source/RobotAPI/libraries/armem/client/Query.cpp
@@ -5,67 +5,73 @@
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-
namespace armarx::armem::client
{
using armarx::fromIce;
using armarx::toIce;
-
- QueryInput QueryInput::fromIce(const armem::query::data::Input& ice)
+ QueryInput
+ QueryInput::fromIce(const armem::query::data::Input& ice)
{
return armarx::fromIce<QueryInput>(ice);
}
- armem::query::data::Input QueryInput::toIce() const
+ armem::query::data::Input
+ QueryInput::toIce() const
{
return armarx::toIce<armem::query::data::Input>(*this);
}
- QueryResult QueryResult::fromIce(const armem::query::data::Result& ice)
+ QueryResult
+ QueryResult::fromIce(const armem::query::data::Result& ice)
{
return armarx::fromIce<QueryResult>(ice);
}
- armem::query::data::Result QueryResult::toIce() const
+ armem::query::data::Result
+ QueryResult::toIce() const
{
return armarx::toIce<armem::query::data::Result>(*this);
}
- std::ostream& operator<<(std::ostream& os, const QueryResult& rhs)
+ std::ostream&
+ operator<<(std::ostream& os, const QueryResult& rhs)
{
return os << "Query result: "
- << "\n- success: \t" << rhs.success
- << "\n- error message: \t" << rhs.errorMessage
- ;
+ << "\n- success: \t" << rhs.success << "\n- error message: \t"
+ << rhs.errorMessage;
}
-}
-
+} // namespace armarx::armem::client
namespace armarx::armem
{
- void client::toIce(armem::query::data::Input& ice, const QueryInput& input)
+ void
+ client::toIce(armem::query::data::Input& ice, const QueryInput& input)
{
ice.memoryQueries = input.memoryQueries;
toIce(ice.withData, (input.dataMode == armem::query::DataMode::WithData));
}
- void client::fromIce(const armem::query::data::Input& ice, QueryInput& input)
+ void
+ client::fromIce(const armem::query::data::Input& ice, QueryInput& input)
{
input.memoryQueries = ice.memoryQueries;
- input.dataMode = (ice.withData ? armem::query::DataMode::WithData : armem::query::DataMode::NoData);
+ input.dataMode =
+ (ice.withData ? armem::query::DataMode::WithData : armem::query::DataMode::NoData);
}
- void client::toIce(armem::query::data::Result& ice, const QueryResult& result)
+ void
+ client::toIce(armem::query::data::Result& ice, const QueryResult& result)
{
toIce(ice, dynamic_cast<const detail::SuccessHeader&>(result));
toIce(ice.memory, result.memory);
}
- void client::fromIce(const armem::query::data::Result& ice, QueryResult& result)
+ void
+ client::fromIce(const armem::query::data::Result& ice, QueryResult& result)
{
fromIce(ice, dynamic_cast<detail::SuccessHeader&>(result));
fromIce(ice.memory, result.memory);
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index 8a7200fdace884fb197530d01622d787a09b5bbc..667365eb9d07185952acb406ca46d0526d765c27 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -1,9 +1,12 @@
#include "Reader.h"
+#include <mutex>
#include <sstream>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <Ice/LocalException.h>
+
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem/aron/MemoryLink.aron.generated.h>
#include <RobotAPI/libraries/armem/core/MemoryID_operators.h>
@@ -16,9 +19,6 @@
#include "query/Builder.h"
#include "query/query_fns.h"
-#include <mutex>
-#include <Ice/LocalException.h>
-
namespace armarx::armem::client
{
@@ -28,14 +28,12 @@ namespace armarx::armem::client
{
}
-
QueryResult
Reader::query(const QueryInput& input) const
{
return QueryResult::fromIce(query(input.toIce()));
}
-
armem::query::data::Result
Reader::query(const armem::query::data::Input& input) const
{
@@ -54,7 +52,6 @@ namespace armarx::armem::client
{
// the proxy is invalid. we must perform a reconnect
ARMARX_INFO << "Trying to reconnect ...";
-
}
catch (const Ice::LocalException& e)
{
@@ -67,14 +64,15 @@ namespace armarx::armem::client
return result;
}
-
- QueryResult Reader::query(armem::query::data::MemoryQueryPtr query, armem::query::DataMode dataMode) const
+ QueryResult
+ Reader::query(armem::query::data::MemoryQueryPtr query, armem::query::DataMode dataMode) const
{
return this->query(armem::query::data::MemoryQuerySeq{query}, dataMode);
}
-
- QueryResult Reader::query(const armem::query::data::MemoryQuerySeq& queries, armem::query::DataMode dataMode) const
+ QueryResult
+ Reader::query(const armem::query::data::MemoryQuerySeq& queries,
+ armem::query::DataMode dataMode) const
{
QueryInput input;
input.memoryQueries = queries;
@@ -82,24 +80,24 @@ namespace armarx::armem::client
return this->query(input);
}
-
- QueryResult Reader::query(const QueryBuilder& queryBuilder) const
+ QueryResult
+ Reader::query(const QueryBuilder& queryBuilder) const
{
return this->query(queryBuilder.buildQueryInput());
}
-
- QueryResult Reader::query(const QueryInput& input,
- armem::client::MemoryNameSystem& mns,
- int recursionDepth) const
+ QueryResult
+ Reader::query(const QueryInput& input,
+ armem::client::MemoryNameSystem& mns,
+ int recursionDepth) const
{
return QueryResult::fromIce(query(input.toIce(), mns, recursionDepth));
}
-
- armem::query::data::Result Reader::query(const armem::query::data::Input& input,
- armem::client::MemoryNameSystem& mns,
- int recursionDepth) const
+ armem::query::data::Result
+ Reader::query(const armem::query::data::Input& input,
+ armem::client::MemoryNameSystem& mns,
+ int recursionDepth) const
{
if (!readingPrx)
{
@@ -133,33 +131,32 @@ namespace armarx::armem::client
return result;
}
-
- QueryResult Reader::query(armem::query::data::MemoryQueryPtr query, // NOLINT
- armem::client::MemoryNameSystem& mns,
- int recursionDepth) const
+ QueryResult
+ Reader::query(armem::query::data::MemoryQueryPtr query, // NOLINT
+ armem::client::MemoryNameSystem& mns,
+ int recursionDepth) const
{
return this->query(armem::query::data::MemoryQuerySeq{query}, mns, recursionDepth);
}
-
- QueryResult Reader::query(const armem::query::data::MemoryQuerySeq& queries,
- armem::client::MemoryNameSystem& mns,
- int recursionDepth) const
+ QueryResult
+ Reader::query(const armem::query::data::MemoryQuerySeq& queries,
+ armem::client::MemoryNameSystem& mns,
+ int recursionDepth) const
{
QueryInput input;
input.memoryQueries = queries;
return this->query(input, mns, recursionDepth);
}
-
- QueryResult Reader::query(const QueryBuilder& queryBuilder,
- armem::client::MemoryNameSystem& mns,
- int recursionDepth) const
+ QueryResult
+ Reader::query(const QueryBuilder& queryBuilder,
+ armem::client::MemoryNameSystem& mns,
+ int recursionDepth) const
{
return this->query(queryBuilder.buildQueryInput(), mns, recursionDepth);
}
-
/**
* Get the MemoryID and data required to fill in the given MemoryLink.
*
@@ -286,15 +283,16 @@ namespace armarx::armem::client
}
}
-
- QueryResult Reader::queryMemoryIDs(const std::vector<MemoryID>& ids, armem::query::DataMode dataMode) const
+ QueryResult
+ Reader::queryMemoryIDs(const std::vector<MemoryID>& ids, armem::query::DataMode dataMode) const
{
using namespace client::query_fns;
query::Builder qb(dataMode);
for (const MemoryID& id : ids)
{
- query::EntitySelector& entity = qb.coreSegments(withID(id)).providerSegments(withID(id)).entities(withID(id));
+ query::EntitySelector& entity =
+ qb.coreSegments(withID(id)).providerSegments(withID(id)).entities(withID(id));
if (id.hasTimestamp())
{
@@ -308,9 +306,8 @@ namespace armarx::armem::client
return query(qb);
}
-
-
- std::optional<wm::EntitySnapshot> Reader::getLatestSnapshotOf(const std::vector<MemoryID>& _snapshotIDs) const
+ std::optional<wm::EntitySnapshot>
+ Reader::getLatestSnapshotOf(const std::vector<MemoryID>& _snapshotIDs) const
{
std::vector<MemoryID> snapshotIDs = _snapshotIDs;
@@ -339,8 +336,8 @@ namespace armarx::armem::client
}
}
-
- QueryResult Reader::getLatestSnapshotsIn(const MemoryID& id, armem::query::DataMode dataMode) const
+ QueryResult
+ Reader::getLatestSnapshotsIn(const MemoryID& id, armem::query::DataMode dataMode) const
{
using namespace client::query_fns;
if (!id.isWellDefined())
@@ -350,41 +347,37 @@ namespace armarx::armem::client
query::Builder qb(dataMode);
query::CoreSegmentSelector& core =
- id.hasCoreSegmentName()
- ? qb.coreSegments(withID(id))
- : qb.coreSegments(all());
- query::ProviderSegmentSelector& prov =
- id.hasProviderSegmentName()
- ? core.providerSegments(withID(id))
- : core.providerSegments(all());
+ id.hasCoreSegmentName() ? qb.coreSegments(withID(id)) : qb.coreSegments(all());
+ query::ProviderSegmentSelector& prov = id.hasProviderSegmentName()
+ ? core.providerSegments(withID(id))
+ : core.providerSegments(all());
query::EntitySelector& entity =
- id.hasEntityName()
- ? prov.entities(withID(id))
- : prov.entities(all());
+ id.hasEntityName() ? prov.entities(withID(id)) : prov.entities(all());
entity.snapshots(latest());
return query(qb);
}
-
- std::optional<wm::EntitySnapshot> Reader::getLatestSnapshotIn(const MemoryID& id, armem::query::DataMode dataMode) const
+ std::optional<wm::EntitySnapshot>
+ Reader::getLatestSnapshotIn(const MemoryID& id, armem::query::DataMode dataMode) const
{
client::QueryResult result = getLatestSnapshotsIn(id, dataMode);
if (result.success)
{
std::optional<wm::EntitySnapshot> latest = std::nullopt;
- result.memory.forEachEntity([&latest](const wm::Entity & entity)
- {
- if (not entity.empty())
+ result.memory.forEachEntity(
+ [&latest](const wm::Entity& entity)
{
- const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
- if (not latest.has_value() or latest->time() < snapshot.time())
+ if (not entity.empty())
{
- latest = snapshot;
+ const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
+ if (not latest.has_value() or latest->time() < snapshot.time())
+ {
+ latest = snapshot;
+ }
}
- }
- return true;
- });
+ return true;
+ });
return latest;
}
else
@@ -394,8 +387,8 @@ namespace armarx::armem::client
}
}
-
- QueryResult Reader::getAllLatestSnapshots(armem::query::DataMode dataMode) const
+ QueryResult
+ Reader::getAllLatestSnapshots(armem::query::DataMode dataMode) const
{
using namespace client::query_fns;
@@ -405,8 +398,8 @@ namespace armarx::armem::client
return this->query(qb);
}
-
- QueryResult Reader::getAll(armem::query::DataMode dataMode) const
+ QueryResult
+ Reader::getAll(armem::query::DataMode dataMode) const
{
using namespace client::query_fns;
@@ -419,21 +412,25 @@ namespace armarx::armem::client
server::dto::DirectlyStoreResult
Reader::directlyStore(const server::dto::DirectlyStoreInput& input) const
{
- server::RecordingMemoryInterfacePrx storingMemoryPrx = server::RecordingMemoryInterfacePrx::checkedCast(readingPrx);
+ server::RecordingMemoryInterfacePrx storingMemoryPrx =
+ server::RecordingMemoryInterfacePrx::checkedCast(readingPrx);
if (storingMemoryPrx)
{
return storingMemoryPrx->directlyStore(input);
}
else
{
- ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does not implement the StoringMemoryInterface.";
+ ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does "
+ "not implement the StoringMemoryInterface.";
return {};
}
}
- void Reader::startRecording() const
+ void
+ Reader::startRecording() const
{
- server::RecordingMemoryInterfacePrx storingMemoryPrx = server::RecordingMemoryInterfacePrx::checkedCast(readingPrx);
+ server::RecordingMemoryInterfacePrx storingMemoryPrx =
+ server::RecordingMemoryInterfacePrx::checkedCast(readingPrx);
if (storingMemoryPrx)
{
server::dto::StartRecordInput i;
@@ -447,13 +444,16 @@ namespace armarx::armem::client
}
else
{
- ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does not implement the StoringMemoryInterface.";
+ ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does "
+ "not implement the StoringMemoryInterface.";
}
}
- void Reader::stopRecording() const
+ void
+ Reader::stopRecording() const
{
- server::RecordingMemoryInterfacePrx storingMemoryPrx = server::RecordingMemoryInterfacePrx::checkedCast(readingPrx);
+ server::RecordingMemoryInterfacePrx storingMemoryPrx =
+ server::RecordingMemoryInterfacePrx::checkedCast(readingPrx);
if (storingMemoryPrx)
{
storingMemoryPrx->stopRecord();
@@ -461,11 +461,11 @@ namespace armarx::armem::client
}
else
{
- ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does not implement the StoringMemoryInterface.";
+ ARMARX_WARNING << "Could not store a query into the LTM. It seems like the Memory does "
+ "not implement the StoringMemoryInterface.";
}
}
-
void
Reader::setReadingMemory(server::ReadingMemoryInterfacePrx readingMemory)
{
diff --git a/source/RobotAPI/libraries/armem/client/Reader.h b/source/RobotAPI/libraries/armem/client/Reader.h
index cffe940bcead2f2875ac88c9daa9ba5abb715c52..30e9ffcfc3f0d1dd59a07da89eba33e7e52eca60 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.h
+++ b/source/RobotAPI/libraries/armem/client/Reader.h
@@ -193,7 +193,8 @@ namespace armarx::armem::client
*/
std::map<MemoryID, std::vector<PredictionEngine>> getAvailablePredictionEngines() const;
- inline operator bool() const
+ inline
+ operator bool() const
{
return bool(readingPrx);
}
diff --git a/source/RobotAPI/libraries/armem/client/forward_declarations.h b/source/RobotAPI/libraries/armem/client/forward_declarations.h
index ab6a8fb90cabb42d6838bcc2f57561aeafe01a30..bcf9d9b2712a95a2ba2ce076e5d1b824ad4ff6df 100644
--- a/source/RobotAPI/libraries/armem/client/forward_declarations.h
+++ b/source/RobotAPI/libraries/armem/client/forward_declarations.h
@@ -3,7 +3,6 @@
// #include <RobotAPI/libraries/armem/core/forward_declarations.h>
-
namespace armarx::armem::client::query
{
class Builder;
@@ -19,20 +18,18 @@ namespace armarx::armem::client
using QueryBuilder = query::Builder;
struct QueryInput;
struct QueryResult;
-}
+} // namespace armarx::armem::client
namespace armarx::armem::client::plugins
{
class Plugin;
class PluginUser;
class ListeningPluginUser;
-}
+} // namespace armarx::armem::client::plugins
namespace armarx::armem
{
using ClientPlugin = client::plugins::Plugin;
using ClientPluginUser = client::plugins::PluginUser;
using ListeningClientPluginUser = client::plugins::ListeningPluginUser;
-}
-
-
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/client/plugins.h b/source/RobotAPI/libraries/armem/client/plugins.h
index b91ccec3895f6a817c800278b3b3c0318d038750..49a962aa643ea6c7a28b66fcaa75df777b6a6278 100644
--- a/source/RobotAPI/libraries/armem/client/plugins.h
+++ b/source/RobotAPI/libraries/armem/client/plugins.h
@@ -1,12 +1,10 @@
#pragma once
#include <RobotAPI/libraries/armem/client/forward_declarations.h>
-
#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
#include <RobotAPI/libraries/armem/client/plugins/Plugin.h>
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
-
namespace armarx::armem::client
{
using ComponentPluginUser = plugins::ListeningPluginUser;
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.cpp b/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.cpp
index 96126b71241148187068990a18e61776cef5ba4a..ef9d47536d5e410e9630586c05796b54b964f6fa 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.cpp
@@ -1,22 +1,22 @@
#include "ListeningPlugin.h"
-#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
namespace armarx::armem::client::plugins
{
ListeningPlugin::~ListeningPlugin()
- {}
-
+ {
+ }
- void ListeningPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ ListeningPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
// This is now done by the client::util::MemoryListener class on subscription of a memory ID.
// Subscribe topics by single servers, use this as a prefix.
// properties->topic<MemoryListenerInterface>("MemoryUpdates");
}
-}
+} // namespace armarx::armem::client::plugins
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.h b/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.h
index 80aac20c61d49ffc67963da3b14be0c984ab5a3a..93edb7aa6a6c2933ff2097ccebf1639e99a1f52e 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.h
+++ b/source/RobotAPI/libraries/armem/client/plugins/ListeningPlugin.h
@@ -2,7 +2,6 @@
#include <ArmarXCore/core/ComponentPlugin.h>
-
namespace armarx::armem::client::plugins
{
@@ -14,16 +13,14 @@ namespace armarx::armem::client::plugins
*
* @see MemoryListenerInterface
*/
- class ListeningPlugin :
- public armarx::ComponentPlugin
+ class ListeningPlugin : public armarx::ComponentPlugin
{
public:
-
using ComponentPlugin::ComponentPlugin;
virtual ~ListeningPlugin() override;
- virtual void postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) override;
-
+ virtual void
+ postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) override;
};
-}
+} // namespace armarx::armem::client::plugins
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.cpp b/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.cpp
index 95eed974f77513f9186aed0d8b2633394b73f3be..1aebc7afaf770c4ba1e302605dbb0ed7d3418ad7 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.cpp
@@ -1,9 +1,8 @@
#include "ListeningPluginUser.h"
-#include "ListeningPlugin.h"
-
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include "ListeningPlugin.h"
namespace armarx::armem::client::plugins
{
@@ -13,16 +12,15 @@ namespace armarx::armem::client::plugins
addPlugin(listeningPlugin);
}
-
ListeningPluginUser::~ListeningPluginUser()
{
}
-
- void ListeningPluginUser::memoryUpdated(
- const std::vector<data::MemoryID>& updatedSnapshotIDs, const Ice::Current&)
+ void
+ ListeningPluginUser::memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs,
+ const Ice::Current&)
{
memoryNameSystem().updated(updatedSnapshotIDs);
}
-}
+} // namespace armarx::armem::client::plugins
diff --git a/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h b/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h
index d43c46cc81664a615f7d652de2c6d55bc37faa2a..5a920af16bc34e630cb7f328ea9f7883f03e6b61 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h
+++ b/source/RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h
@@ -1,17 +1,15 @@
#pragma once
-#include "PluginUser.h"
+#include <vector>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.h>
-#include <vector>
-
+#include "PluginUser.h"
namespace armarx::armem::client::plugins
{
class ListeningPlugin;
-
/**
* @brief A memory name system client which listens to the memory updates
* topic (`MemoryListenerInterface`).
@@ -20,33 +18,29 @@ namespace armarx::armem::client::plugins
* If your class already inherits from an ice interface, your ice interface might need to inherit from the
* MemoryListenerInterface to avoid errors.
*/
- class ListeningPluginUser :
- virtual public PluginUser,
- virtual public MemoryListenerInterface
+ class ListeningPluginUser : virtual public PluginUser, virtual public MemoryListenerInterface
{
protected:
-
ListeningPluginUser();
virtual ~ListeningPluginUser() override;
// MemoryListenerInterface
- virtual void
- memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs,
- const Ice::Current&) override;
+ virtual void memoryUpdated(const std::vector<data::MemoryID>& updatedSnapshotIDs,
+ const Ice::Current&) override;
private:
-
ListeningPlugin* listeningPlugin = nullptr;
-
};
-}
+} // namespace armarx::armem::client::plugins
+
namespace armarx::armem::client
{
using ListeningPluginUser = plugins::ListeningPluginUser;
}
+
namespace armarx::armem
{
using ListeningClientPluginUser = client::ListeningPluginUser;
diff --git a/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
index ba9c398f9717bfc5be2a22384d7e660097d6c63b..e7f832ae7ad85b86553cd9a08dd5a2cead26da5b 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/client/plugins/Plugin.cpp
@@ -1,29 +1,26 @@
#include "Plugin.h"
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
namespace armarx::armem::client::plugins
{
- Plugin::Plugin(
- ManagedIceObject& parent, std::string pre) :
- ComponentPlugin(parent, pre)
+ Plugin::Plugin(ManagedIceObject& parent, std::string pre) : ComponentPlugin(parent, pre)
{
}
-
Plugin::~Plugin()
- {}
-
+ {
+ }
- void Plugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ void
+ Plugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
if (not properties->hasDefinition(makePropertyName(PROPERTY_MNS_NAME_NAME)))
{
@@ -42,11 +39,13 @@ namespace armarx::armem::client::plugins
}
}
-
- void Plugin::preOnInitComponent()
+ void
+ Plugin::preOnInitComponent()
{
- parent<Component>().getProperty(memoryNameSystemName, makePropertyName(PROPERTY_MNS_NAME_NAME));
- parent<Component>().getProperty(memoryNameSystemEnabled, makePropertyName(PROPERTY_MNS_ENABLED_NAME));
+ parent<Component>().getProperty(memoryNameSystemName,
+ makePropertyName(PROPERTY_MNS_NAME_NAME));
+ parent<Component>().getProperty(memoryNameSystemEnabled,
+ makePropertyName(PROPERTY_MNS_ENABLED_NAME));
if (isMemoryNameSystemEnabled())
{
@@ -54,51 +53,48 @@ namespace armarx::armem::client::plugins
}
}
-
- void Plugin::preOnConnectComponent()
+ void
+ Plugin::preOnConnectComponent()
{
if (isMemoryNameSystemEnabled())
{
- ARMARX_DEBUG << "Creating MemoryNameSystem client with owning component '" << parent().getName() << "'.";
+ ARMARX_DEBUG << "Creating MemoryNameSystem client with owning component '"
+ << parent().getName() << "'.";
memoryNameSystem.initialize(getMemoryNameSystemProxy(), &parent());
}
}
-
bool
Plugin::isMemoryNameSystemEnabled()
{
return memoryNameSystemEnabled;
}
-
std::string
Plugin::getMemoryNameSystemName()
{
return memoryNameSystemName;
}
-
mns::MemoryNameSystemInterfacePrx
Plugin::getMemoryNameSystemProxy()
{
return isMemoryNameSystemEnabled() and parentDerives<ManagedIceObject>()
- ? parent<ManagedIceObject>().getProxy<mns::MemoryNameSystemInterfacePrx>(getMemoryNameSystemName())
- : nullptr;
+ ? parent<ManagedIceObject>().getProxy<mns::MemoryNameSystemInterfacePrx>(
+ getMemoryNameSystemName())
+ : nullptr;
}
-
MemoryNameSystem&
Plugin::getMemoryNameSystemClient()
{
return memoryNameSystem;
}
-
const MemoryNameSystem&
Plugin::getMemoryNameSystemClient() const
{
return memoryNameSystem;
}
-}
+} // namespace armarx::armem::client::plugins
diff --git a/source/RobotAPI/libraries/armem/client/plugins/Plugin.h b/source/RobotAPI/libraries/armem/client/plugins/Plugin.h
index 047b7208ff21e71a5f55c97160cf184c2d4a752f..b9e6f7780faa94da979d5470d9a6934e54f78b66 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/Plugin.h
+++ b/source/RobotAPI/libraries/armem/client/plugins/Plugin.h
@@ -1,12 +1,11 @@
#pragma once
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <string>
#include <ArmarXCore/core/ComponentPlugin.h>
-#include <string>
-
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
namespace armarx::armem::client::plugins
{
@@ -17,11 +16,9 @@ namespace armarx::armem::client::plugins
*
* @see MemoryNameSystem
*/
- class Plugin :
- public armarx::ComponentPlugin
+ class Plugin : public armarx::ComponentPlugin
{
public:
-
Plugin(ManagedIceObject& parent, std::string pre);
virtual ~Plugin() override;
@@ -33,7 +30,6 @@ namespace armarx::armem::client::plugins
public:
-
/**
* @brief Get the MNS client.
*
@@ -61,7 +57,6 @@ namespace armarx::armem::client::plugins
private:
-
/// The MNS client.
MemoryNameSystem memoryNameSystem;
@@ -71,7 +66,6 @@ namespace armarx::armem::client::plugins
static constexpr const char* PROPERTY_MNS_ENABLED_NAME = "mns.MemoryNameSystemEnabled";
static constexpr const char* PROPERTY_MNS_NAME_NAME = "mns.MemoryNameSystemName";
-
};
-}
+} // namespace armarx::armem::client::plugins
diff --git a/source/RobotAPI/libraries/armem/client/plugins/PluginUser.cpp b/source/RobotAPI/libraries/armem/client/plugins/PluginUser.cpp
index c4129c63d9c51b011bbeb42f412a5372f244a532..b44194b66b168ec0b3f7431cf4e2862595e6e153 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/PluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/client/plugins/PluginUser.cpp
@@ -1,9 +1,8 @@
#include "PluginUser.h"
-#include "Plugin.h"
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-
+#include "Plugin.h"
namespace armarx::armem::client::plugins
{
@@ -13,21 +12,20 @@ namespace armarx::armem::client::plugins
addPlugin(plugin);
}
-
PluginUser::~PluginUser()
{
}
-
- MemoryNameSystem& PluginUser::memoryNameSystem()
+ MemoryNameSystem&
+ PluginUser::memoryNameSystem()
{
return plugin->getMemoryNameSystemClient();
}
-
- const MemoryNameSystem& PluginUser::memoryNameSystem() const
+ const MemoryNameSystem&
+ PluginUser::memoryNameSystem() const
{
return plugin->getMemoryNameSystemClient();
}
-}
+} // namespace armarx::armem::client::plugins
diff --git a/source/RobotAPI/libraries/armem/client/plugins/PluginUser.h b/source/RobotAPI/libraries/armem/client/plugins/PluginUser.h
index 0638084dd61bc6563a6a99797a0e7eef96415204..5d45cd40d6ef75530ccf70795e123e5950e479f7 100644
--- a/source/RobotAPI/libraries/armem/client/plugins/PluginUser.h
+++ b/source/RobotAPI/libraries/armem/client/plugins/PluginUser.h
@@ -1,9 +1,8 @@
#pragma once
-#include <RobotAPI/libraries/armem/client/forward_declarations.h>
-
#include <ArmarXCore/core/ManagedIceObject.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
// Use this one include in your .cpp:
// #include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
@@ -13,7 +12,6 @@ namespace armarx::armem::client::plugins
{
class Plugin;
-
/**
* @brief Adds the Memory Name System client component plugin.
*
@@ -26,32 +24,29 @@ namespace armarx::armem::client::plugins
* @see MemoryNameSystemPlugin
* @see ListeningMemoryNameSystemPluginUser
*/
- class PluginUser :
- virtual public armarx::ManagedIceObject
+ class PluginUser : virtual public armarx::ManagedIceObject
{
protected:
-
PluginUser();
virtual ~PluginUser() override;
public:
-
MemoryNameSystem& memoryNameSystem();
const MemoryNameSystem& memoryNameSystem() const;
private:
-
plugins::Plugin* plugin = nullptr;
-
};
-}
+} // namespace armarx::armem::client::plugins
+
namespace armarx::armem::client
{
using PluginUser = plugins::PluginUser;
}
+
namespace armarx::armem
{
using ClientPluginUser = client::PluginUser;
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.cpp b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
index ff6300d5673b76b0a1cdb9757dbf4f72dce5b152..2bc5a2951943d9ccfc1e21a9a5b0023f0810f0af 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
@@ -1,26 +1,30 @@
#include "Builder.h"
-
namespace armarx::armem::client::query
{
- Builder::Builder(armem::query::DataMode dataMode, armem::query::QueryTarget target) : dataMode(dataMode), queryTarget(target)
+ Builder::Builder(armem::query::DataMode dataMode, armem::query::QueryTarget target) :
+ dataMode(dataMode), queryTarget(target)
{
}
- QueryInput Builder::buildQueryInput() const
+ QueryInput
+ Builder::buildQueryInput() const
{
QueryInput input;
input.memoryQueries = buildMemoryQueries();
input.dataMode = dataMode;
return input;
}
- armem::query::data::Input Builder::buildQueryInputIce() const
+
+ armem::query::data::Input
+ Builder::buildQueryInputIce() const
{
return buildQueryInput().toIce();
}
- armem::query::data::MemoryQuerySeq Builder::buildMemoryQueries() const
+ armem::query::data::MemoryQuerySeq
+ Builder::buildMemoryQueries() const
{
armem::query::data::MemoryQuerySeq memoryQueries;
for (const CoreSegmentSelector& child : _children)
@@ -34,93 +38,123 @@ namespace armarx::armem::client::query
return memoryQueries;
}
-
- CoreSegmentSelector& Builder::coreSegments()
+ CoreSegmentSelector&
+ Builder::coreSegments()
{
return _addChild();
}
- CoreSegmentSelector& Builder::coreSegments(const CoreSegmentSelector& selector)
+ CoreSegmentSelector&
+ Builder::coreSegments(const CoreSegmentSelector& selector)
{
return _addChild(selector);
}
-
- void Builder::all()
+ void
+ Builder::all()
{
- coreSegments().all()
- .providerSegments().all()
- .entities().all()
- .snapshots().all();
+ coreSegments().all().providerSegments().all().entities().all().snapshots().all();
}
-
- void Builder::allLatest()
+ void
+ Builder::allLatest()
{
- coreSegments().all()
- .providerSegments().all()
- .entities().all()
- .snapshots().latest();
+ coreSegments().all().providerSegments().all().entities().all().snapshots().latest();
}
-
- void Builder::allInCoreSegment(const MemoryID& coreSegmentID)
+ void
+ Builder::allInCoreSegment(const MemoryID& coreSegmentID)
{
- coreSegments().withName(coreSegmentID.coreSegmentName)
- .providerSegments().all()
- .entities().all()
- .snapshots().all();
+ coreSegments()
+ .withName(coreSegmentID.coreSegmentName)
+ .providerSegments()
+ .all()
+ .entities()
+ .all()
+ .snapshots()
+ .all();
}
- void Builder::allLatestInCoreSegment(const MemoryID& coreSegmentID)
+ void
+ Builder::allLatestInCoreSegment(const MemoryID& coreSegmentID)
{
- coreSegments().withName(coreSegmentID.coreSegmentName)
- .providerSegments().all()
- .entities().all()
- .snapshots().latest();
+ coreSegments()
+ .withName(coreSegmentID.coreSegmentName)
+ .providerSegments()
+ .all()
+ .entities()
+ .all()
+ .snapshots()
+ .latest();
}
- void Builder::allInProviderSegment(const MemoryID& providerSegmentID)
+ void
+ Builder::allInProviderSegment(const MemoryID& providerSegmentID)
{
- coreSegments().withName(providerSegmentID.coreSegmentName)
- .providerSegments().withName(providerSegmentID.providerSegmentName)
- .entities().all()
- .snapshots().all();
+ coreSegments()
+ .withName(providerSegmentID.coreSegmentName)
+ .providerSegments()
+ .withName(providerSegmentID.providerSegmentName)
+ .entities()
+ .all()
+ .snapshots()
+ .all();
}
- void Builder::allLatestInProviderSegment(const MemoryID& providerSegmentID)
+ void
+ Builder::allLatestInProviderSegment(const MemoryID& providerSegmentID)
{
- coreSegments().withName(providerSegmentID.coreSegmentName)
- .providerSegments().withName(providerSegmentID.providerSegmentName)
- .entities().all()
- .snapshots().latest();
+ coreSegments()
+ .withName(providerSegmentID.coreSegmentName)
+ .providerSegments()
+ .withName(providerSegmentID.providerSegmentName)
+ .entities()
+ .all()
+ .snapshots()
+ .latest();
}
- void Builder::allEntitySnapshots(const MemoryID& entityID)
+ void
+ Builder::allEntitySnapshots(const MemoryID& entityID)
{
- coreSegments().withName(entityID.coreSegmentName)
- .providerSegments().withName(entityID.providerSegmentName)
- .entities().withName(entityID.entityName)
- .snapshots().all();
+ coreSegments()
+ .withName(entityID.coreSegmentName)
+ .providerSegments()
+ .withName(entityID.providerSegmentName)
+ .entities()
+ .withName(entityID.entityName)
+ .snapshots()
+ .all();
}
- void Builder::latestEntitySnapshot(const MemoryID& entityID)
+ void
+ Builder::latestEntitySnapshot(const MemoryID& entityID)
{
- coreSegments().withName(entityID.coreSegmentName)
- .providerSegments().withName(entityID.providerSegmentName)
- .entities().withName(entityID.entityName)
- .snapshots().latest();
+ coreSegments()
+ .withName(entityID.coreSegmentName)
+ .providerSegments()
+ .withName(entityID.providerSegmentName)
+ .entities()
+ .withName(entityID.entityName)
+ .snapshots()
+ .latest();
}
- void Builder::singleEntitySnapshot(const MemoryID& snapshotID)
+ void
+ Builder::singleEntitySnapshot(const MemoryID& snapshotID)
{
- coreSegments().withName(snapshotID.coreSegmentName)
- .providerSegments().withName(snapshotID.providerSegmentName)
- .entities().withName(snapshotID.entityName)
- .snapshots().atTime(snapshotID.timestamp);
+ coreSegments()
+ .withName(snapshotID.coreSegmentName)
+ .providerSegments()
+ .withName(snapshotID.providerSegmentName)
+ .entities()
+ .withName(snapshotID.entityName)
+ .snapshots()
+ .atTime(snapshotID.timestamp);
}
- void Builder::multipleEntitySnapshots(const std::vector<MemoryID>& snapshotIDs)
+ void
+ Builder::multipleEntitySnapshots(const std::vector<MemoryID>& snapshotIDs)
{
for (const MemoryID& snapshotID : snapshotIDs)
{
@@ -128,4 +162,4 @@ namespace armarx::armem::client::query
}
}
-}
+} // namespace armarx::armem::client::query
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.h b/source/RobotAPI/libraries/armem/client/query/Builder.h
index c4a33bc7a20f6abdc5356e9c65ea93eb2f97dd4e..9ba5914967fb85d064bbf8cd48f4ac37b891b2fc 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.h
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.h
@@ -5,7 +5,6 @@
#include "selectors.h"
-
namespace armarx::armem::client::query
{
// ToDo: Make a memory selector (but this level is not in ice, yet)
@@ -19,24 +18,23 @@ namespace armarx::armem::client::query
* TODO
* @endcode
*/
- class Builder :
- public detail::ParentSelectorOps<Builder, CoreSegmentSelector>
+ class Builder : public detail::ParentSelectorOps<Builder, CoreSegmentSelector>
{
public:
-
- Builder(armem::query::DataMode dataMode = armem::query::DataMode::WithData, armem::query::QueryTarget target = armem::query::QueryTarget::WM);
+ Builder(armem::query::DataMode dataMode = armem::query::DataMode::WithData,
+ armem::query::QueryTarget target = armem::query::QueryTarget::WM);
/// Start specifying core segments.
CoreSegmentSelector& coreSegments();
CoreSegmentSelector& coreSegments(const CoreSegmentSelector& selector);
- template <class ...Ts>
- CoreSegmentSelector& coreSegments(Ts... args)
+ template <class... Ts>
+ CoreSegmentSelector&
+ coreSegments(Ts... args)
{
return _addChild(args...);
}
-
// Short hands for common queries
/// Get all snapshots from all entities in all segments.
@@ -70,7 +68,6 @@ namespace armarx::armem::client::query
public:
armem::query::DataMode dataMode;
armem::query::QueryTarget queryTarget;
-
};
-}
+} // namespace armarx::armem::client::query
diff --git a/source/RobotAPI/libraries/armem/client/query/detail/NameSelectorOps.h b/source/RobotAPI/libraries/armem/client/query/detail/NameSelectorOps.h
index 79f501fe4e370185eaae7f7b18b34ec903b8cc85..94a9d8640a64fb9dfe30257da70856971143633e 100644
--- a/source/RobotAPI/libraries/armem/client/query/detail/NameSelectorOps.h
+++ b/source/RobotAPI/libraries/armem/client/query/detail/NameSelectorOps.h
@@ -3,7 +3,6 @@
#include <string>
#include <vector>
-
namespace armarx::armem::client::query::detail
{
@@ -11,7 +10,6 @@ namespace armarx::armem::client::query::detail
class NameSelectorOps
{
public:
-
NameSelectorOps() = default;
virtual ~NameSelectorOps() = default;
@@ -19,19 +17,22 @@ namespace armarx::armem::client::query::detail
virtual DerivedT& withName(const std::string& name) = 0;
virtual DerivedT& withNamesMatching(const std::string& regex) = 0;
-
- virtual DerivedT& withNames(const std::vector<std::string>& names)
+ virtual DerivedT&
+ withNames(const std::vector<std::string>& names)
{
return withNames<std::vector<std::string>>(names);
}
template <class StringContainerT>
- DerivedT& withNames(const StringContainerT& names)
+ DerivedT&
+ withNames(const StringContainerT& names)
{
return withNames(names.begin(), names.end());
}
+
template <class IteratorT>
- DerivedT& withNames(IteratorT begin, IteratorT end)
+ DerivedT&
+ withNames(IteratorT begin, IteratorT end)
{
for (auto it = begin; it != end; ++it)
{
@@ -40,21 +41,23 @@ namespace armarx::armem::client::query::detail
return dynamic_cast<DerivedT&>(*this);
}
-
- virtual DerivedT& withNamesStartingWith(const std::string& prefix)
+ virtual DerivedT&
+ withNamesStartingWith(const std::string& prefix)
{
return withNamesMatching(prefix + ".*");
}
- virtual DerivedT& withNamesEndingWith(const std::string& suffix)
+
+ virtual DerivedT&
+ withNamesEndingWith(const std::string& suffix)
{
return withNamesMatching(".*" + suffix);
}
- virtual DerivedT& withNamesContaining(const std::string& substring)
+
+ virtual DerivedT&
+ withNamesContaining(const std::string& substring)
{
return withNamesMatching(".*" + substring + ".*");
}
-
-
};
-}
+} // namespace armarx::armem::client::query::detail
diff --git a/source/RobotAPI/libraries/armem/client/query/detail/SelectorOps.h b/source/RobotAPI/libraries/armem/client/query/detail/SelectorOps.h
index 95388b1393db0e13023fe18fbcbd905625397f71..d54f6af64f14626c5938e89fa0f4a5350c5a3275 100644
--- a/source/RobotAPI/libraries/armem/client/query/detail/SelectorOps.h
+++ b/source/RobotAPI/libraries/armem/client/query/detail/SelectorOps.h
@@ -4,9 +4,8 @@
#include <Ice/Handle.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/interface/armem/query.h>
-
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
namespace armarx::armem::client::query::detail
{
@@ -25,12 +24,14 @@ namespace armarx::armem::client::query::detail
virtual DerivedT& all() = 0;
virtual DerivedT& withID(const MemoryID& id) = 0;
-
- void addQuery(const IceInternal::Handle<QueryT>& query)
+ void
+ addQuery(const IceInternal::Handle<QueryT>& query)
{
_queries.push_back(query);
}
- void addQueries(const std::vector<IceInternal::Handle<QueryT>>& queries)
+
+ void
+ addQueries(const std::vector<IceInternal::Handle<QueryT>>& queries)
{
for (auto q : queries)
{
@@ -40,9 +41,9 @@ namespace armarx::armem::client::query::detail
protected:
-
template <class DerivedQueryT>
- DerivedQueryT& _addQuery()
+ DerivedQueryT&
+ _addQuery()
{
IceInternal::Handle<DerivedQueryT> query(new DerivedQueryT);
_queries.push_back(query);
@@ -58,23 +59,21 @@ namespace armarx::armem::client::query::detail
friend class ParentSelectorOps;
template <class T>
- inline void _apply(T arg)
+ inline void
+ _apply(T arg)
{
arg(dynamic_cast<DerivedT&>(*this));
}
- template <class T, class ...Ts>
- inline void _apply(T arg, Ts... args)
+
+ template <class T, class... Ts>
+ inline void
+ _apply(T arg, Ts... args)
{
_apply(arg);
_apply(args...);
}
-
-
-
};
-
-
template <class _DerivedT, class _ChildT>
class ParentSelectorOps
{
@@ -87,40 +86,40 @@ namespace armarx::armem::client::query::detail
virtual ~ParentSelectorOps() = default;
protected:
-
- ChildT& _addChild()
+ ChildT&
+ _addChild()
{
return _children.emplace_back();
}
- ChildT& _addChild(const ChildT& child)
+
+ ChildT&
+ _addChild(const ChildT& child)
{
return _children.emplace_back(child);
}
- template <class ...Ts>
- ChildT& _addChild(Ts... args)
+ template <class... Ts>
+ ChildT&
+ _addChild(Ts... args)
{
ChildT& sel = _addChild();
sel._apply(args...);
return sel;
}
-
std::vector<ChildT> _children;
};
-
-
template <class DerivedT, class QueryT, class ChildT>
class InnerSelectorOps :
public ParentSelectorOps<DerivedT, ChildT>,
public ChildSelectorOps<DerivedT, QueryT>
{
public:
-
InnerSelectorOps() = default;
- virtual std::vector<QueryT> buildQueries() const
+ virtual std::vector<QueryT>
+ buildQueries() const
{
std::vector<typename ChildT::QueryT> childQueries;
for (const auto& child : this->_children)
@@ -141,9 +140,9 @@ namespace armarx::armem::client::query::detail
protected:
-
- virtual void _setChildQueries(QueryT& query, const std::vector<typename ChildT::QueryT>& childQueries) const = 0;
-
+ virtual void
+ _setChildQueries(QueryT& query,
+ const std::vector<typename ChildT::QueryT>& childQueries) const = 0;
};
-}
+} // namespace armarx::armem::client::query::detail
diff --git a/source/RobotAPI/libraries/armem/client/query/query_fns.h b/source/RobotAPI/libraries/armem/client/query/query_fns.h
index 35203a014fa423cec4f8ef0f4d09759bc54385a9..7068201e76e58bd18499d2f449ff2245044046c4 100644
--- a/source/RobotAPI/libraries/armem/client/query/query_fns.h
+++ b/source/RobotAPI/libraries/armem/client/query/query_fns.h
@@ -2,166 +2,115 @@
#include "selectors.h"
-
namespace armarx::armem::client::query_fns
{
inline auto
all()
{
- return [ ](auto & selector)
- {
- selector.all();
- };
+ return [](auto& selector) { selector.all(); };
}
-
inline auto
withID(const MemoryID& id)
{
- return [ & ](auto & selector)
- {
- selector.withID(id);
- };
+ return [&](auto& selector) { selector.withID(id); };
}
-
// NAME-BASED QUERIES
- inline
- auto withName(const std::string& name)
+ inline auto
+ withName(const std::string& name)
{
- return [ &name ](auto & selector)
- {
- selector.withName(name);
- };
+ return [&name](auto& selector) { selector.withName(name); };
}
- inline auto withNamesMatching(const std::string& regex)
+ inline auto
+ withNamesMatching(const std::string& regex)
{
- return [ ®ex ](auto & selector)
- {
- selector.withNamesMatching(regex);
- };
+ return [®ex](auto& selector) { selector.withNamesMatching(regex); };
}
- inline auto withNames(const std::vector<std::string>& names)
+ inline auto
+ withNames(const std::vector<std::string>& names)
{
- return [ &names ](auto & selector)
- {
- selector.withNames(names);
- };
+ return [&names](auto& selector) { selector.withNames(names); };
}
+
template <class StringContainerT>
- auto withNames(const StringContainerT& names)
+ auto
+ withNames(const StringContainerT& names)
{
- return [ &names ](auto & selector)
- {
- selector.withNames(names);
- };
+ return [&names](auto& selector) { selector.withNames(names); };
}
+
template <class IteratorT>
- auto withNames(IteratorT begin, IteratorT end)
+ auto
+ withNames(IteratorT begin, IteratorT end)
{
- return [ begin, end ](auto & selector)
- {
- selector.withNames(begin, end);
- };
+ return [begin, end](auto& selector) { selector.withNames(begin, end); };
}
- inline auto withNamesStartingWith(const std::string& prefix)
+ inline auto
+ withNamesStartingWith(const std::string& prefix)
{
- return [ &prefix ](auto & selector)
- {
- selector.withNamesStartingWith(prefix);
- };
+ return [&prefix](auto& selector) { selector.withNamesStartingWith(prefix); };
}
- inline auto withNamesEndingWith(const std::string& suffix)
+
+ inline auto
+ withNamesEndingWith(const std::string& suffix)
{
- return [ &suffix ](auto & selector)
- {
- selector.withNamesEndingWith(suffix);
- };
+ return [&suffix](auto& selector) { selector.withNamesEndingWith(suffix); };
}
- inline auto withNamesContaining(const std::string& substring)
+
+ inline auto
+ withNamesContaining(const std::string& substring)
{
- return [ &substring ](auto & selector)
- {
- selector.withNamesContaining(substring);
- };
+ return [&substring](auto& selector) { selector.withNamesContaining(substring); };
}
-
// SNAPSHOT QUERIES
- inline
- std::function<void(query::SnapshotSelector&)>
+ inline std::function<void(query::SnapshotSelector&)>
atTime(Time time)
{
- return [ = ](query::SnapshotSelector & selector)
- {
- selector.atTime(time);
- };
+ return [=](query::SnapshotSelector& selector) { selector.atTime(time); };
}
- inline
- std::function<void(query::SnapshotSelector&)>
+ inline std::function<void(query::SnapshotSelector&)>
latest()
{
- return [ = ](query::SnapshotSelector & selector)
- {
- selector.latest();
- };
+ return [=](query::SnapshotSelector& selector) { selector.latest(); };
}
- inline
- std::function<void(query::SnapshotSelector&)>
+ inline std::function<void(query::SnapshotSelector&)>
indexRange(long first, long last)
{
- return [ = ](query::SnapshotSelector & selector)
- {
- selector.indexRange(first, last);
- };
+ return [=](query::SnapshotSelector& selector) { selector.indexRange(first, last); };
}
- inline
- std::function<void(query::SnapshotSelector&)>
+ inline std::function<void(query::SnapshotSelector&)>
timeRange(Time min, Time max)
{
- return [ = ](query::SnapshotSelector & selector)
- {
- selector.timeRange(min, max);
- };
+ return [=](query::SnapshotSelector& selector) { selector.timeRange(min, max); };
}
- inline
- std::function<void(query::SnapshotSelector&)>
+ inline std::function<void(query::SnapshotSelector&)>
atTimeApprox(Time time, Duration eps)
{
- return [ = ](query::SnapshotSelector & selector)
- {
- selector.atTimeApprox(time, eps);
- };
+ return [=](query::SnapshotSelector& selector) { selector.atTimeApprox(time, eps); };
}
- inline
- std::function<void(query::SnapshotSelector&)>
+ inline std::function<void(query::SnapshotSelector&)>
beforeOrAtTime(Time time)
{
- return [ = ](query::SnapshotSelector & selector)
- {
- selector.beforeOrAtTime(time);
- };
+ return [=](query::SnapshotSelector& selector) { selector.beforeOrAtTime(time); };
}
- inline
- std::function<void(query::SnapshotSelector&)>
+ inline std::function<void(query::SnapshotSelector&)>
beforeTime(Time time, long nElements = 1)
{
- return [ = ](query::SnapshotSelector & selector)
- {
- selector.beforeTime(time, nElements);
- };
+ return [=](query::SnapshotSelector& selector) { selector.beforeTime(time, nElements); };
}
-} // namespace armarx::armem::client::query_fns
+} // namespace armarx::armem::client::query_fns
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.cpp b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
index aeae579c070d8c54692997956589b423956df643..c268f736673fad2287119a5584de32b164a11bda 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
@@ -1,8 +1,8 @@
#include "selectors.h"
#include <ArmarXCore/core/ice_conversions.h>
-#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
@@ -10,36 +10,40 @@
namespace dq = ::armarx::armem::query::data;
-
namespace armarx::armem::client::query
{
- dq::EntityQuerySeq SnapshotSelector::buildQueries() const
+ dq::EntityQuerySeq
+ SnapshotSelector::buildQueries() const
{
return _queries;
}
- SnapshotSelector& SnapshotSelector::all()
+ SnapshotSelector&
+ SnapshotSelector::all()
{
auto& q = _addQuery<dq::entity::All>();
- (void) q;
+ (void)q;
return *this;
}
- SnapshotSelector& SnapshotSelector::latest()
+ SnapshotSelector&
+ SnapshotSelector::latest()
{
auto& q = _addQuery<dq::entity::Single>();
toIce(q.timestamp, Time::Invalid());
return *this;
}
- SnapshotSelector& SnapshotSelector::atTime(Time timestamp)
+ SnapshotSelector&
+ SnapshotSelector::atTime(Time timestamp)
{
auto& q = _addQuery<dq::entity::Single>();
toIce(q.timestamp, timestamp);
return *this;
}
- SnapshotSelector& SnapshotSelector::timeRange(Time min, Time max)
+ SnapshotSelector&
+ SnapshotSelector::timeRange(Time min, Time max)
{
auto& q = _addQuery<dq::entity::TimeRange>();
toIce(q.minTimestamp, min);
@@ -47,7 +51,8 @@ namespace armarx::armem::client::query
return *this;
}
- SnapshotSelector& SnapshotSelector::atTimeApprox(Time timestamp, Duration eps)
+ SnapshotSelector&
+ SnapshotSelector::atTimeApprox(Time timestamp, Duration eps)
{
auto& q = _addQuery<dq::entity::TimeApprox>();
toIce(q.timestamp, timestamp);
@@ -55,8 +60,8 @@ namespace armarx::armem::client::query
return *this;
}
-
- SnapshotSelector& SnapshotSelector::indexRange(long first, long last)
+ SnapshotSelector&
+ SnapshotSelector::indexRange(long first, long last)
{
auto& q = _addQuery<dq::entity::IndexRange>();
q.first = first;
@@ -64,16 +69,16 @@ namespace armarx::armem::client::query
return *this;
}
-
- SnapshotSelector& SnapshotSelector::beforeOrAtTime(Time timestamp)
+ SnapshotSelector&
+ SnapshotSelector::beforeOrAtTime(Time timestamp)
{
auto& q = _addQuery<dq::entity::BeforeOrAtTime>();
toIce(q.timestamp, timestamp);
return *this;
}
-
- SnapshotSelector& SnapshotSelector::beforeTime(Time timestamp, long maxEntries)
+ SnapshotSelector&
+ SnapshotSelector::beforeTime(Time timestamp, long maxEntries)
{
using armarx::toIce;
@@ -83,113 +88,133 @@ namespace armarx::armem::client::query
return *this;
}
- SnapshotSelector& EntitySelector::snapshots()
+ SnapshotSelector&
+ EntitySelector::snapshots()
{
return _addChild();
}
- SnapshotSelector& EntitySelector::snapshots(const ChildT& selector)
+ SnapshotSelector&
+ EntitySelector::snapshots(const ChildT& selector)
{
return _addChild(selector);
}
- EntitySelector& EntitySelector::all()
+ EntitySelector&
+ EntitySelector::all()
{
auto& q = _addQuery<dq::provider::All>();
- (void) q;
+ (void)q;
return *this;
}
- EntitySelector& EntitySelector::withName(const std::string& name)
+ EntitySelector&
+ EntitySelector::withName(const std::string& name)
{
auto& q = _addQuery<dq::provider::Single>();
q.entityName = name;
return *this;
}
- EntitySelector& EntitySelector::withNamesMatching(const std::string& regex)
+ EntitySelector&
+ EntitySelector::withNamesMatching(const std::string& regex)
{
auto& q = _addQuery<dq::provider::Regex>();
q.entityNameRegex = regex;
return *this;
}
- void EntitySelector::_setChildQueries(dq::ProviderSegmentQueryPtr& query, const dq::EntityQuerySeq& childQueries) const
+ void
+ EntitySelector::_setChildQueries(dq::ProviderSegmentQueryPtr& query,
+ const dq::EntityQuerySeq& childQueries) const
{
query->entityQueries = childQueries;
}
-
- EntitySelector& ProviderSegmentSelector::entities()
+ EntitySelector&
+ ProviderSegmentSelector::entities()
{
return _addChild();
}
- EntitySelector& ProviderSegmentSelector::entities(const EntitySelector& selector)
+ EntitySelector&
+ ProviderSegmentSelector::entities(const EntitySelector& selector)
{
return _addChild(selector);
}
- ProviderSegmentSelector& ProviderSegmentSelector::all()
+ ProviderSegmentSelector&
+ ProviderSegmentSelector::all()
{
auto& q = _addQuery<dq::core::All>();
- (void) q;
+ (void)q;
return *this;
}
- ProviderSegmentSelector& ProviderSegmentSelector::withName(const std::string& name)
+ ProviderSegmentSelector&
+ ProviderSegmentSelector::withName(const std::string& name)
{
auto& q = _addQuery<dq::core::Single>();
q.providerSegmentName = name;
return *this;
}
- ProviderSegmentSelector& ProviderSegmentSelector::withNamesMatching(const std::string& regex)
+ ProviderSegmentSelector&
+ ProviderSegmentSelector::withNamesMatching(const std::string& regex)
{
auto& q = _addQuery<dq::core::Regex>();
q.providerSegmentNameRegex = regex;
return *this;
}
- void ProviderSegmentSelector::_setChildQueries(dq::CoreSegmentQueryPtr& query, const dq::ProviderSegmentQuerySeq& childQueries) const
+ void
+ ProviderSegmentSelector::_setChildQueries(dq::CoreSegmentQueryPtr& query,
+ const dq::ProviderSegmentQuerySeq& childQueries) const
{
query->providerSegmentQueries = childQueries;
}
-
- ProviderSegmentSelector& CoreSegmentSelector::providerSegments()
+ ProviderSegmentSelector&
+ CoreSegmentSelector::providerSegments()
{
return _addChild();
}
- ProviderSegmentSelector& CoreSegmentSelector::providerSegments(const ProviderSegmentSelector& selector)
+ ProviderSegmentSelector&
+ CoreSegmentSelector::providerSegments(const ProviderSegmentSelector& selector)
{
return _addChild(selector);
}
- CoreSegmentSelector& CoreSegmentSelector::all()
+ CoreSegmentSelector&
+ CoreSegmentSelector::all()
{
auto& q = _addQuery<dq::memory::All>();
- (void) q;
+ (void)q;
return *this;
}
- CoreSegmentSelector& CoreSegmentSelector::withName(const std::string& name)
+ CoreSegmentSelector&
+ CoreSegmentSelector::withName(const std::string& name)
{
auto& q = _addQuery<dq::memory::Single>();
q.coreSegmentName = name;
return *this;
}
- CoreSegmentSelector& CoreSegmentSelector::withNamesMatching(const std::string& regex)
+
+ CoreSegmentSelector&
+ CoreSegmentSelector::withNamesMatching(const std::string& regex)
{
auto& q = _addQuery<dq::memory::Regex>();
q.coreSegmentNameRegex = regex;
return *this;
}
- void CoreSegmentSelector::_setChildQueries(dq::MemoryQueryPtr& query, const dq::CoreSegmentQuerySeq& childQueries) const
+ void
+ CoreSegmentSelector::_setChildQueries(dq::MemoryQueryPtr& query,
+ const dq::CoreSegmentQuerySeq& childQueries) const
{
query->coreSegmentQueries = childQueries;
}
-}
+} // namespace armarx::armem::client::query
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.h b/source/RobotAPI/libraries/armem/client/query/selectors.h
index cf67e44e1823d335b6e1f89b657c7bb15c49e418..ea041877c77302dea3e638eee946122a8a00a781 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.h
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.h
@@ -1,27 +1,26 @@
#pragma once
#include <RobotAPI/interface/armem/query.h>
-
#include <RobotAPI/libraries/armem/core/Time.h>
#include "detail/NameSelectorOps.h"
#include "detail/SelectorOps.h"
-
namespace armarx::armem::client::query
{
class SnapshotSelector :
public detail::ChildSelectorOps<SnapshotSelector, armem::query::data::EntityQueryPtr>
{
public:
-
SnapshotSelector() = default;
armem::query::data::EntityQuerySeq buildQueries() const;
SnapshotSelector& all() override;
- SnapshotSelector& withID(const MemoryID& id) override
+
+ SnapshotSelector&
+ withID(const MemoryID& id) override
{
return atTime(id.timestamp);
}
@@ -35,32 +34,33 @@ namespace armarx::armem::client::query
SnapshotSelector& timeRange(Time min, Time max);
SnapshotSelector& indexRange(long first, long last);
-
};
-
class EntitySelector :
- public detail::InnerSelectorOps<EntitySelector, armem::query::data::ProviderSegmentQueryPtr, SnapshotSelector>
- , public detail::NameSelectorOps<EntitySelector>
+ public detail::InnerSelectorOps<EntitySelector,
+ armem::query::data::ProviderSegmentQueryPtr,
+ SnapshotSelector>,
+ public detail::NameSelectorOps<EntitySelector>
{
public:
-
EntitySelector() = default;
/// Start specifying entity snapshots.
SnapshotSelector& snapshots();
SnapshotSelector& snapshots(const SnapshotSelector& selector);
- template <class ...Ts>
- SnapshotSelector& snapshots(Ts... args)
+ template <class... Ts>
+ SnapshotSelector&
+ snapshots(Ts... args)
{
return _addChild(args...);
}
-
EntitySelector& all() override;
- EntitySelector& withID(const MemoryID& id) override
+
+ EntitySelector&
+ withID(const MemoryID& id) override
{
return withName(id.entityName);
}
@@ -71,33 +71,35 @@ namespace armarx::armem::client::query
protected:
- void _setChildQueries(armem::query::data::ProviderSegmentQueryPtr& query, const armem::query::data::EntityQuerySeq& childQueries) const override;
-
+ void
+ _setChildQueries(armem::query::data::ProviderSegmentQueryPtr& query,
+ const armem::query::data::EntityQuerySeq& childQueries) const override;
};
-
-
class ProviderSegmentSelector :
- public detail::InnerSelectorOps<ProviderSegmentSelector, armem::query::data::CoreSegmentQueryPtr, EntitySelector>
- , public detail::NameSelectorOps<ProviderSegmentSelector>
+ public detail::InnerSelectorOps<ProviderSegmentSelector,
+ armem::query::data::CoreSegmentQueryPtr,
+ EntitySelector>,
+ public detail::NameSelectorOps<ProviderSegmentSelector>
{
public:
-
ProviderSegmentSelector() = default;
/// Start specifying entities.
EntitySelector& entities();
EntitySelector& entities(const EntitySelector& selector);
- template <class ...Ts>
- EntitySelector& entities(Ts... args)
+ template <class... Ts>
+ EntitySelector&
+ entities(Ts... args)
{
return _addChild(args...);
}
-
ProviderSegmentSelector& all() override;
- ProviderSegmentSelector& withID(const MemoryID& id) override
+
+ ProviderSegmentSelector&
+ withID(const MemoryID& id) override
{
return withName(id.providerSegmentName);
}
@@ -108,33 +110,35 @@ namespace armarx::armem::client::query
protected:
- void _setChildQueries(armem::query::data::CoreSegmentQueryPtr& query, const armem::query::data::ProviderSegmentQuerySeq& childQueries) const override;
-
+ void _setChildQueries(
+ armem::query::data::CoreSegmentQueryPtr& query,
+ const armem::query::data::ProviderSegmentQuerySeq& childQueries) const override;
};
-
-
class CoreSegmentSelector :
- public detail::InnerSelectorOps<CoreSegmentSelector, armem::query::data::MemoryQueryPtr, ProviderSegmentSelector>
- , public detail::NameSelectorOps<CoreSegmentSelector>
+ public detail::InnerSelectorOps<CoreSegmentSelector,
+ armem::query::data::MemoryQueryPtr,
+ ProviderSegmentSelector>,
+ public detail::NameSelectorOps<CoreSegmentSelector>
{
public:
-
CoreSegmentSelector() = default;
/// Start specifying provider segments.
ProviderSegmentSelector& providerSegments();
ProviderSegmentSelector& providerSegments(const ProviderSegmentSelector& selector);
- template <class ...Ts>
- ProviderSegmentSelector& providerSegments(Ts... args)
+ template <class... Ts>
+ ProviderSegmentSelector&
+ providerSegments(Ts... args)
{
return _addChild(args...);
}
-
CoreSegmentSelector& all() override;
- CoreSegmentSelector& withID(const MemoryID& id) override
+
+ CoreSegmentSelector&
+ withID(const MemoryID& id) override
{
return withName(id.coreSegmentName);
}
@@ -145,8 +149,9 @@ namespace armarx::armem::client::query
protected:
- void _setChildQueries(armem::query::data::MemoryQueryPtr& query, const armem::query::data::CoreSegmentQuerySeq& childQueries) const override;
-
+ void _setChildQueries(
+ armem::query::data::MemoryQueryPtr& query,
+ const armem::query::data::CoreSegmentQuerySeq& childQueries) const override;
};
-}
+} // namespace armarx::armem::client::query
diff --git a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
index ecad82410f820bcf6c4fbafa67440e9cb4abbaed..3e789d01a3eb6e15a0f0d36c2b5d82d1218d0533 100644
--- a/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/MemoryToDebugObserver.cpp
@@ -21,6 +21,7 @@
*/
#include "MemoryToDebugObserver.h"
+
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <RobotAPI/libraries/armem/core/error/mns.h>
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
index c46755e3d9a3c94f6e8634b15884604db71226b9..38881ebb2c3305b45f32939363447bcef762ef36 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
@@ -33,7 +33,8 @@ namespace armarx::armem::client::util
SimpleWriterBase::connect(armarx::armem::client::MemoryNameSystem& mns)
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '" << properties().memoryName << "' ...";
+ ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '" << properties().memoryName
+ << "' ...";
try
{
memoryWriterClient = mns.useWriter(MemoryID().withMemoryName(properties().memoryName));
diff --git a/source/RobotAPI/libraries/armem/core.h b/source/RobotAPI/libraries/armem/core.h
index 58ef0c64aca0216cabd11b3091841f5ab0f25f6b..f34bfc1b7cf86c1edb3604130b5d33e78bec0797 100644
--- a/source/RobotAPI/libraries/armem/core.h
+++ b/source/RobotAPI/libraries/armem/core.h
@@ -1,14 +1,12 @@
#pragma once
-#include "core/error.h"
#include "core/Commit.h"
-#include "core/query.h"
#include "core/MemoryID.h"
#include "core/Time.h"
-
+#include "core/error.h"
+#include "core/query.h"
#include "core/wm.h"
-
namespace armarx::armem
{
}
diff --git a/source/RobotAPI/libraries/armem/core/Commit.cpp b/source/RobotAPI/libraries/armem/core/Commit.cpp
index 341d160ec7852bb9d397d283289a4944c1f98409..f6c3a0cace4f4b7e92ab14cbca8c6aef768c9335 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.cpp
+++ b/source/RobotAPI/libraries/armem/core/Commit.cpp
@@ -1,37 +1,35 @@
#include "Commit.h"
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <SimoxUtility/algorithm/apply.hpp>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <SimoxUtility/algorithm/apply.hpp>
-
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::armem
{
- std::ostream& operator<<(std::ostream& os, const EntityUpdate& rhs)
+ std::ostream&
+ operator<<(std::ostream& os, const EntityUpdate& rhs)
{
return os << "Entity update: "
- << "\n- success: \t" << rhs.entityID
- << "\n- timestamp: \t" << toDateTimeMilliSeconds(rhs.referencedTime)
- << "\n- #instances: \t" << rhs.instancesData.size()
- << "\n"
- ;
+ << "\n- success: \t" << rhs.entityID << "\n- timestamp: \t"
+ << toDateTimeMilliSeconds(rhs.referencedTime) << "\n- #instances: \t"
+ << rhs.instancesData.size() << "\n";
}
- std::ostream& operator<<(std::ostream& os, const EntityUpdateResult& rhs)
+ std::ostream&
+ operator<<(std::ostream& os, const EntityUpdateResult& rhs)
{
return os << "Entity update result: "
- << "\n- success: \t" << (rhs.success ? "true" : "false")
- << "\n- snapshotID: \t" << rhs.snapshotID
- << "\n- time arrived: \t" << toDateTimeMilliSeconds(rhs.arrivedTime)
- << "\n- error message: \t" << rhs.errorMessage
- << "\n"
- ;
+ << "\n- success: \t" << (rhs.success ? "true" : "false")
+ << "\n- snapshotID: \t" << rhs.snapshotID << "\n- time arrived: \t"
+ << toDateTimeMilliSeconds(rhs.arrivedTime) << "\n- error message: \t"
+ << rhs.errorMessage << "\n";
}
- std::ostream& operator<<(std::ostream& os, const Commit& rhs)
+ std::ostream&
+ operator<<(std::ostream& os, const Commit& rhs)
{
os << "Commit with " << rhs.updates.size() << " entity update";
if (rhs.updates.size() > 1)
@@ -45,7 +43,9 @@ namespace armarx::armem
}
return os;
}
- std::ostream& operator<<(std::ostream& os, const CommitResult& rhs)
+
+ std::ostream&
+ operator<<(std::ostream& os, const CommitResult& rhs)
{
os << "Commit results of " << rhs.results.size() << " entity update";
if (rhs.results.size() > 1)
@@ -60,35 +60,36 @@ namespace armarx::armem
return os;
}
-
- bool CommitResult::allSuccess() const
+ bool
+ CommitResult::allSuccess() const
{
- return std::find_if(results.begin(), results.end(), [](const EntityUpdateResult & r)
- {
- return !r.success;
- }) == results.end();
+ return std::find_if(results.begin(),
+ results.end(),
+ [](const EntityUpdateResult& r)
+ { return !r.success; }) == results.end();
}
- std::vector<std::string> CommitResult::allErrorMessages() const
+ std::vector<std::string>
+ CommitResult::allErrorMessages() const
{
- return simox::alg::apply(results, [](const EntityUpdateResult & res)
- {
- return res.errorMessage;
- });
+ return simox::alg::apply(results,
+ [](const EntityUpdateResult& res) { return res.errorMessage; });
}
-
- EntityUpdate& Commit::add()
+ EntityUpdate&
+ Commit::add()
{
return updates.emplace_back();
}
- EntityUpdate& Commit::add(const EntityUpdate& update)
+ EntityUpdate&
+ Commit::add(const EntityUpdate& update)
{
return updates.emplace_back(update);
}
- void Commit::add(const std::vector<EntityUpdate>& updates)
+ void
+ Commit::add(const std::vector<EntityUpdate>& updates)
{
for (const auto& update : updates)
{
@@ -96,9 +97,10 @@ namespace armarx::armem
}
}
- void Commit::append(const Commit& c)
+ void
+ Commit::append(const Commit& c)
{
add(c.updates);
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/Commit.h b/source/RobotAPI/libraries/armem/core/Commit.h
index 81d0413849549af39abb5f2733e76d53565a6f3b..f424ec66e15f65fcb1f8c832e44469e76925771c 100644
--- a/source/RobotAPI/libraries/armem/core/Commit.h
+++ b/source/RobotAPI/libraries/armem/core/Commit.h
@@ -1,13 +1,11 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
-#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
-
#include <memory>
#include <vector>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
namespace armarx::armem
{
@@ -65,7 +63,6 @@ namespace armarx::armem
friend std::ostream& operator<<(std::ostream& os, const EntityUpdate& rhs);
};
-
/**
* @brief Result of an `EntityUpdate`.
*/
@@ -81,8 +78,6 @@ namespace armarx::armem
friend std::ostream& operator<<(std::ostream& os, const EntityUpdateResult& rhs);
};
-
-
/**
* @brief A bundle of updates to be sent to the memory.
*/
@@ -118,4 +113,4 @@ namespace armarx::armem
};
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index f127ebd92200346de7421fdd493443971e984045..be0b7224eca898e0f3a15727246c14f60dd0aa46 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -6,11 +6,11 @@
#include <SimoxUtility/algorithm/advanced.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
+
#include "ArmarXCore/core/logging/Logging.h"
#include "error/ArMemError.h"
-
namespace armarx::armem
{
@@ -129,23 +129,29 @@ namespace armarx::armem
return "";
}
- MemoryID MemoryID::cleanID() const
+ MemoryID
+ MemoryID::cleanID() const
{
std::vector<std::string> allItems = this->getAllItems(true);
std::string newID = "";
- for(std::size_t i = 0; i < allItems.size(); i++){
- if(!allItems.at(i).empty()){
+ for (std::size_t i = 0; i < allItems.size(); i++)
+ {
+ if (!allItems.at(i).empty())
+ {
std::string toAppend = allItems.at(i);
- if(allItems.at(i).find(delimiter) != std::string::npos){
+ if (allItems.at(i).find(delimiter) != std::string::npos)
+ {
size_t pos = 0;
std::string token;
- while ((pos = toAppend.find(delimiter)) != std::string::npos) {
+ while ((pos = toAppend.find(delimiter)) != std::string::npos)
+ {
token = toAppend.substr(0, pos);
toAppend.erase(0, pos + delimiter.length());
}
}
newID.append(toAppend);
- if(i < allItems.size() - 1 && !allItems.at(i+1).empty()){
+ if (i < allItems.size() - 1 && !allItems.at(i + 1).empty())
+ {
newID.append(delimiter);
}
}
@@ -153,7 +159,6 @@ namespace armarx::armem
return MemoryID(newID);
}
-
bool
MemoryID::hasGap() const
{
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID_operators.cpp b/source/RobotAPI/libraries/armem/core/MemoryID_operators.cpp
index 8a1e8aa17bf66356061696eee972e587eef96e61..1f1c9dc38159b37759049d266abd4f693889ace0 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID_operators.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID_operators.cpp
@@ -2,8 +2,8 @@
#include "MemoryID.h"
-
-std::ostream& armarx::armem::operator<<(std::ostream& os, const std::vector<MemoryID>& rhs)
+std::ostream&
+armarx::armem::operator<<(std::ostream& os, const std::vector<MemoryID>& rhs)
{
os << "std::vector<MemoryID> with " << rhs.size() << " entries:";
for (size_t i = 0; i < rhs.size(); ++i)
@@ -13,14 +13,14 @@ std::ostream& armarx::armem::operator<<(std::ostream& os, const std::vector<Memo
return os;
}
-
-bool armarx::armem::compareTimestamp(const MemoryID& lhs, const MemoryID& rhs)
+bool
+armarx::armem::compareTimestamp(const MemoryID& lhs, const MemoryID& rhs)
{
return lhs.timestamp < rhs.timestamp;
}
-
-bool armarx::armem::compareTimestampDecreasing(const MemoryID& lhs, const MemoryID& rhs)
+bool
+armarx::armem::compareTimestampDecreasing(const MemoryID& lhs, const MemoryID& rhs)
{
return lhs.timestamp > rhs.timestamp;
}
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID_operators.h b/source/RobotAPI/libraries/armem/core/MemoryID_operators.h
index 7508c81e8ce2245e3b6924f7a706b0eff4390e7e..0824a47981980f5bb22982e10ba93a5e3bc80835 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID_operators.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID_operators.h
@@ -2,9 +2,8 @@
// #include "MemoryID.h"
-#include <vector>
#include <ostream>
-
+#include <vector>
namespace armarx::armem
{
@@ -17,6 +16,4 @@ namespace armarx::armem
/// lhs.timestamp > rhs.timstamp
bool compareTimestampDecreasing(const MemoryID& lhs, const MemoryID& rhs);
-}
-
-
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/Prediction.h b/source/RobotAPI/libraries/armem/core/Prediction.h
index 259ea375e546c64bfee9993939312fb3476ae60a..4ac8d89208484de3142d63b9cd101a1b12f9ca09 100644
--- a/source/RobotAPI/libraries/armem/core/Prediction.h
+++ b/source/RobotAPI/libraries/armem/core/Prediction.h
@@ -26,7 +26,6 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
namespace armarx::armem
{
@@ -67,11 +66,8 @@ namespace armarx::armem
armem::prediction::data::PredictionResult toIce() const;
};
-
- void toIce(armem::prediction::data::PredictionEngine& ice,
- const PredictionEngine& engine);
- void fromIce(const armem::prediction::data::PredictionEngine& ice,
- PredictionEngine& engine);
+ void toIce(armem::prediction::data::PredictionEngine& ice, const PredictionEngine& engine);
+ void fromIce(const armem::prediction::data::PredictionEngine& ice, PredictionEngine& engine);
void toIce(armem::prediction::data::PredictionSettings& ice,
const PredictionSettings& settings);
@@ -79,8 +75,7 @@ namespace armarx::armem
PredictionSettings& settings);
void toIce(armem::prediction::data::PredictionRequest& ice, const PredictionRequest& request);
- void fromIce(const armem::prediction::data::PredictionRequest& ice,
- PredictionRequest& request);
+ void fromIce(const armem::prediction::data::PredictionRequest& ice, PredictionRequest& request);
void toIce(armem::prediction::data::PredictionResult& ice, const PredictionResult& result);
void fromIce(const armem::prediction::data::PredictionResult& ice, PredictionResult& result);
diff --git a/source/RobotAPI/libraries/armem/core/SuccessHeader.cpp b/source/RobotAPI/libraries/armem/core/SuccessHeader.cpp
index 940c5445519b686d8b80acdb5b651f7c3ecdc45f..5d9b5510595a07f309137fef76536873b9b95d5e 100644
--- a/source/RobotAPI/libraries/armem/core/SuccessHeader.cpp
+++ b/source/RobotAPI/libraries/armem/core/SuccessHeader.cpp
@@ -1,6 +1,5 @@
#include "SuccessHeader.h"
-
namespace armarx::armem
{
diff --git a/source/RobotAPI/libraries/armem/core/SuccessHeader.h b/source/RobotAPI/libraries/armem/core/SuccessHeader.h
index da4b241d84648d5a1e670a18ea88812f4bd812ab..f4aa63d5c3787edb271b595e5910a9829407713d 100644
--- a/source/RobotAPI/libraries/armem/core/SuccessHeader.h
+++ b/source/RobotAPI/libraries/armem/core/SuccessHeader.h
@@ -7,7 +7,6 @@
#include "../core/MemoryID.h"
#include "../core/Time.h"
-
namespace armarx::armem::detail
{
struct SuccessHeader
@@ -21,17 +20,19 @@ namespace armarx::armem::detail
std::string errorMessage;
};
-
template <class Ice>
- void toIce(Ice& ice, const SuccessHeader& header)
+ void
+ toIce(Ice& ice, const SuccessHeader& header)
{
ice.success = header.success;
ice.errorMessage = header.errorMessage;
}
+
template <class Ice>
- void fromIce(const Ice& ice, SuccessHeader& header)
+ void
+ fromIce(const Ice& ice, SuccessHeader& header)
{
header.success = ice.success;
header.errorMessage = ice.errorMessage;
}
-}
+} // namespace armarx::armem::detail
diff --git a/source/RobotAPI/libraries/armem/core/Time.cpp b/source/RobotAPI/libraries/armem/core/Time.cpp
index 80933ac0bd178a17f49c50fe8c89931177d496d5..95851775ec4c58a411cda88cf539ecc6d106921a 100644
--- a/source/RobotAPI/libraries/armem/core/Time.cpp
+++ b/source/RobotAPI/libraries/armem/core/Time.cpp
@@ -3,12 +3,12 @@
#include <cmath>
#include <iomanip>
-
namespace armarx
{
- std::string armem::toStringMilliSeconds(const Time& time, int decimals)
+ std::string
+ armem::toStringMilliSeconds(const Time& time, int decimals)
{
std::stringstream ss;
ss << time.toMicroSecondsSinceEpoch() / 1000;
@@ -22,8 +22,8 @@ namespace armarx
return ss.str();
}
-
- std::string armem::toStringMicroSeconds(const Time& time)
+ std::string
+ armem::toStringMicroSeconds(const Time& time)
{
static const char* mu = "\u03BC";
std::stringstream ss;
@@ -31,8 +31,8 @@ namespace armarx
return ss.str();
}
-
- std::string armem::toDateTimeMilliSeconds(const Time& time, int decimals)
+ std::string
+ armem::toDateTimeMilliSeconds(const Time& time, int decimals)
{
std::stringstream ss;
ss << time.toString("%Y-%m-%d %H:%M:%S");
@@ -46,13 +46,10 @@ namespace armarx
return ss.str();
}
-
- armem::Time armem::timeFromStringMicroSeconds(const std::string& microSeconds)
+ armem::Time
+ armem::timeFromStringMicroSeconds(const std::string& microSeconds)
{
return Time(Duration::MicroSeconds(std::stol(microSeconds)), ClockType::Virtual);
}
-}
-
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem/core/Time.h b/source/RobotAPI/libraries/armem/core/Time.h
index b4e100e91334abeedac42b62efbcfa79ad6b40c6..6476b612e763a8b438268b495290007607120304 100644
--- a/source/RobotAPI/libraries/armem/core/Time.h
+++ b/source/RobotAPI/libraries/armem/core/Time.h
@@ -7,7 +7,6 @@
#include "forward_declarations.h"
-
namespace armarx::armem
{
@@ -35,4 +34,4 @@ namespace armarx::armem
*/
Time timeFromStringMicroSeconds(const std::string& microSeconds);
-} // namespace armarx::armem
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/actions.cpp b/source/RobotAPI/libraries/armem/core/actions.cpp
index 9eb28f7187d9ac51295dfa3be351f14fc97ef2cd..77f476be4e1937bca724adcb51487aec7a357f75 100644
--- a/source/RobotAPI/libraries/armem/core/actions.cpp
+++ b/source/RobotAPI/libraries/armem/core/actions.cpp
@@ -72,7 +72,6 @@ namespace armarx::armem::actions
return this->entries.emplace_back(id, text, entries);
}
-
Menu::Menu(std::initializer_list<MenuEntry> entries) : entries(entries)
{
}
@@ -81,7 +80,6 @@ namespace armarx::armem::actions
{
}
-
MenuEntry&
Menu::add(const std::string& id, const std::string& text, const std::vector<MenuEntry>& entries)
{
diff --git a/source/RobotAPI/libraries/armem/core/actions.h b/source/RobotAPI/libraries/armem/core/actions.h
index 2d96dbbac3f5b8f614772a881b504793d78b40a9..5c0ce71963c4199a75b55ba1e4ea7925132f26c3 100644
--- a/source/RobotAPI/libraries/armem/core/actions.h
+++ b/source/RobotAPI/libraries/armem/core/actions.h
@@ -30,7 +30,6 @@ namespace armarx::armem::actions
std::vector<MenuEntry> entries;
};
-
struct Action : public MenuEntry
{
Action(const std::string& id, const std::string& text);
@@ -47,7 +46,6 @@ namespace armarx::armem::actions
const std::vector<MenuEntry>& entries = {});
};
-
class Menu
{
@@ -68,10 +66,10 @@ namespace armarx::armem::actions
std::vector<MenuEntry> entries;
};
+ using data::ActionPath;
using data::ExecuteActionInputSeq;
using data::ExecuteActionOutputSeq;
using data::GetActionsInputSeq;
using data::GetActionsOutputSeq;
- using data::ActionPath;
} // namespace armarx::armem::actions
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
index 575b15314fb4fd4095ccade3db8b436e09b5e5af..152b5f18f7984bed0b373689640b33d2c738cd74 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.cpp
@@ -1,11 +1,11 @@
#include "aron_conversions.h"
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-void armarx::armem::fromAron(const arondto::MemoryID& dto, MemoryID& bo)
+void
+armarx::armem::fromAron(const arondto::MemoryID& dto, MemoryID& bo)
{
aron::fromAron(dto.memoryName, bo.memoryName);
aron::fromAron(dto.coreSegmentName, bo.coreSegmentName);
@@ -13,10 +13,10 @@ void armarx::armem::fromAron(const arondto::MemoryID& dto, MemoryID& bo)
aron::fromAron(dto.entityName, bo.entityName);
aron::fromAron(dto.timestamp, bo.timestamp);
aron::fromAron(dto.instanceIndex, bo.instanceIndex);
-
}
-void armarx::armem::toAron(arondto::MemoryID& dto, const MemoryID& bo)
+void
+armarx::armem::toAron(arondto::MemoryID& dto, const MemoryID& bo)
{
aron::toAron(dto.memoryName, bo.memoryName);
aron::toAron(dto.coreSegmentName, bo.coreSegmentName);
@@ -24,10 +24,10 @@ void armarx::armem::toAron(arondto::MemoryID& dto, const MemoryID& bo)
aron::toAron(dto.entityName, bo.entityName);
aron::toAron(dto.timestamp, bo.timestamp);
aron::toAron(dto.instanceIndex, bo.instanceIndex);
-
}
-std::ostream& armarx::armem::arondto::operator<<(std::ostream& os, const MemoryID& rhs)
+std::ostream&
+armarx::armem::arondto::operator<<(std::ostream& os, const MemoryID& rhs)
{
armem::MemoryID bo;
fromAron(rhs, bo);
diff --git a/source/RobotAPI/libraries/armem/core/aron_conversions.h b/source/RobotAPI/libraries/armem/core/aron_conversions.h
index f0b1a26591333440dd41dcd5471a30c061e48f38..a71f3115ed67af4cb13752d0440b1979c0c8a5db 100644
--- a/source/RobotAPI/libraries/armem/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/aron_conversions.h
@@ -4,7 +4,6 @@
#include "forward_declarations.h"
-
namespace armarx::armem
{
// Implemented in <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
@@ -13,7 +12,8 @@ namespace armarx::armem
void fromAron(const arondto::MemoryID& dto, MemoryID& bo);
void toAron(arondto::MemoryID& dto, const MemoryID& bo);
-}
+} // namespace armarx::armem
+
namespace armarx::armem::arondto
{
std::ostream& operator<<(std::ostream& os, const MemoryID& rhs);
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp b/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp
index d3038a357bdad823f04e8d154a569a7eebaf90d5..0d2c67b63d566f7170bd9b31cfaf18785b85f87b 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/EntityBase.cpp
@@ -1,2 +1 @@
#include "EntityBase.h"
-
diff --git a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp
index a2cabd58bd92564c68374f0bf61ab06ef5990135..3853007a3f464e3359441e257cb003a4ca1a7161 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/EntityInstanceBase.cpp
@@ -1,32 +1,31 @@
#include "EntityInstanceBase.h"
-
namespace armarx::armem::base
{
- void EntityInstanceMetadata::access() const
+ void
+ EntityInstanceMetadata::access() const
{
numAccessed++;
lastAccessedTime = armarx::core::time::DateTime::Now();
}
- bool EntityInstanceMetadata::operator==(const EntityInstanceMetadata& other) const
+ bool
+ EntityInstanceMetadata::operator==(const EntityInstanceMetadata& other) const
{
- return referencedTime == other.referencedTime
- && sentTime == other.sentTime
- && arrivedTime == other.arrivedTime
- && std::abs(confidence - other.confidence) < 1e-6f;
+ return referencedTime == other.referencedTime && sentTime == other.sentTime &&
+ arrivedTime == other.arrivedTime && std::abs(confidence - other.confidence) < 1e-6f;
}
-}
-
+} // namespace armarx::armem::base
-std::ostream& armarx::armem::base::operator<<(std::ostream& os, const EntityInstanceMetadata& d)
+std::ostream&
+armarx::armem::base::operator<<(std::ostream& os, const EntityInstanceMetadata& d)
{
os << "EntityInstanceMetadata: "
<< "\n - t_referenced = \t" << armem::toStringMicroSeconds(d.referencedTime) << " us"
<< "\n - t_sent = \t" << armem::toStringMicroSeconds(d.sentTime) << " us"
<< "\n - t_arrived = \t" << armem::toStringMicroSeconds(d.arrivedTime) << " us"
- << "\n - t_accessed = \t" << armem::toStringMicroSeconds(d.lastAccessedTime) << " us (" << d.numAccessed << ")"
- << "\n - confidence = \t" << d.confidence << " us"
- ;
+ << "\n - t_accessed = \t" << armem::toStringMicroSeconds(d.lastAccessedTime) << " us ("
+ << d.numAccessed << ")"
+ << "\n - confidence = \t" << d.confidence << " us";
return os;
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp
index 72b4c48877c78dce70737fa58c58cd1b82b2d2e5..a728693895b881bee31d27289a89b65ef8923ff3 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.cpp
@@ -2,10 +2,10 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-void armarx::armem::base::detail::throwIfNotEqual(const Time& ownTime, const Time& updateTime)
+void
+armarx::armem::base::detail::throwIfNotEqual(const Time& ownTime, const Time& updateTime)
{
ARMARX_CHECK_EQUAL(ownTime, updateTime)
- << "Cannot update a snapshot to an update with another timestamp. \n"
- << "Note: A snapshot's timestamp must not be changed after construction.";
+ << "Cannot update a snapshot to an update with another timestamp. \n"
+ << "Note: A snapshot's timestamp must not be changed after construction.";
}
diff --git a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
index c1eeea7275cecc1f4dd2c76d7723720681fcead1..f0e661215b148eba60e7f3359500611c6b669e4a 100644
--- a/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h
@@ -269,7 +269,8 @@ namespace armarx::armem::base
EntityInstanceT&
addInstance(EntityInstanceT&& instance)
{
- if (instance.index() > 0 && static_cast<size_t>(instance.index()) > this->_container.size())
+ if (instance.index() > 0 &&
+ static_cast<size_t>(instance.index()) > this->_container.size())
{
throw error::InvalidArgument(
std::to_string(instance.index()),
@@ -288,7 +289,8 @@ namespace armarx::armem::base
addInstance()
{
//ARMARX_INFO << "Trying to add an instance without instance yet generated";
- int index = static_cast<int>(this->size()); //this is the problem, because instances have names 0 and 11 (I do not know why)
+ int index = static_cast<int>(
+ this->size()); //this is the problem, because instances have names 0 and 11 (I do not know why)
EntityInstanceT& added = this->_container.emplace_back(EntityInstanceT());
added.id() = this->id().withInstanceIndex(index);
return added;
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
index 488b02830061c4c391e77e85875f34987051c90b..dd7f85089e79c4f0dad5623313fef5cba5ab9362 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.cpp
@@ -2,28 +2,30 @@
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
-
namespace armarx::armem::base::detail
{
- AronTyped::AronTyped(aron::type::ObjectPtr aronType) :
- _aronType(aronType)
- {}
+ AronTyped::AronTyped(aron::type::ObjectPtr aronType) : _aronType(aronType)
+ {
+ }
- bool AronTyped::hasAronType() const
+ bool
+ AronTyped::hasAronType() const
{
return _aronType != nullptr;
}
- aron::type::ObjectPtr& AronTyped::aronType()
+ aron::type::ObjectPtr&
+ AronTyped::aronType()
{
return _aronType;
}
- aron::type::ObjectPtr AronTyped::aronType() const
+ aron::type::ObjectPtr
+ AronTyped::aronType() const
{
return _aronType;
}
-}
+} // namespace armarx::armem::base::detail
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
index 04537a1be95b88147f68091334195e601298bd77..d5258b525ef6548d6f511e23ea2fa2b8989a57c0 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/AronTyped.h
@@ -2,7 +2,6 @@
#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
-
namespace armarx::armem::base::detail
{
@@ -12,7 +11,6 @@ namespace armarx::armem::base::detail
class AronTyped
{
public:
-
explicit AronTyped(aron::type::ObjectPtr aronType = nullptr);
@@ -22,11 +20,9 @@ namespace armarx::armem::base::detail
protected:
-
/// The expected Aron type. May be nullptr, in which case no type information is available.
aron::type::ObjectPtr _aronType;
-
};
-}
+} // namespace armarx::armem::base::detail
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
index 4b7fce0a6805fb1e7505f09cca0b9f802e994f61..9aa0e124d440faa2adfd6362488346a8f9ac075c 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryContainerBase.h
@@ -5,7 +5,6 @@
#include "MemoryItem.h"
#include "iteration_mixins.h"
-
namespace armarx::armem::base::detail
{
/**
@@ -13,23 +12,21 @@ namespace armarx::armem::base::detail
* iterators (which requires a template).
*/
template <class _ContainerT, class _DerivedT>
- class MemoryContainerBase :
- public MemoryItem
+ class MemoryContainerBase : public MemoryItem
{
using Base = MemoryItem;
public:
-
using DerivedT = _DerivedT;
using ContainerT = _ContainerT;
public:
-
MemoryContainerBase()
- {}
- explicit MemoryContainerBase(const MemoryID& id) :
- MemoryItem(id)
+ {
+ }
+
+ explicit MemoryContainerBase(const MemoryID& id) : MemoryItem(id)
{
}
@@ -38,91 +35,109 @@ namespace armarx::armem::base::detail
MemoryContainerBase& operator=(const MemoryContainerBase& other) = default;
MemoryContainerBase& operator=(MemoryContainerBase&& other) = default;
-
// Container methods
- bool empty() const
+ bool
+ empty() const
{
return _container.empty();
}
- std::size_t size() const
+
+ std::size_t
+ size() const
{
return _container.size();
}
- void clear()
+
+ void
+ clear()
{
return _container.clear();
}
-
// ITERATION
/**
* @param func Function like: bool process(ChildT& provSeg)
*/
template <class ChildFunctionT>
- bool forEachChild(ChildFunctionT&& func)
+ bool
+ forEachChild(ChildFunctionT&& func)
{
return base::detail::forEachChild(this->_container, func);
}
+
/**
* @param func Function like: bool process(const ChildT& provSeg)
*/
template <class ChildFunctionT>
- bool forEachChild(ChildFunctionT&& func) const
+ bool
+ forEachChild(ChildFunctionT&& func) const
{
return base::detail::forEachChild(this->_container, func);
}
-
[[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
- typename ContainerT::const_iterator begin() const
+ typename ContainerT::const_iterator
+ begin() const
{
return _container.begin();
}
+
[[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
- typename ContainerT::iterator begin()
+ typename ContainerT::iterator
+ begin()
{
return _container.begin();
}
+
[[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
- typename ContainerT::const_iterator end() const
+ typename ContainerT::const_iterator
+ end() const
{
return _container.end();
}
+
[[deprecated("Direct container access is deprecated. Use forEach*() instead.")]]
- typename ContainerT::iterator end()
+ typename ContainerT::iterator
+ end()
{
return _container.end();
}
protected:
-
- const ContainerT& container() const
+ const ContainerT&
+ container() const
{
return _container;
}
- ContainerT& container()
+
+ ContainerT&
+ container()
{
return _container;
}
- DerivedT& _derived()
+ DerivedT&
+ _derived()
{
return static_cast<DerivedT&>(*this);
}
- const DerivedT& _derived() const
+
+ const DerivedT&
+ _derived() const
{
return static_cast<DerivedT&>(*this);
}
-
/**
* @throw `armem::error::ContainerNameMismatch` if `gottenName` does not match `actualName`.
*/
- void _checkContainerName(const std::string& gottenName, const std::string& actualName,
- bool emptyOk = true) const
+ void
+ _checkContainerName(const std::string& gottenName,
+ const std::string& actualName,
+ bool emptyOk = true) const
{
if (!((emptyOk && gottenName.empty()) || gottenName == actualName))
{
@@ -131,9 +146,9 @@ namespace armarx::armem::base::detail
}
}
-
- template <class ChildT, class KeyT, class ...ChildArgs>
- ChildT& _addChild(const KeyT& key, ChildArgs... childArgs)
+ template <class ChildT, class KeyT, class... ChildArgs>
+ ChildT&
+ _addChild(const KeyT& key, ChildArgs... childArgs)
{
auto [it, inserted] = this->_container.try_emplace(key, childArgs...);
if (not inserted)
@@ -146,9 +161,7 @@ namespace armarx::armem::base::detail
protected:
-
mutable ContainerT _container;
-
};
-}
+} // namespace armarx::armem::base::detail
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp
index 8f4a1920f0a1fcd669b9c3fc2193a617b4dba205..ed4c899a634cf8aa85cdd4e61cb34c0dd61672eb 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.cpp
@@ -1,6 +1,5 @@
#include "MemoryItem.h"
-
namespace armarx::armem::base::detail
{
@@ -8,15 +7,12 @@ namespace armarx::armem::base::detail
{
}
-
- MemoryItem::MemoryItem(const MemoryID& id) :
- _id(id)
+ MemoryItem::MemoryItem(const MemoryID& id) : _id(id)
{
}
-
MemoryItem::~MemoryItem()
{
}
-}
+} // namespace armarx::armem::base::detail
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
index 6aea07121aa201fcbbf0f0b18f46ccc3ab4e2aac..850d929e92d2168f6dc78d87a7e3f43b55f5a0c0 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/MemoryItem.h
@@ -4,7 +4,6 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
namespace armarx::armem::base::detail
{
@@ -14,7 +13,6 @@ namespace armarx::armem::base::detail
class MemoryItem
{
public:
-
MemoryItem();
explicit MemoryItem(const MemoryID& id);
@@ -23,28 +21,27 @@ namespace armarx::armem::base::detail
MemoryItem& operator=(const MemoryItem& other) = default;
MemoryItem& operator=(MemoryItem&& other) = default;
-
- inline MemoryID& id()
+ inline MemoryID&
+ id()
{
return _id;
}
- inline const MemoryID& id() const
+
+ inline const MemoryID&
+ id() const
{
return _id;
}
protected:
-
// Protected so we get a compile error if someone tries to destruct a MemoryItem
// instead of a derived type.
~MemoryItem();
protected:
-
MemoryID _id;
-
};
-}
+} // namespace armarx::armem::base::detail
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/Predictive.h b/source/RobotAPI/libraries/armem/core/base/detail/Predictive.h
index ea9ec0dde71cbdb19c6bebe0e160bfbcea8c8155..cbb7320b834f03d89fcc4e999cbaae2472e80a94 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/Predictive.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/Predictive.h
@@ -27,7 +27,6 @@
#include "derived.h"
-
namespace armarx::armem::base::detail
{
@@ -38,7 +37,6 @@ namespace armarx::armem::base::detail
class Predictive
{
public:
-
explicit Predictive(const std::vector<PredictionEngine>& engines = {}) :
_predictionEngines(engines)
{
@@ -50,12 +48,14 @@ namespace armarx::armem::base::detail
return _predictionEngines;
}
- void addPredictionEngine(const PredictionEngine& engine)
+ void
+ addPredictionEngine(const PredictionEngine& engine)
{
_predictionEngines.push_back(engine);
}
- void setPredictionEngines(const std::vector<PredictionEngine>& engines)
+ void
+ setPredictionEngines(const std::vector<PredictionEngine>& engines)
{
this->_predictionEngines = engines;
}
@@ -78,12 +78,9 @@ namespace armarx::armem::base::detail
private:
-
std::vector<PredictionEngine> _predictionEngines;
-
};
-
/**
* @brief Something that supports a set of prediction engines.
*/
@@ -91,10 +88,8 @@ namespace armarx::armem::base::detail
class PredictiveContainer : public Predictive<DerivedT>
{
public:
-
using Predictive<DerivedT>::Predictive;
-
std::map<MemoryID, std::vector<PredictionEngine>>
getAllPredictionEngines() const
{
@@ -118,7 +113,6 @@ namespace armarx::armem::base::detail
return engines;
}
-
};
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/derived.h b/source/RobotAPI/libraries/armem/core/base/detail/derived.h
index 721becfa05e34e603f2c1bdb852cea191faf42a5..14c4fd7078787836841fc62c0916fc2d06419f34 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/derived.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/derived.h
@@ -1,6 +1,5 @@
#pragma once
-
namespace armarx::armem::base::detail
{
@@ -11,7 +10,6 @@ namespace armarx::armem::base::detail
return static_cast<DerivedT&>(*t);
}
-
template <class DerivedT, class ThisT>
const DerivedT&
derived(const ThisT* t)
@@ -19,4 +17,4 @@ namespace armarx::armem::base::detail
return static_cast<const DerivedT&>(*t);
}
-}
+} // namespace armarx::armem::base::detail
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp
index 459cd4d5c7bd694bcb289496352d7e2cad4966cd..bc5fa1712ab280aa446f2f0f26af077d8edfba12 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.cpp
@@ -1,6 +1,5 @@
#include "iteration_mixins.h"
-
namespace armarx::armem::base::detail
{
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
index 07930b46e931513cd42f4a6dcdaacd5a3f80a56d..29c3a55e674b506e44121d1186815aae95bce27e 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/iteration_mixins.h
@@ -2,6 +2,7 @@
#include <functional>
#include <type_traits>
+
#include "ArmarXCore/core/logging/Logging.h"
#include <RobotAPI/libraries/armem/core/MemoryID.h>
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.cpp b/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.cpp
index 2b35f0b895cf6e4ef3e53a81a8af3ac0eb31a89d..cd18b8cfb786f2f9aff74ec3e06d35a326f3a18a 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/lookup_mixins.cpp
@@ -2,11 +2,11 @@
#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
-
namespace armarx::armem::base
{
- void detail::checkHasInstanceIndex(const MemoryID& instanceID)
+ void
+ detail::checkHasInstanceIndex(const MemoryID& instanceID)
{
if (not instanceID.hasInstanceIndex())
{
@@ -14,8 +14,8 @@ namespace armarx::armem::base
}
}
-
- void detail::checkHasTimestamp(const MemoryID& snapshotID)
+ void
+ detail::checkHasTimestamp(const MemoryID& snapshotID)
{
if (not snapshotID.hasTimestamp())
{
@@ -23,8 +23,8 @@ namespace armarx::armem::base
}
}
-
- void detail::checkHasEntityName(const MemoryID& entityID)
+ void
+ detail::checkHasEntityName(const MemoryID& entityID)
{
if (not entityID.hasEntityName())
{
@@ -32,26 +32,28 @@ namespace armarx::armem::base
}
}
-
- void detail::checkHasProviderSegmentName(const MemoryID& providerSegmentID)
+ void
+ detail::checkHasProviderSegmentName(const MemoryID& providerSegmentID)
{
if (not providerSegmentID.hasProviderSegmentName())
{
- throw armem::error::InvalidMemoryID(providerSegmentID, "Provider segment ID has no provider segment name.");
+ throw armem::error::InvalidMemoryID(
+ providerSegmentID, "Provider segment ID has no provider segment name.");
}
}
-
- void detail::checkHasCoreSegmentName(const MemoryID& coreSegmentID)
+ void
+ detail::checkHasCoreSegmentName(const MemoryID& coreSegmentID)
{
if (not coreSegmentID.hasCoreSegmentName())
{
- throw armem::error::InvalidMemoryID(coreSegmentID, "Core segment ID has no core segment name.");
+ throw armem::error::InvalidMemoryID(coreSegmentID,
+ "Core segment ID has no core segment name.");
}
}
-
- void detail::checkHasMemoryName(const MemoryID& memoryID)
+ void
+ detail::checkHasMemoryName(const MemoryID& memoryID)
{
if (not memoryID.hasMemoryName())
{
@@ -60,4 +62,4 @@ namespace armarx::armem::base
}
-}
+} // namespace armarx::armem::base
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.cpp b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.cpp
index a1875ff53545a3d2574cf9764a14d321946e5514..303f66d1f40f87c07115e2f54dd9b5ebfcff9aed 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.cpp
@@ -2,8 +2,8 @@
#include <algorithm>
-
-size_t armarx::armem::base::detail::negativeIndexSemantics(long index, size_t size)
+size_t
+armarx::armem::base::detail::negativeIndexSemantics(long index, size_t size)
{
const size_t max = size > 0 ? size - 1 : 0;
if (index >= 0)
@@ -12,6 +12,7 @@ size_t armarx::armem::base::detail::negativeIndexSemantics(long index, size_t si
}
else
{
- return static_cast<size_t>(std::clamp<long>(static_cast<long>(size) + index, 0, static_cast<long>(max)));
+ return static_cast<size_t>(
+ std::clamp<long>(static_cast<long>(size) + index, 0, static_cast<long>(max)));
}
}
diff --git a/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h
index 4efe169276c83ff12c13400e6de162a8b4db5d8d..712ce8a0ed24c94db46dcb1a15e36ccfa7cb7246 100644
--- a/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h
+++ b/source/RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h
@@ -2,7 +2,6 @@
#include <stddef.h>
-
namespace armarx::armem::base::detail
{
diff --git a/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
index 42620f2d928b91f7941600315a9970b16111618c..04d6edcdbc795816ab26c03ef0f6e3dd1f8a35d2 100644
--- a/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/base/ice_conversions.cpp
@@ -1,32 +1,35 @@
#include "ice_conversions.h"
-#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-
-
namespace armarx::armem::base
{
- void detail::toIceItem(data::detail::MemoryItem& ice, const armem::base::detail::MemoryItem& item)
+ void
+ detail::toIceItem(data::detail::MemoryItem& ice, const armem::base::detail::MemoryItem& item)
{
toIce(ice.id, item.id());
}
- void detail::fromIceItem(const data::detail::MemoryItem& ice, armem::base::detail::MemoryItem& item)
+
+ void
+ detail::fromIceItem(const data::detail::MemoryItem& ice, armem::base::detail::MemoryItem& item)
{
fromIce(ice.id, item.id());
}
-
- void detail::toIce(aron::data::dto::DictPtr& ice, const aron::data::DictPtr& data)
+ void
+ detail::toIce(aron::data::dto::DictPtr& ice, const aron::data::DictPtr& data)
{
ice = data ? data->toAronDictDTO() : nullptr;
}
- void detail::fromIce(const aron::data::dto::DictPtr& ice, aron::data::DictPtr& data)
+
+ void
+ detail::fromIce(const aron::data::dto::DictPtr& ice, aron::data::DictPtr& data)
{
if (ice)
{
@@ -38,22 +41,26 @@ namespace armarx::armem::base
};
}
- void detail::toIce(aron::type::dto::GenericTypePtr& ice, const aron::type::ObjectPtr& bo)
+ void
+ detail::toIce(aron::type::dto::GenericTypePtr& ice, const aron::type::ObjectPtr& bo)
{
ice = bo ? bo->toAronDTO() : nullptr;
}
- void detail::fromIce(const aron::type::dto::GenericTypePtr& ice, aron::type::ObjectPtr& bo)
+
+ void
+ detail::fromIce(const aron::type::dto::GenericTypePtr& ice, aron::type::ObjectPtr& bo)
{
- bo = ice
- ? aron::type::Object::DynamicCastAndCheck(aron::type::Variant::FromAronDTO(*ice))
- : nullptr;
+ bo = ice ? aron::type::Object::DynamicCastAndCheck(aron::type::Variant::FromAronDTO(*ice))
+ : nullptr;
}
-}
+} // namespace armarx::armem::base
+
namespace armarx::armem
{
- void base::toIce(data::EntityInstanceMetadata& ice, const EntityInstanceMetadata& metadata)
+ void
+ base::toIce(data::EntityInstanceMetadata& ice, const EntityInstanceMetadata& metadata)
{
ice.confidence = metadata.confidence;
toIce(ice.arrivedTime, metadata.arrivedTime);
@@ -62,7 +69,9 @@ namespace armarx::armem
toIce(ice.lastAccessedTime, metadata.lastAccessedTime);
ice.accessed = metadata.numAccessed;
}
- void base::fromIce(const data::EntityInstanceMetadata& ice, EntityInstanceMetadata& metadata)
+
+ void
+ base::fromIce(const data::EntityInstanceMetadata& ice, EntityInstanceMetadata& metadata)
{
metadata.confidence = ice.confidence;
fromIce(ice.arrivedTime, metadata.arrivedTime);
@@ -72,14 +81,16 @@ namespace armarx::armem
metadata.numAccessed = ice.accessed;
}
-
- void base::toIce(data::EntityInstanceMetadataPtr& ice, const EntityInstanceMetadata& metadata)
+ void
+ base::toIce(data::EntityInstanceMetadataPtr& ice, const EntityInstanceMetadata& metadata)
{
armarx::toIce<data::EntityInstanceMetadata>(ice, metadata);
}
- void base::fromIce(const data::EntityInstanceMetadataPtr& ice, EntityInstanceMetadata& metadata)
+
+ void
+ base::fromIce(const data::EntityInstanceMetadataPtr& ice, EntityInstanceMetadata& metadata)
{
armarx::fromIce<data::EntityInstanceMetadata>(ice, metadata);
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/container_maps.h b/source/RobotAPI/libraries/armem/core/container_maps.h
index 6b836da0b833ea76ce24a1920e0247255e9b6e61..4d8dfad3e871be4e334382a92807d14f5ee61ab3 100644
--- a/source/RobotAPI/libraries/armem/core/container_maps.h
+++ b/source/RobotAPI/libraries/armem/core/container_maps.h
@@ -67,8 +67,7 @@ namespace armarx::armem
{
curKey = prefixFunc(curKey.value());
}
- }
- while (result == keyValMap.end() and curKey.has_value());
+ } while (result == keyValMap.end() and curKey.has_value());
return result;
}
@@ -112,8 +111,7 @@ namespace armarx::armem
{
curKey = prefixFunc(curKey.value());
}
- }
- while (result == keyValMap.end() and curKey.has_value());
+ } while (result == keyValMap.end() and curKey.has_value());
return result;
}
@@ -135,10 +133,11 @@ namespace armarx::armem
*/
template <typename KeyT, typename ValueT, typename AccumulateT>
AccumulateT
- accumulateFromPrefixes(const std::map<KeyT, ValueT>& keyValMap,
- const std::function<std::optional<KeyT>(const KeyT&)>& prefixFunc,
- const std::function<void(AccumulateT&, const ValueT&)> accumulateFunc,
- const KeyT& key)
+ accumulateFromPrefixes(
+ const std::map<KeyT, ValueT>& keyValMap,
+ const std::function<std::optional<KeyT>(const KeyT&)>& prefixFunc,
+ const std::function<void(AccumulateT&, const ValueT&)> accumulateFunc,
+ const KeyT& key)
{
std::optional<KeyT> curKey = key;
AccumulateT values;
@@ -155,8 +154,7 @@ namespace armarx::armem
{
curKey.reset();
}
- }
- while (curKey.has_value());
+ } while (curKey.has_value());
return values;
}
@@ -212,7 +210,6 @@ namespace armarx::armem
} // namespace detail
-
std::optional<MemoryID> inline getMemoryIDParent(const MemoryID& memID)
{
if (!memID.hasMemoryName())
@@ -223,7 +220,6 @@ namespace armarx::armem
return {parent};
}
-
/**
* @brief Find the entry with the most specific key that contains the given ID,
* or `idMap.end()` if no key contains the ID.
@@ -237,7 +233,6 @@ namespace armarx::armem
return detail::findEntryWithLongestPrefix<MemoryID, ValueT>(idMap, &getMemoryIDParent, id);
}
-
/**
* @brief Find the entry with the most specific key that contains the given ID
* and satisfies the predicate, or `idMap.end()` if no key contains the ID and satisfies
@@ -247,14 +242,15 @@ namespace armarx::armem
*/
template <typename ValueT>
typename std::map<MemoryID, ValueT>::const_iterator
- findMostSpecificEntryContainingIDAnd(const std::map<MemoryID, ValueT>& idMap, const MemoryID& id,
- const std::function<bool(const MemoryID&, const ValueT&)>& predicate)
+ findMostSpecificEntryContainingIDAnd(
+ const std::map<MemoryID, ValueT>& idMap,
+ const MemoryID& id,
+ const std::function<bool(const MemoryID&, const ValueT&)>& predicate)
{
return detail::findEntryWithLongestPrefixAnd<MemoryID, ValueT>(
idMap, &getMemoryIDParent, id, predicate);
}
-
/**
* @brief Return all values of keys containing the given ID.
*
@@ -267,7 +263,6 @@ namespace armarx::armem
return detail::accumulateFromPrefixes<MemoryID, ValueT>(idMap, &getMemoryIDParent, id);
}
-
/**
* @brief Return all values of keys containing the given ID in a flattened vector.
*
diff --git a/source/RobotAPI/libraries/armem/core/error.h b/source/RobotAPI/libraries/armem/core/error.h
index 31bef6c2746eb276f23cb9102c61c295810c39d7..0e2b63f95189304ba351e44c1f83cbb021d94d9a 100644
--- a/source/RobotAPI/libraries/armem/core/error.h
+++ b/source/RobotAPI/libraries/armem/core/error.h
@@ -3,10 +3,8 @@
#include "error/ArMemError.h"
#include "error/mns.h"
-
namespace armarx::armem::error
{
}
-
diff --git a/source/RobotAPI/libraries/armem/core/error/mns.cpp b/source/RobotAPI/libraries/armem/core/error/mns.cpp
index da8b8f4855ac67e575cb1e9a677eea277dc1cc58..b80c02181f45862a5647e6ddcbb304d56c39ec59 100644
--- a/source/RobotAPI/libraries/armem/core/error/mns.cpp
+++ b/source/RobotAPI/libraries/armem/core/error/mns.cpp
@@ -4,16 +4,18 @@
#include "../MemoryID.h"
-
namespace armarx::armem::error
{
- MemoryNameSystemQueryFailed::MemoryNameSystemQueryFailed(const std::string& function, const std::string& errorMessage) :
+ MemoryNameSystemQueryFailed::MemoryNameSystemQueryFailed(const std::string& function,
+ const std::string& errorMessage) :
ArMemError(makeMsg(function, errorMessage))
{
}
- std::string MemoryNameSystemQueryFailed::makeMsg(const std::string& function, const std::string& errorMessage)
+ std::string
+ MemoryNameSystemQueryFailed::makeMsg(const std::string& function,
+ const std::string& errorMessage)
{
std::stringstream ss;
ss << "Failed to call '" << function << "' on the memory name system.\n";
@@ -24,14 +26,14 @@ namespace armarx::armem::error
return ss.str();
}
-
-
- CouldNotResolveMemoryServer::CouldNotResolveMemoryServer(const MemoryID& memoryID, const std::string& errorMessage) :
+ CouldNotResolveMemoryServer::CouldNotResolveMemoryServer(const MemoryID& memoryID,
+ const std::string& errorMessage) :
ArMemError(makeMsg(memoryID, errorMessage))
{
}
- std::string CouldNotResolveMemoryServer::makeMsg(const MemoryID& memoryID, const std::string& errorMessage)
+ std::string
+ CouldNotResolveMemoryServer::makeMsg(const MemoryID& memoryID, const std::string& errorMessage)
{
std::stringstream ss;
ss << "Could not resolve the memory name '" << memoryID << "'."
@@ -43,17 +45,22 @@ namespace armarx::armem::error
return ss.str();
}
-
- ServerRegistrationOrRemovalFailed::ServerRegistrationOrRemovalFailed(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage) :
+ ServerRegistrationOrRemovalFailed::ServerRegistrationOrRemovalFailed(
+ const std::string& verb,
+ const MemoryID& memoryID,
+ const std::string& errorMessage) :
ArMemError(makeMsg(verb, memoryID, errorMessage))
{
-
}
- std::string ServerRegistrationOrRemovalFailed::makeMsg(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage)
+ std::string
+ ServerRegistrationOrRemovalFailed::makeMsg(const std::string& verb,
+ const MemoryID& memoryID,
+ const std::string& errorMessage)
{
std::stringstream ss;
- ss << "Failed to " << verb << " memory server for '" << memoryID << "' in the Memory Name System (MNS).";
+ ss << "Failed to " << verb << " memory server for '" << memoryID
+ << "' in the Memory Name System (MNS).";
if (not errorMessage.empty())
{
ss << "\n" << errorMessage;
@@ -61,4 +68,4 @@ namespace armarx::armem::error
return ss.str();
}
-}
+} // namespace armarx::armem::error
diff --git a/source/RobotAPI/libraries/armem/core/error/mns.h b/source/RobotAPI/libraries/armem/core/error/mns.h
index 428df8cae66494f72b1a56267a97b3942f63f7a6..3edb5250c20b1d620c673981dfacc2bef45103aa 100644
--- a/source/RobotAPI/libraries/armem/core/error/mns.h
+++ b/source/RobotAPI/libraries/armem/core/error/mns.h
@@ -2,7 +2,6 @@
#include "ArMemError.h"
-
namespace armarx::armem::error
{
@@ -12,41 +11,38 @@ namespace armarx::armem::error
class MemoryNameSystemQueryFailed : public ArMemError
{
public:
+ MemoryNameSystemQueryFailed(const std::string& function,
+ const std::string& errorMessage = "");
- MemoryNameSystemQueryFailed(const std::string& function, const std::string& errorMessage = "");
-
- static std::string makeMsg(const std::string& function, const std::string& errorMessage = "");
-
+ static std::string makeMsg(const std::string& function,
+ const std::string& errorMessage = "");
};
-
/**
* @brief Indicates that a query to the Memory Name System failed.
*/
class CouldNotResolveMemoryServer : public ArMemError
{
public:
-
CouldNotResolveMemoryServer(const MemoryID& memoryID, const std::string& errorMessage = "");
static std::string makeMsg(const MemoryID& memoryID, const std::string& errorMessage = "");
-
};
-
/**
* @brief Indicates that a query to the Memory Name System failed.
*/
class ServerRegistrationOrRemovalFailed : public ArMemError
{
public:
+ ServerRegistrationOrRemovalFailed(const std::string& verb,
+ const MemoryID& memoryID,
+ const std::string& errorMessage = "");
- ServerRegistrationOrRemovalFailed(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage = "");
-
- static std::string makeMsg(const std::string& verb, const MemoryID& memoryID, const std::string& errorMessage = "");
-
+ static std::string makeMsg(const std::string& verb,
+ const MemoryID& memoryID,
+ const std::string& errorMessage = "");
};
-}
-
+} // namespace armarx::armem::error
diff --git a/source/RobotAPI/libraries/armem/core/forward_declarations.h b/source/RobotAPI/libraries/armem/core/forward_declarations.h
index 283988be5244c8b9f49af8ea474faa6713439183..3c5de916df1e58028720dfd4551e484c8d5b9743 100644
--- a/source/RobotAPI/libraries/armem/core/forward_declarations.h
+++ b/source/RobotAPI/libraries/armem/core/forward_declarations.h
@@ -2,10 +2,10 @@
#include <ArmarXCore/core/time/forward_declarations.h>
-
namespace IceInternal
{
- template<typename T> class Handle;
+ template <typename T>
+ class Handle;
}
namespace armarx::armem
@@ -18,12 +18,13 @@ namespace armarx::armem
class EntityUpdate;
class CommitResult;
class EntityUpdateResult;
-}
+} // namespace armarx::armem
+
namespace armarx::armem::dto
{
using Time = armarx::core::time::dto::DateTime;
using Duration = armarx::core::time::dto::Duration;
-}
+} // namespace armarx::armem::dto
namespace armarx::armem::arondto
{
@@ -47,7 +48,7 @@ namespace armarx::armem::base
class CoreSegmentBase;
template <class _CoreSegmentT, class _Derived>
class MemoryBase;
-}
+} // namespace armarx::armem::base
namespace armarx::armem::wm
{
@@ -57,7 +58,7 @@ namespace armarx::armem::wm
class ProviderSegment;
class CoreSegment;
class Memory;
-}
+} // namespace armarx::armem::wm
namespace armarx::armem::server::wm
{
@@ -67,7 +68,7 @@ namespace armarx::armem::server::wm
class ProviderSegment;
class CoreSegment;
class Memory;
-}
+} // namespace armarx::armem::server::wm
namespace armarx::armem::data
{
@@ -76,7 +77,7 @@ namespace armarx::armem::data
struct CommitResult;
struct EntityUpdate;
struct EntityUpdateResult;
-}
+} // namespace armarx::armem::data
namespace armarx::armem::actions
{
@@ -87,4 +88,4 @@ namespace armarx::armem::actions::data
{
class Menu;
using MenuPtr = ::IceInternal::Handle<Menu>;
-}
+} // namespace armarx::armem::actions::data
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
index 5e1ace16263e1bcdd11a760e8aba7c5216afc36d..04cc2464e34529b91edc7ce586da83cafb8b9010 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
@@ -1,24 +1,23 @@
#include "ice_conversions.h"
-#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
-
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <RobotAPI/interface/armem/actions.h>
#include <RobotAPI/interface/armem/commit.h>
#include <RobotAPI/interface/armem/memory.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include "actions.h"
#include "Commit.h"
#include "MemoryID.h"
#include "Time.h"
-
+#include "actions.h"
namespace armarx
{
- void armem::toIce(data::MemoryID& ice, const MemoryID& id)
+ void
+ armem::toIce(data::MemoryID& ice, const MemoryID& id)
{
ice.memoryName = id.memoryName;
ice.coreSegmentName = id.coreSegmentName;
@@ -28,7 +27,8 @@ namespace armarx
ice.instanceIndex = id.instanceIndex;
}
- void armem::fromIce(const data::MemoryID& ice, MemoryID& id)
+ void
+ armem::fromIce(const data::MemoryID& ice, MemoryID& id)
{
id.memoryName = ice.memoryName;
id.coreSegmentName = ice.coreSegmentName;
@@ -38,7 +38,8 @@ namespace armarx
id.instanceIndex = ice.instanceIndex;
}
- void armem::fromIce(const data::Commit& ice, Commit& commit)
+ void
+ armem::fromIce(const data::Commit& ice, Commit& commit)
{
commit.updates.clear();
for (const auto& ice_update : ice.updates)
@@ -48,7 +49,8 @@ namespace armarx
}
}
- void armem::toIce(data::Commit& ice, const Commit& commit)
+ void
+ armem::toIce(data::Commit& ice, const Commit& commit)
{
ice.updates.clear();
for (const auto& update : commit.updates)
@@ -58,7 +60,8 @@ namespace armarx
}
}
- void armem::fromIce(const data::CommitResult& ice, CommitResult& result)
+ void
+ armem::fromIce(const data::CommitResult& ice, CommitResult& result)
{
result.results.clear();
for (const auto& ice_res : ice.results)
@@ -68,7 +71,8 @@ namespace armarx
}
}
- void armem::toIce(data::CommitResult& ice, const CommitResult& result)
+ void
+ armem::toIce(data::CommitResult& ice, const CommitResult& result)
{
ice.results.clear();
for (const auto& res : result.results)
@@ -78,13 +82,16 @@ namespace armarx
}
}
- void armem::fromIce(const data::EntityUpdate& ice, EntityUpdate& update)
+ void
+ armem::fromIce(const data::EntityUpdate& ice, EntityUpdate& update)
{
fromIce(ice.entityID, update.entityID);
update.instancesData.clear();
update.instancesData.reserve(ice.instancesData.size());
- std::transform(ice.instancesData.begin(), ice.instancesData.end(), std::back_inserter(update.instancesData),
+ std::transform(ice.instancesData.begin(),
+ ice.instancesData.end(),
+ std::back_inserter(update.instancesData),
aron::data::Dict::FromAronDictDTO);
fromIce(ice.referencedTime, update.referencedTime);
@@ -93,13 +100,16 @@ namespace armarx
fromIce(ice.timeSent, update.sentTime);
}
- void armem::toIce(data::EntityUpdate& ice, const EntityUpdate& update)
+ void
+ armem::toIce(data::EntityUpdate& ice, const EntityUpdate& update)
{
toIce(ice.entityID, update.entityID);
ice.instancesData.clear();
ice.instancesData.reserve(update.instancesData.size());
- std::transform(update.instancesData.begin(), update.instancesData.end(), std::back_inserter(ice.instancesData),
+ std::transform(update.instancesData.begin(),
+ update.instancesData.end(),
+ std::back_inserter(ice.instancesData),
aron::data::Dict::ToAronDictDTO);
toIce(ice.referencedTime, update.referencedTime);
@@ -108,7 +118,8 @@ namespace armarx
toIce(ice.timeSent, update.sentTime);
}
- void armem::fromIce(const data::EntityUpdateResult& ice, EntityUpdateResult& result)
+ void
+ armem::fromIce(const data::EntityUpdateResult& ice, EntityUpdateResult& result)
{
result.success = ice.success;
fromIce(ice.snapshotID, result.snapshotID);
@@ -116,7 +127,8 @@ namespace armarx
result.errorMessage = ice.errorMessage;
}
- void armem::toIce(data::EntityUpdateResult& ice, const EntityUpdateResult& result)
+ void
+ armem::toIce(data::EntityUpdateResult& ice, const EntityUpdateResult& result)
{
ice.success = result.success;
toIce(ice.snapshotID, result.snapshotID);
@@ -124,7 +136,8 @@ namespace armarx
ice.errorMessage = result.errorMessage;
}
- void armem::fromIce(const data::Commit& ice, Commit& commit, Time timeArrived)
+ void
+ armem::fromIce(const data::Commit& ice, Commit& commit, Time timeArrived)
{
commit.updates.clear();
for (const auto& ice_update : ice.updates)
@@ -134,20 +147,22 @@ namespace armarx
}
}
- void armem::fromIce(const data::EntityUpdate& ice, EntityUpdate& update, Time timeArrived)
+ void
+ armem::fromIce(const data::EntityUpdate& ice, EntityUpdate& update, Time timeArrived)
{
fromIce(ice, update);
update.arrivedTime = timeArrived;
}
- void armem::fromIce(const actions::data::MenuPtr& ice, actions::Menu& menu)
+ void
+ armem::fromIce(const actions::data::MenuPtr& ice, actions::Menu& menu)
{
menu = actions::Menu::fromIce(ice);
}
- void armem::toIce(actions::data::MenuPtr& ice, const actions::Menu& menu)
+ void
+ armem::toIce(actions::data::MenuPtr& ice, const actions::Menu& menu)
{
ice = menu.toIce();
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.h b/source/RobotAPI/libraries/armem/core/ice_conversions.h
index b88157c688bcaeefd301b1764bf6a78d33badf71..23fd06084b89882b99860290bce336216ff0ec9a 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.h
@@ -2,7 +2,6 @@
#include "forward_declarations.h"
-
namespace armarx::armem
{
@@ -31,5 +30,4 @@ namespace armarx::armem
void fromIce(const actions::data::MenuPtr& ice, actions::Menu& menu);
void toIce(actions::data::MenuPtr& ice, const actions::Menu& menu);
-}
-
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions_boost_templates.h b/source/RobotAPI/libraries/armem/core/ice_conversions_boost_templates.h
index 7b6681236bc3d0517d6fd61656a1dc598a8d6a36..db963123a5e888d6848c685477be5c81a98f3219 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions_boost_templates.h
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions_boost_templates.h
@@ -4,14 +4,14 @@
#include <boost/container/flat_map.hpp>
-
namespace armarx::armem
{
// std::map <-> boost::container::flat_map
template <class IceKeyT, class IceValueT, class CppKeyT, class CppValueT>
- void toIce(std::map<IceKeyT, IceValueT>& iceMap,
- const boost::container::flat_map<CppKeyT, CppValueT>& cppMap)
+ void
+ toIce(std::map<IceKeyT, IceValueT>& iceMap,
+ const boost::container::flat_map<CppKeyT, CppValueT>& cppMap)
{
iceMap.clear();
for (const auto& [key, value] : cppMap)
@@ -21,8 +21,9 @@ namespace armarx::armem
}
template <class IceKeyT, class IceValueT, class CppKeyT, class CppValueT>
- void fromIce(const std::map<IceKeyT, IceValueT>& iceMap,
- boost::container::flat_map<CppKeyT, CppValueT>& cppMap)
+ void
+ fromIce(const std::map<IceKeyT, IceValueT>& iceMap,
+ boost::container::flat_map<CppKeyT, CppValueT>& cppMap)
{
cppMap.clear();
for (const auto& [key, value] : iceMap)
@@ -31,4 +32,4 @@ namespace armarx::armem
}
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h b/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h
index d5bd8f32a4b6bfc9f57e35fc22fd9a79d4bdae2e..3a62a816fa72dd4b99fab3d3b6aaef00cfac5112 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions_templates.h
@@ -2,39 +2,43 @@
#include <ArmarXCore/core/ice_conversions.h>
-#define DEPRECATION_TO_ICE "This function is deprecated. Use armarx::toIce() from <ArmarXCore/core/ice_conversions.h> instead."
-#define DEPRECATION_FROM_ICE "This function is deprecated. Use armarx::fromIce() from <ArmarXCore/core/ice_conversions.h> instead."
-
+#define DEPRECATION_TO_ICE \
+ "This function is deprecated. Use armarx::toIce() from <ArmarXCore/core/ice_conversions.h> " \
+ "instead."
+#define DEPRECATION_FROM_ICE \
+ "This function is deprecated. Use armarx::fromIce() from <ArmarXCore/core/ice_conversions.h> " \
+ "instead."
namespace armarx::armem
{
// Same type
template <class T>
- [[deprecated(DEPRECATION_TO_ICE)]]
- void toIce(T& ice, const T& cpp)
+ [[deprecated(DEPRECATION_TO_ICE)]] void
+ toIce(T& ice, const T& cpp)
{
ice = cpp;
}
+
template <class T>
- [[deprecated(DEPRECATION_FROM_ICE)]]
- void fromIce(const T& ice, T& cpp)
+ [[deprecated(DEPRECATION_FROM_ICE)]] void
+ fromIce(const T& ice, T& cpp)
{
cpp = ice;
}
-
// Ice Handle
template <class IceT, class CppT>
- [[deprecated(DEPRECATION_TO_ICE)]]
- void toIce(::IceInternal::Handle<IceT>& ice, const CppT& cpp)
+ [[deprecated(DEPRECATION_TO_ICE)]] void
+ toIce(::IceInternal::Handle<IceT>& ice, const CppT& cpp)
{
ice = new IceT();
toIce(*ice, cpp);
}
+
template <class IceT, class CppT>
- [[deprecated(DEPRECATION_FROM_ICE)]]
- void fromIce(const ::IceInternal::Handle<IceT>& ice, CppT& cpp)
+ [[deprecated(DEPRECATION_FROM_ICE)]] void
+ fromIce(const ::IceInternal::Handle<IceT>& ice, CppT& cpp)
{
if (ice)
{
@@ -42,19 +46,19 @@ namespace armarx::armem
}
}
-
// General return version
template <class IceT, class CppT>
- [[deprecated(DEPRECATION_TO_ICE)]]
- IceT toIce(const CppT& cpp)
+ [[deprecated(DEPRECATION_TO_ICE)]] IceT
+ toIce(const CppT& cpp)
{
IceT ice;
toIce(ice, cpp);
return ice;
}
+
template <class CppT, class IceT>
- [[deprecated(DEPRECATION_FROM_ICE)]]
- CppT fromIce(const IceT& ice)
+ [[deprecated(DEPRECATION_FROM_ICE)]] CppT
+ fromIce(const IceT& ice)
{
CppT cpp;
fromIce(ice, cpp);
@@ -64,28 +68,28 @@ namespace armarx::armem
// std::unique_ptr
template <class IceT, class CppT>
- [[deprecated(DEPRECATION_TO_ICE)]]
- void toIce(IceT& ice, const std::unique_ptr<CppT>& cppPointer)
+ [[deprecated(DEPRECATION_TO_ICE)]] void
+ toIce(IceT& ice, const std::unique_ptr<CppT>& cppPointer)
{
if (cppPointer)
{
toIce(ice, *cppPointer);
}
}
+
template <class IceT, class CppT>
- [[deprecated(DEPRECATION_FROM_ICE)]]
- void fromIce(const IceT& ice, std::unique_ptr<CppT>& cppPointer)
+ [[deprecated(DEPRECATION_FROM_ICE)]] void
+ fromIce(const IceT& ice, std::unique_ptr<CppT>& cppPointer)
{
cppPointer = std::make_unique<CppT>();
fromIce(ice, *cppPointer);
}
-
// Ice Handle <-> std::unique_ptr
template <class IceT, class CppT>
- [[deprecated(DEPRECATION_TO_ICE)]]
- void toIce(::IceInternal::Handle<IceT>& ice, const std::unique_ptr<CppT>& cppPointer)
+ [[deprecated(DEPRECATION_TO_ICE)]] void
+ toIce(::IceInternal::Handle<IceT>& ice, const std::unique_ptr<CppT>& cppPointer)
{
if (cppPointer)
{
@@ -97,9 +101,10 @@ namespace armarx::armem
ice = nullptr;
}
}
+
template <class IceT, class CppT>
- [[deprecated(DEPRECATION_FROM_ICE)]]
- void fromIce(const ::IceInternal::Handle<IceT>& ice, std::unique_ptr<CppT>& cppPointer)
+ [[deprecated(DEPRECATION_FROM_ICE)]] void
+ fromIce(const ::IceInternal::Handle<IceT>& ice, std::unique_ptr<CppT>& cppPointer)
{
if (ice)
{
@@ -112,12 +117,11 @@ namespace armarx::armem
}
}
-
// std::vector
template <class IceT, class CppT>
- [[deprecated(DEPRECATION_TO_ICE)]]
- void toIce(std::vector<IceT>& ices, const std::vector<CppT>& cpps)
+ [[deprecated(DEPRECATION_TO_ICE)]] void
+ toIce(std::vector<IceT>& ices, const std::vector<CppT>& cpps)
{
ices.clear();
ices.reserve(cpps.size());
@@ -126,9 +130,10 @@ namespace armarx::armem
toIce(ices.emplace_back(), cpp);
}
}
+
template <class IceT, class CppT>
- [[deprecated(DEPRECATION_FROM_ICE)]]
- void fromIce(const std::vector<IceT>& ices, std::vector<CppT>& cpps)
+ [[deprecated(DEPRECATION_FROM_ICE)]] void
+ fromIce(const std::vector<IceT>& ices, std::vector<CppT>& cpps)
{
cpps.clear();
cpps.reserve(ices.size());
@@ -139,21 +144,19 @@ namespace armarx::armem
}
template <class IceT, class CppT>
- [[deprecated(DEPRECATION_TO_ICE)]]
- std::vector<IceT> toIce(const std::vector<CppT>& cpps)
+ [[deprecated(DEPRECATION_TO_ICE)]] std::vector<IceT>
+ toIce(const std::vector<CppT>& cpps)
{
std::vector<IceT> ices;
toIce(ices, cpps);
return ices;
}
-
// std::map
template <class IceKeyT, class IceValueT, class CppKeyT, class CppValueT>
- [[deprecated(DEPRECATION_TO_ICE)]]
- void toIce(std::map<IceKeyT, IceValueT>& iceMap,
- const std::map<CppKeyT, CppValueT>& cppMap)
+ [[deprecated(DEPRECATION_TO_ICE)]] void
+ toIce(std::map<IceKeyT, IceValueT>& iceMap, const std::map<CppKeyT, CppValueT>& cppMap)
{
iceMap.clear();
for (const auto& [key, value] : cppMap)
@@ -161,10 +164,10 @@ namespace armarx::armem
iceMap.emplace(toIce<IceKeyT>(key), toIce<IceValueT>(value));
}
}
+
template <class IceKeyT, class IceValueT, class CppKeyT, class CppValueT>
- [[deprecated(DEPRECATION_FROM_ICE)]]
- void fromIce(const std::map<IceKeyT, IceValueT>& iceMap,
- std::map<CppKeyT, CppValueT>& cppMap)
+ [[deprecated(DEPRECATION_FROM_ICE)]] void
+ fromIce(const std::map<IceKeyT, IceValueT>& iceMap, std::map<CppKeyT, CppValueT>& cppMap)
{
cppMap.clear();
for (const auto& [key, value] : iceMap)
@@ -174,11 +177,11 @@ namespace armarx::armem
}
template <class IceKeyT, class IceValueT, class CppKeyT, class CppValueT>
- [[deprecated(DEPRECATION_TO_ICE)]]
- std::map<IceKeyT, IceValueT> toIce(const std::map<CppKeyT, CppValueT>& cppMap)
+ [[deprecated(DEPRECATION_TO_ICE)]] std::map<IceKeyT, IceValueT>
+ toIce(const std::map<CppKeyT, CppValueT>& cppMap)
{
std::map<IceKeyT, IceValueT> iceMap;
toIce(iceMap, cppMap);
return iceMap;
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/json_conversions.cpp b/source/RobotAPI/libraries/armem/core/json_conversions.cpp
index 2a6f2a1f6cbff1d7d348d92f9ea7b482b6c25b3b..54d19cc328d33223d3f1123504bf5d508abdf3ff 100644
--- a/source/RobotAPI/libraries/armem/core/json_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/json_conversions.cpp
@@ -2,8 +2,8 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
-void armarx::armem::to_json(nlohmann::json& j, const MemoryID& bo)
+void
+armarx::armem::to_json(nlohmann::json& j, const MemoryID& bo)
{
j["memoryName"] = bo.memoryName;
j["coreSegmentName"] = bo.coreSegmentName;
@@ -14,7 +14,8 @@ void armarx::armem::to_json(nlohmann::json& j, const MemoryID& bo)
j["instanceIndex"] = bo.instanceIndex;
}
-void armarx::armem::from_json(const nlohmann::json& j, MemoryID& bo)
+void
+armarx::armem::from_json(const nlohmann::json& j, MemoryID& bo)
{
j.at("memoryName").get_to(bo.memoryName);
j.at("coreSegmentName").get_to(bo.coreSegmentName);
diff --git a/source/RobotAPI/libraries/armem/core/json_conversions.h b/source/RobotAPI/libraries/armem/core/json_conversions.h
index 86e508c228fe57063216c2f1c6fa7710066992bc..b2db75a53c40baca2640847ecbbf9d664affeb31 100644
--- a/source/RobotAPI/libraries/armem/core/json_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/json_conversions.h
@@ -2,11 +2,10 @@
#include <SimoxUtility/json/json.hpp>
-
namespace armarx::armem
{
class MemoryID;
void to_json(nlohmann::json& j, const MemoryID& bo);
void from_json(const nlohmann::json& j, MemoryID& bo);
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/operations.cpp b/source/RobotAPI/libraries/armem/core/operations.cpp
index 5ac621a9ec9bd0e1ca9bf3ef6a6ca0e16ceb5d49..79f35b8817105786c918ad74e561cfb936f2966c 100644
--- a/source/RobotAPI/libraries/armem/core/operations.cpp
+++ b/source/RobotAPI/libraries/armem/core/operations.cpp
@@ -1,9 +1,8 @@
#include "operations.h"
-#include <RobotAPI/libraries/armem/core/Time.h>
-
#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
namespace armarx
{
@@ -12,16 +11,17 @@ namespace armarx
armem::getAronData(const wm::EntitySnapshot& snapshot)
{
std::vector<aron::data::DictPtr> result;
- snapshot.forEachChild([&result](const wm::EntityInstance & instance)
- {
- result.push_back(instance.data());
- return true;
- });
+ snapshot.forEachChild(
+ [&result](const wm::EntityInstance& instance)
+ {
+ result.push_back(instance.data());
+ return true;
+ });
return result;
}
-
- armem::EntityUpdate armem::toEntityUpdate(const wm::EntitySnapshot& snapshot)
+ armem::EntityUpdate
+ armem::toEntityUpdate(const wm::EntitySnapshot& snapshot)
{
EntityUpdate up;
up.entityID = snapshot.id().getEntityID();
@@ -30,9 +30,9 @@ namespace armarx
return up;
}
-
template <class DataT>
- static std::string makeLine(int depth, const DataT& data, std::optional<size_t> size = std::nullopt)
+ static std::string
+ makeLine(int depth, const DataT& data, std::optional<size_t> size = std::nullopt)
{
std::stringstream ss;
while (depth > 1)
@@ -54,48 +54,56 @@ namespace armarx
return ss.str();
}
-
template <class DataT>
- static std::string _print(const DataT& data, int maxDepth, int depth)
+ static std::string
+ _print(const DataT& data, int maxDepth, int depth)
{
std::stringstream ss;
ss << makeLine(depth, data, data.size());
if (maxDepth < 0 || maxDepth > 0)
{
data.forEachChild([&ss, maxDepth, depth](const auto& instance)
- {
- ss << armem::print(instance, maxDepth - 1, depth + 1);
- });
+ { ss << armem::print(instance, maxDepth - 1, depth + 1); });
}
return ss.str();
}
- std::string armem::print(const wm::Memory& data, int maxDepth, int depth)
+ std::string
+ armem::print(const wm::Memory& data, int maxDepth, int depth)
{
return _print(data, maxDepth, depth);
}
- std::string armem::print(const wm::CoreSegment& data, int maxDepth, int depth)
+
+ std::string
+ armem::print(const wm::CoreSegment& data, int maxDepth, int depth)
{
return _print(data, maxDepth, depth);
}
- std::string armem::print(const wm::ProviderSegment& data, int maxDepth, int depth)
+
+ std::string
+ armem::print(const wm::ProviderSegment& data, int maxDepth, int depth)
{
return _print(data, maxDepth, depth);
}
- std::string armem::print(const wm::Entity& data, int maxDepth, int depth)
+
+ std::string
+ armem::print(const wm::Entity& data, int maxDepth, int depth)
{
return _print(data, maxDepth, depth);
}
- std::string armem::print(const wm::EntitySnapshot& data, int maxDepth, int depth)
+
+ std::string
+ armem::print(const wm::EntitySnapshot& data, int maxDepth, int depth)
{
return _print(data, maxDepth, depth);
}
- std::string armem::print(const wm::EntityInstance& data, int maxDepth, int depth)
+ std::string
+ armem::print(const wm::EntityInstance& data, int maxDepth, int depth)
{
- (void) maxDepth;
+ (void)maxDepth;
std::stringstream ss;
ss << makeLine(depth, data);
return ss.str();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem/core/operations.h b/source/RobotAPI/libraries/armem/core/operations.h
index 6f7ae4d48ffe65665affe83a9356858c57e29044..4df6664496d2d624b37b20f1acd605bddb0f1373 100644
--- a/source/RobotAPI/libraries/armem/core/operations.h
+++ b/source/RobotAPI/libraries/armem/core/operations.h
@@ -1,9 +1,8 @@
#pragma once
-#include "wm/memory_definitions.h"
-
#include <string>
+#include "wm/memory_definitions.h"
/*
* Functions performing some "advanced" operations on the memory
@@ -14,56 +13,51 @@
namespace armarx::armem
{
- std::vector<aron::data::DictPtr>
- getAronData(const wm::EntitySnapshot& snapshot);
-
+ std::vector<aron::data::DictPtr> getAronData(const wm::EntitySnapshot& snapshot);
- EntityUpdate
- toEntityUpdate(const wm::EntitySnapshot& snapshot);
+ EntityUpdate toEntityUpdate(const wm::EntitySnapshot& snapshot);
template <class ContainerT>
Commit
toCommit(const ContainerT& container)
{
Commit commit;
- container.forEachSnapshot([&commit](const wm::EntitySnapshot & snapshot)
- {
- commit.add(toEntityUpdate(snapshot));
- return true;
- });
+ container.forEachSnapshot(
+ [&commit](const wm::EntitySnapshot& snapshot)
+ {
+ commit.add(toEntityUpdate(snapshot));
+ return true;
+ });
return commit;
}
-
template <class ContainerT>
const typename ContainerT::EntityInstanceT*
findFirstInstance(const ContainerT& container)
{
const typename ContainerT::EntityInstanceT* instance = nullptr;
- container.forEachInstance([&instance](const wm::EntityInstance & i)
- {
- instance = &i;
- return false;
- });
+ container.forEachInstance(
+ [&instance](const wm::EntityInstance& i)
+ {
+ instance = &i;
+ return false;
+ });
return instance;
}
-
template <class ContainerT>
- std::vector<MemoryID> getEntityIDs(const ContainerT& container)
+ std::vector<MemoryID>
+ getEntityIDs(const ContainerT& container)
{
std::vector<armem::MemoryID> entityIDs;
container.forEachEntity([&entityIDs](const auto& entity)
- {
- entityIDs.push_back(entity.id());
- });
+ { entityIDs.push_back(entity.id()); });
return entityIDs;
}
-
std::string print(const wm::Memory& data, int maxDepth = -1, int depth = 0);
std::string print(const wm::CoreSegment& data, int maxDepth = -1, int depth = 0);
std::string print(const wm::ProviderSegment& data, int maxDepth = -1, int depth = 0);
@@ -71,5 +65,4 @@ namespace armarx::armem
std::string print(const wm::EntitySnapshot& data, int maxDepth = -1, int depth = 0);
std::string print(const wm::EntityInstance& data, int maxDepth = -1, int depth = 0);
-}
-
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/query.h b/source/RobotAPI/libraries/armem/core/query.h
index 975361744ba134477cdd4600b10ba6c4a8bacf62..68b5d69914215acae3d03d7a7b18d3e71554d879 100644
--- a/source/RobotAPI/libraries/armem/core/query.h
+++ b/source/RobotAPI/libraries/armem/core/query.h
@@ -3,10 +3,8 @@
#include "query/DataMode.h"
#include "query/QueryTarget.h"
-
namespace armarx::armem::query
{
}
-
diff --git a/source/RobotAPI/libraries/armem/core/query/DataMode.cpp b/source/RobotAPI/libraries/armem/core/query/DataMode.cpp
index 07877d2dcbf2333e08bf5233146c3f3481d166f2..8997d300f753d5f92b20310aad7cf08f0c0efdc5 100644
--- a/source/RobotAPI/libraries/armem/core/query/DataMode.cpp
+++ b/source/RobotAPI/libraries/armem/core/query/DataMode.cpp
@@ -2,8 +2,9 @@
namespace armarx::armem::query
{
- DataMode boolToDataMode(bool withData)
+ DataMode
+ boolToDataMode(bool withData)
{
return withData ? DataMode::WithData : DataMode::NoData;
}
-}
+} // namespace armarx::armem::query
diff --git a/source/RobotAPI/libraries/armem/core/query/DataMode.h b/source/RobotAPI/libraries/armem/core/query/DataMode.h
index 0bcd7be091aeb5766a248400bd0767f8e36f95b7..01020fd963d37127c6bf6ba0dcd22f09e84f2e3f 100644
--- a/source/RobotAPI/libraries/armem/core/query/DataMode.h
+++ b/source/RobotAPI/libraries/armem/core/query/DataMode.h
@@ -1,14 +1,13 @@
#pragma once
-
namespace armarx::armem::query
{
enum class DataMode
{
- NoData, ///< Just get the structure, but no ARON data.
- WithData, ///< Get structure and ARON data.
+ NoData, ///< Just get the structure, but no ARON data.
+ WithData, ///< Get structure and ARON data.
};
DataMode boolToDataMode(bool withData);
-}
+} // namespace armarx::armem::query
diff --git a/source/RobotAPI/libraries/armem/core/query/QueryTarget.cpp b/source/RobotAPI/libraries/armem/core/query/QueryTarget.cpp
index e66a41907a55ff0e91e111cc60a60d157f1e4182..1bae7fb6d8aae141d2cf300d4af5622c2294bdb4 100644
--- a/source/RobotAPI/libraries/armem/core/query/QueryTarget.cpp
+++ b/source/RobotAPI/libraries/armem/core/query/QueryTarget.cpp
@@ -2,20 +2,31 @@
namespace armarx::armem::query
{
- void toIce(armem::query::data::QueryTarget::QueryTargetEnum& ice, const QueryTarget bo)
+ void
+ toIce(armem::query::data::QueryTarget::QueryTargetEnum& ice, const QueryTarget bo)
{
- switch(bo)
+ switch (bo)
{
- case QueryTarget::WM: ice = armem::query::data::QueryTarget::WM; break;
- case QueryTarget::WM_LTM: ice = armem::query::data::QueryTarget::WM_LTM; break;
+ case QueryTarget::WM:
+ ice = armem::query::data::QueryTarget::WM;
+ break;
+ case QueryTarget::WM_LTM:
+ ice = armem::query::data::QueryTarget::WM_LTM;
+ break;
}
}
- void fromIce(const armem::query::data::QueryTarget::QueryTargetEnum ice, QueryTarget& bo)
+
+ void
+ fromIce(const armem::query::data::QueryTarget::QueryTargetEnum ice, QueryTarget& bo)
{
- switch(ice)
+ switch (ice)
{
- case armem::query::data::QueryTarget::WM: bo = QueryTarget::WM; break;
- case armem::query::data::QueryTarget::WM_LTM: bo = QueryTarget::WM_LTM; break;
+ case armem::query::data::QueryTarget::WM:
+ bo = QueryTarget::WM;
+ break;
+ case armem::query::data::QueryTarget::WM_LTM:
+ bo = QueryTarget::WM_LTM;
+ break;
}
}
-}
+} // namespace armarx::armem::query
diff --git a/source/RobotAPI/libraries/armem/core/query/QueryTarget.h b/source/RobotAPI/libraries/armem/core/query/QueryTarget.h
index 304157213e22296358b857b225a1421ed44095b1..143d9523807151a057aa28b9b456881dad6a8699 100644
--- a/source/RobotAPI/libraries/armem/core/query/QueryTarget.h
+++ b/source/RobotAPI/libraries/armem/core/query/QueryTarget.h
@@ -12,4 +12,4 @@ namespace armarx::armem::query
};
void toIce(armem::query::data::QueryTarget::QueryTargetEnum& ice, const QueryTarget bo);
void fromIce(const armem::query::data::QueryTarget::QueryTargetEnum ice, QueryTarget& bo);
-}
+} // namespace armarx::armem::query
diff --git a/source/RobotAPI/libraries/armem/core/wm.h b/source/RobotAPI/libraries/armem/core/wm.h
index 83776ed701261bde89ed1fe2fd1e4ff479f06f44..91f6df98345e2b86f7ddf33226b8da811f5d0de8 100644
--- a/source/RobotAPI/libraries/armem/core/wm.h
+++ b/source/RobotAPI/libraries/armem/core/wm.h
@@ -2,9 +2,7 @@
#include "wm/memory_definitions.h"
-
namespace armarx::armem::wm
{
}
-
diff --git a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
index e2ceb0f2ac5e77f23d0a8a330b9b7837699691fa..4fc114b77b8f47f6c9c21c5301e7de4f32d1e74e 100644
--- a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.cpp
@@ -2,18 +2,19 @@
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-
namespace armarx::armem
{
- constexpr const char* DATA_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
+ constexpr const char* DATA_WRAPPER_TIME_STORED_FIELD = "__WRITER_METADATA__TIME_STORED";
constexpr const char* DATA_WRAPPER_TIME_REFERENCED_FIELD = "__ENTITY_METADATA__TIME_REFERENCED";
- constexpr const char* DATA_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
- constexpr const char* DATA_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
- constexpr const char* DATA_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
-}
-
-
-void armarx::armem::from_aron(const aron::data::DictPtr& metadata, const aron::data::DictPtr& data, wm::EntityInstance& e)
+ constexpr const char* DATA_WRAPPER_TIME_SENT_FIELD = "__ENTITY_METADATA__TIME_SENT";
+ constexpr const char* DATA_WRAPPER_TIME_ARRIVED_FIELD = "__ENTITY_METADATA__TIME_ARRIVED";
+ constexpr const char* DATA_WRAPPER_CONFIDENCE_FIELD = "__ENTITY_METADATA__CONFIDENCE";
+} // namespace armarx::armem
+
+void
+armarx::armem::from_aron(const aron::data::DictPtr& metadata,
+ const aron::data::DictPtr& data,
+ wm::EntityInstance& e)
{
ARMARX_CHECK_NOT_NULL(metadata);
// Todo: What if data is null?
@@ -22,43 +23,54 @@ void armarx::armem::from_aron(const aron::data::DictPtr& metadata, const aron::d
e.data() = data;
- auto referencedTime = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_REFERENCED_FIELD));
+ auto referencedTime = aron::data::Long::DynamicCastAndCheck(
+ metadata->getElement(DATA_WRAPPER_TIME_REFERENCED_FIELD));
m.referencedTime = Time(Duration::MicroSeconds(referencedTime->toAronLongDTO()->value));
- auto timeSent = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
+ auto timeSent =
+ aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_SENT_FIELD));
m.sentTime = Time(Duration::MicroSeconds(timeSent->toAronLongDTO()->value));
- auto timeArrived = aron::data::Long::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
+ auto timeArrived = aron::data::Long::DynamicCastAndCheck(
+ metadata->getElement(DATA_WRAPPER_TIME_ARRIVED_FIELD));
m.arrivedTime = Time(Duration::MicroSeconds(timeArrived->toAronLongDTO()->value));
- auto confidence = aron::data::Double::DynamicCastAndCheck(metadata->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
+ auto confidence = aron::data::Double::DynamicCastAndCheck(
+ metadata->getElement(DATA_WRAPPER_CONFIDENCE_FIELD));
m.confidence = static_cast<float>(confidence->toAronDoubleDTO()->value);
}
-
-void armarx::armem::to_aron(aron::data::DictPtr& metadata, aron::data::DictPtr& data, const wm::EntityInstance& e)
+void
+armarx::armem::to_aron(aron::data::DictPtr& metadata,
+ aron::data::DictPtr& data,
+ const wm::EntityInstance& e)
{
data = e.data();
metadata = std::make_shared<aron::data::Dict>();
- auto timeWrapped = std::make_shared<aron::data::Long>(armarx::aron::Path(std::vector<std::string>{DATA_WRAPPER_TIME_STORED_FIELD}));
+ auto timeWrapped = std::make_shared<aron::data::Long>(
+ armarx::aron::Path(std::vector<std::string>{DATA_WRAPPER_TIME_STORED_FIELD}));
timeWrapped->setValue(Time::Now().toMicroSecondsSinceEpoch());
metadata->addElement(DATA_WRAPPER_TIME_STORED_FIELD, timeWrapped);
const wm::EntityInstanceMetadata& m = e.metadata();
- auto referencedTime = std::make_shared<aron::data::Long>(armarx::aron::Path(std::vector<std::string>{DATA_WRAPPER_TIME_REFERENCED_FIELD}));
+ auto referencedTime = std::make_shared<aron::data::Long>(
+ armarx::aron::Path(std::vector<std::string>{DATA_WRAPPER_TIME_REFERENCED_FIELD}));
referencedTime->setValue(m.referencedTime.toMicroSecondsSinceEpoch());
metadata->addElement(DATA_WRAPPER_TIME_REFERENCED_FIELD, referencedTime);
- auto timeSent = std::make_shared<aron::data::Long>(armarx::aron::Path(std::vector<std::string>{DATA_WRAPPER_TIME_SENT_FIELD}));
+ auto timeSent = std::make_shared<aron::data::Long>(
+ armarx::aron::Path(std::vector<std::string>{DATA_WRAPPER_TIME_SENT_FIELD}));
timeSent->setValue(m.sentTime.toMicroSecondsSinceEpoch());
metadata->addElement(DATA_WRAPPER_TIME_SENT_FIELD, timeSent);
- auto timeArrived = std::make_shared<aron::data::Long>(armarx::aron::Path(std::vector<std::string>{DATA_WRAPPER_TIME_ARRIVED_FIELD}));
+ auto timeArrived = std::make_shared<aron::data::Long>(
+ armarx::aron::Path(std::vector<std::string>{DATA_WRAPPER_TIME_ARRIVED_FIELD}));
timeArrived->setValue(m.arrivedTime.toMicroSecondsSinceEpoch());
metadata->addElement(DATA_WRAPPER_TIME_ARRIVED_FIELD, timeArrived);
- auto confidence = std::make_shared<aron::data::Double>(armarx::aron::Path(std::vector<std::string>{DATA_WRAPPER_CONFIDENCE_FIELD}));
+ auto confidence = std::make_shared<aron::data::Double>(
+ armarx::aron::Path(std::vector<std::string>{DATA_WRAPPER_CONFIDENCE_FIELD}));
confidence->setValue(static_cast<double>(m.confidence));
metadata->addElement(DATA_WRAPPER_CONFIDENCE_FIELD, confidence);
}
diff --git a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.h b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.h
index d5de002ba8e74b961c8ec28f341e8a15778c0001..3fc1c12acd77dc9018c11dcaac3a35053795105e 100644
--- a/source/RobotAPI/libraries/armem/core/wm/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/wm/aron_conversions.h
@@ -1,15 +1,16 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
-
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
namespace armarx::armem
{
/// convert from metadata and data aron instance
- void from_aron(const aron::data::DictPtr& metadata, const aron::data::DictPtr& data, wm::EntityInstance&);
+ void from_aron(const aron::data::DictPtr& metadata,
+ const aron::data::DictPtr& data,
+ wm::EntityInstance&);
/// convert to metadata and aron instance
- void to_aron(aron::data::DictPtr& metadata, aron::data::DictPtr& data, const wm::EntityInstance&);
-}
+ void
+ to_aron(aron::data::DictPtr& metadata, aron::data::DictPtr& data, const wm::EntityInstance&);
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.cpp b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.cpp
index 50fd63bf64dabfedbab4e980f9450563d301e02b..2fb563477176aa40791e56380bfe7f6dcd5a5833 100644
--- a/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.cpp
@@ -1,6 +1,5 @@
#include "data_lookup_mixins.h"
-
namespace armarx::armem::base
{
diff --git a/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
index 68619e58b0f5705403d98480f6a1b369f2d606e3..df2949ab357c8fe09def6e89f4c0e5213108e1e8 100644
--- a/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
+++ b/source/RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h
@@ -1,18 +1,15 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-#include <RobotAPI/libraries/armem/core/base/detail/derived.h>
-
-#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
-
#include <optional>
+#include <RobotAPI/libraries/armem/core/base/detail/derived.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
namespace armarx::armem::wm::detail
{
using base::detail::derived;
-
template <class AronDtoT>
std::optional<AronDtoT>
getInstanceDataAs(aron::data::DictPtr data)
@@ -29,8 +26,6 @@ namespace armarx::armem::wm::detail
}
}
-
-
template <class DerivedT>
struct FindInstanceDataMixinForSnapshot
{
@@ -42,18 +37,15 @@ namespace armarx::armem::wm::detail
return instance ? instance->data() : nullptr;
}
-
template <class AronDtoT>
std::optional<AronDtoT>
findInstanceDataAs(int instanceIndex = 0) const
{
- return getInstanceDataAs<AronDtoT>(derived<DerivedT>(this).findInstanceData(instanceIndex));
+ return getInstanceDataAs<AronDtoT>(
+ derived<DerivedT>(this).findInstanceData(instanceIndex));
}
-
};
-
-
template <class DerivedT>
struct FindInstanceDataMixinForEntity
{
@@ -65,18 +57,15 @@ namespace armarx::armem::wm::detail
return instance ? instance->data() : nullptr;
}
-
template <class AronDtoT>
std::optional<AronDtoT>
findLatestInstanceDataAs(int instanceIndex = 0) const
{
- return getInstanceDataAs<AronDtoT>(derived<DerivedT>(this).findLatestInstanceData(instanceIndex));
+ return getInstanceDataAs<AronDtoT>(
+ derived<DerivedT>(this).findLatestInstanceData(instanceIndex));
}
-
};
-
-
template <class DerivedT>
struct FindInstanceDataMixin
{
@@ -84,7 +73,8 @@ namespace armarx::armem::wm::detail
aron::data::DictPtr
findLatestInstanceData(const MemoryID& entityID, int instanceIndex = 0) const
{
- const auto* instance = derived<DerivedT>(this).findLatestInstance(entityID, instanceIndex);
+ const auto* instance =
+ derived<DerivedT>(this).findLatestInstance(entityID, instanceIndex);
return instance ? instance->data() : nullptr;
}
@@ -92,9 +82,9 @@ namespace armarx::armem::wm::detail
std::optional<AronDtoT>
findLatestInstanceDataAs(const MemoryID& entityID, int instanceIndex = 0) const
{
- return getInstanceDataAs<AronDtoT>(derived<DerivedT>(this).findLatestInstanceData(entityID, instanceIndex));
+ return getInstanceDataAs<AronDtoT>(
+ derived<DerivedT>(this).findLatestInstanceData(entityID, instanceIndex));
}
-
};
-}
+} // namespace armarx::armem::wm::detail
diff --git a/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp
index 826bf6d9f2f980cd99bbd35a38a2357f585b5f79..776a0f10009a9df70fc9515c281c578fbb9007bf 100644
--- a/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.cpp
@@ -1,64 +1,81 @@
#include "ice_conversions.h"
-#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/base/ice_conversions.h>
-
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
namespace armarx::armem
{
- void wm::toIce(data::EntityInstance& ice, const EntityInstance& data)
+ void
+ wm::toIce(data::EntityInstance& ice, const EntityInstance& data)
{
base::toIce(ice, data);
}
- void wm::fromIce(const data::EntityInstance& ice, EntityInstance& data)
+
+ void
+ wm::fromIce(const data::EntityInstance& ice, EntityInstance& data)
{
base::fromIce(ice, data);
}
- void wm::toIce(data::EntitySnapshot& ice, const EntitySnapshot& snapshot)
+ void
+ wm::toIce(data::EntitySnapshot& ice, const EntitySnapshot& snapshot)
{
base::toIce(ice, snapshot);
}
- void wm::fromIce(const data::EntitySnapshot& ice, EntitySnapshot& snapshot)
+
+ void
+ wm::fromIce(const data::EntitySnapshot& ice, EntitySnapshot& snapshot)
{
base::fromIce(ice, snapshot);
}
- void wm::toIce(data::Entity& ice, const Entity& entity)
+ void
+ wm::toIce(data::Entity& ice, const Entity& entity)
{
base::toIce(ice, entity);
}
- void wm::fromIce(const data::Entity& ice, Entity& entity)
+
+ void
+ wm::fromIce(const data::Entity& ice, Entity& entity)
{
base::fromIce(ice, entity);
}
- void wm::toIce(data::ProviderSegment& ice, const ProviderSegment& providerSegment)
+ void
+ wm::toIce(data::ProviderSegment& ice, const ProviderSegment& providerSegment)
{
base::toIce(ice, providerSegment);
}
- void wm::fromIce(const data::ProviderSegment& ice, ProviderSegment& providerSegment)
+
+ void
+ wm::fromIce(const data::ProviderSegment& ice, ProviderSegment& providerSegment)
{
base::fromIce(ice, providerSegment);
}
- void wm::toIce(data::CoreSegment& ice, const CoreSegment& coreSegment)
+ void
+ wm::toIce(data::CoreSegment& ice, const CoreSegment& coreSegment)
{
base::toIce(ice, coreSegment);
}
- void wm::fromIce(const data::CoreSegment& ice, CoreSegment& coreSegment)
+
+ void
+ wm::fromIce(const data::CoreSegment& ice, CoreSegment& coreSegment)
{
base::fromIce(ice, coreSegment);
}
- void wm::toIce(data::Memory& ice, const Memory& memory)
+ void
+ wm::toIce(data::Memory& ice, const Memory& memory)
{
base::toIce(ice, memory);
}
- void wm::fromIce(const data::Memory& ice, Memory& memory)
+
+ void
+ wm::fromIce(const data::Memory& ice, Memory& memory)
{
base::fromIce(ice, memory);
}
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/core/wm/ice_conversions.h b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.h
index 4a58f16aaa6eef4c6f6685a86ed66298c1407454..0a67ebf2c0af7737196eaa7d8e3542f86cbafde6 100644
--- a/source/RobotAPI/libraries/armem/core/wm/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/wm/ice_conversions.h
@@ -5,7 +5,6 @@
#include "memory_definitions.h"
-
namespace armarx::armem::wm
{
@@ -28,7 +27,7 @@ namespace armarx::armem::wm
void toIce(data::Memory& ice, const Memory& memory);
void fromIce(const data::Memory& ice, Memory& memory);
-}
+} // namespace armarx::armem::wm
// Must be included after the prototypes. Otherwise the compiler cannot find the correct methods in ice_coversion_templates.h
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp
index e72b21abffa5e571ffab8437d35ce6026dd1ab62..bcb99340f28f3c8d073ecda34b1bd3cecfc6aea9 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.cpp
@@ -110,7 +110,10 @@ namespace armarx::armem::wm
}
}
- void toMemory(Memory &m, const armarx::armem::server::wm::Memory &structure, const std::vector<EntitySnapshot> &e)
+ void
+ toMemory(Memory& m,
+ const armarx::armem::server::wm::Memory& structure,
+ const std::vector<EntitySnapshot>& e)
{
// create an empty working memory:
m.clear();
diff --git a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h
index 7ea40b5f54eb2a994eb98cd5f278ee29582ce11b..7fd34cf89df28c575ef49397ee53350b1b44b94f 100644
--- a/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/wm/memory_conversions.h
@@ -2,9 +2,10 @@
#include <vector>
-#include "memory_definitions.h"
#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
+#include "memory_definitions.h"
+
namespace armarx::armem::wm
{
void toMemory(Memory& m, const std::vector<CoreSegment>& e);
@@ -19,7 +20,9 @@ namespace armarx::armem::wm
* @param structure the server working memory for structure information
* @param e the vector of entity snapshots
*/
- void toMemory(Memory& m, const armarx::armem::server::wm::Memory& structure, const std::vector<EntitySnapshot>& e);
+ void toMemory(Memory& m,
+ const armarx::armem::server::wm::Memory& structure,
+ const std::vector<EntitySnapshot>& e);
void toMemory(Memory& m, const std::vector<EntityInstance>& e);
} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.cpp b/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.cpp
index f72b37c1a7ab68320c8ff2ec8c071dd741e61f22..51faa13c6ce834b73870365d9ee81165dab9a468 100644
--- a/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.cpp
@@ -2,9 +2,8 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/core/error.h>
-
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
namespace armarx::armem::wm
{
@@ -13,9 +12,8 @@ namespace armarx::armem::wm
{
}
-
FunctionalVisitor::~FunctionalVisitor()
{
}
-}
+} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h b/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h
index 34390a1823a6f3f1651bbe59ec815a0c1ab48bd6..822a44a4b01e14a985bf3e77fa4e5d079a6cfe06 100644
--- a/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h
+++ b/source/RobotAPI/libraries/armem/core/wm/visitor/FunctionalVisitor.h
@@ -4,7 +4,6 @@
#include "Visitor.h"
-
namespace armarx::armem::wm
{
@@ -15,69 +14,87 @@ namespace armarx::armem::wm
class FunctionalVisitor : public Visitor
{
public:
-
FunctionalVisitor();
virtual ~FunctionalVisitor() override;
-
- bool visitEnter(Memory& memory) override
+ bool
+ visitEnter(Memory& memory) override
{
return memoryFn ? memoryFn(memory) : Visitor::visitEnter(memory);
}
- bool visitEnter(CoreSegment& coreSegment) override
+
+ bool
+ visitEnter(CoreSegment& coreSegment) override
{
return coreSegmentFn ? coreSegmentFn(coreSegment) : Visitor::visitEnter(coreSegment);
}
- bool visitEnter(ProviderSegment& providerSegment) override
+
+ bool
+ visitEnter(ProviderSegment& providerSegment) override
{
- return providerSegmentFn ? providerSegmentFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ return providerSegmentFn ? providerSegmentFn(providerSegment)
+ : Visitor::visitEnter(providerSegment);
}
- bool visitEnter(Entity& entity) override
+
+ bool
+ visitEnter(Entity& entity) override
{
return entityFn ? entityFn(entity) : Visitor::visitEnter(entity);
}
- bool visitEnter(EntitySnapshot& snapshot) override
+
+ bool
+ visitEnter(EntitySnapshot& snapshot) override
{
return snapshotFn ? snapshotFn(snapshot) : Visitor::visitEnter(snapshot);
}
- bool visit(EntityInstance& instance) override
+ bool
+ visit(EntityInstance& instance) override
{
return instanceFn ? instanceFn(instance) : Visitor::visit(instance);
}
-
-
// Const versions
- bool visitEnter(const Memory& memory) override
+ bool
+ visitEnter(const Memory& memory) override
{
return memoryConstFn ? memoryConstFn(memory) : Visitor::visitEnter(memory);
}
- bool visitEnter(const CoreSegment& coreSegment) override
+
+ bool
+ visitEnter(const CoreSegment& coreSegment) override
{
- return coreSegmentConstFn ? coreSegmentConstFn(coreSegment) : Visitor::visitEnter(coreSegment);
+ return coreSegmentConstFn ? coreSegmentConstFn(coreSegment)
+ : Visitor::visitEnter(coreSegment);
}
- bool visitEnter(const ProviderSegment& providerSegment) override
+
+ bool
+ visitEnter(const ProviderSegment& providerSegment) override
{
- return providerSegmentConstFn ? providerSegmentConstFn(providerSegment) : Visitor::visitEnter(providerSegment);
+ return providerSegmentConstFn ? providerSegmentConstFn(providerSegment)
+ : Visitor::visitEnter(providerSegment);
}
- bool visitEnter(const Entity& entity) override
+
+ bool
+ visitEnter(const Entity& entity) override
{
return entityConstFn ? entityConstFn(entity) : Visitor::visitEnter(entity);
}
- bool visitEnter(const EntitySnapshot& snapshot) override
+
+ bool
+ visitEnter(const EntitySnapshot& snapshot) override
{
return snapshotConstFn ? snapshotConstFn(snapshot) : Visitor::visitEnter(snapshot);
}
- bool visit(const EntityInstance& instance) override
+ bool
+ visit(const EntityInstance& instance) override
{
return instanceConstFn ? instanceConstFn(instance) : Visitor::visit(instance);
}
-
std::function<bool(Memory& memory)> memoryFn;
std::function<bool(const Memory& memory)> memoryConstFn;
@@ -95,7 +112,6 @@ namespace armarx::armem::wm
std::function<bool(EntityInstance& instance)> instanceFn;
std::function<bool(const EntityInstance& instance)> instanceConstFn;
-
};
-}
+} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.cpp b/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.cpp
index 9448f55f8f68385d716f9b8e7da26e890ffb48fd..8e3cab7a4a628a1da807ca52690f2922a7eed101 100644
--- a/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.cpp
+++ b/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.cpp
@@ -2,10 +2,9 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-
namespace armarx::armem::wm
{
@@ -18,125 +17,116 @@ namespace armarx::armem::wm
{
}
- bool Visitor::applyTo(Memory& memory)
+ bool
+ Visitor::applyTo(Memory& memory)
{
visitEnter(memory);
- bool cont = memory.forEachCoreSegment([this](CoreSegment & coreSeg)
- {
- return applyTo(coreSeg);
- });
+ bool cont =
+ memory.forEachCoreSegment([this](CoreSegment& coreSeg) { return applyTo(coreSeg); });
visitExit(memory);
return cont;
}
- bool Visitor::applyTo(CoreSegment& coreSegment)
+ bool
+ Visitor::applyTo(CoreSegment& coreSegment)
{
visitEnter(coreSegment);
- bool cont = coreSegment.forEachProviderSegment([this](ProviderSegment & provSeg)
- {
- return applyTo(provSeg);
- });
+ bool cont = coreSegment.forEachProviderSegment([this](ProviderSegment& provSeg)
+ { return applyTo(provSeg); });
visitExit(coreSegment);
return cont;
}
- bool Visitor::applyTo(ProviderSegment& providerSegment)
+ bool
+ Visitor::applyTo(ProviderSegment& providerSegment)
{
visitEnter(providerSegment);
- bool cont = providerSegment.forEachEntity([this](Entity & entity)
- {
- return applyTo(entity);
- });
+ bool cont =
+ providerSegment.forEachEntity([this](Entity& entity) { return applyTo(entity); });
visitExit(providerSegment);
return cont;
}
- bool Visitor::applyTo(Entity& entity)
+ bool
+ Visitor::applyTo(Entity& entity)
{
visitEnter(entity);
- bool cont = entity.forEachSnapshot([this](EntitySnapshot & snapshot)
- {
- return applyTo(snapshot);
- });
+ bool cont =
+ entity.forEachSnapshot([this](EntitySnapshot& snapshot) { return applyTo(snapshot); });
visitExit(entity);
return cont;
}
- bool Visitor::applyTo(EntitySnapshot& snapshot)
+ bool
+ Visitor::applyTo(EntitySnapshot& snapshot)
{
visitEnter(snapshot);
- bool cont = snapshot.forEachInstance([this](EntityInstance & instance)
- {
- return applyTo(instance);
- });
+ bool cont = snapshot.forEachInstance([this](EntityInstance& instance)
+ { return applyTo(instance); });
visitExit(snapshot);
return cont;
}
- bool Visitor::applyTo(EntityInstance& instance)
+ bool
+ Visitor::applyTo(EntityInstance& instance)
{
return visit(instance);
}
-
- bool Visitor::applyTo(const Memory& memory)
+ bool
+ Visitor::applyTo(const Memory& memory)
{
visitEnter(memory);
- bool cont = memory.forEachCoreSegment([this](const CoreSegment & coreSeg)
- {
- return applyTo(coreSeg);
- });
+ bool cont = memory.forEachCoreSegment([this](const CoreSegment& coreSeg)
+ { return applyTo(coreSeg); });
visitExit(memory);
return cont;
}
- bool Visitor::applyTo(const CoreSegment& coreSegment)
+ bool
+ Visitor::applyTo(const CoreSegment& coreSegment)
{
visitEnter(coreSegment);
- bool cont = coreSegment.forEachProviderSegment([this](const ProviderSegment & provSeg)
- {
- return applyTo(provSeg);
- });
+ bool cont = coreSegment.forEachProviderSegment([this](const ProviderSegment& provSeg)
+ { return applyTo(provSeg); });
visitExit(coreSegment);
return cont;
}
- bool Visitor::applyTo(const ProviderSegment& providerSegment)
+ bool
+ Visitor::applyTo(const ProviderSegment& providerSegment)
{
visitEnter(providerSegment);
- bool cont = providerSegment.forEachEntity([this](const Entity & entity)
- {
- return applyTo(entity);
- });
+ bool cont =
+ providerSegment.forEachEntity([this](const Entity& entity) { return applyTo(entity); });
visitExit(providerSegment);
return cont;
}
- bool Visitor::applyTo(const Entity& entity)
+ bool
+ Visitor::applyTo(const Entity& entity)
{
visitEnter(entity);
- bool cont = entity.forEachSnapshot([this](const EntitySnapshot & snapshot)
- {
- return applyTo(snapshot);
- });
+ bool cont = entity.forEachSnapshot([this](const EntitySnapshot& snapshot)
+ { return applyTo(snapshot); });
visitExit(entity);
return cont;
}
- bool Visitor::applyTo(const EntitySnapshot& snapshot)
+ bool
+ Visitor::applyTo(const EntitySnapshot& snapshot)
{
visitEnter(snapshot);
- bool cont = snapshot.forEachInstance([this](const EntityInstance & instance)
- {
- return applyTo(instance);
- });
+ bool cont = snapshot.forEachInstance([this](const EntityInstance& instance)
+ { return applyTo(instance); });
visitExit(snapshot);
return cont;
}
- bool Visitor::applyTo(const EntityInstance& instance)
+ bool
+ Visitor::applyTo(const EntityInstance& instance)
{
return visit(instance);
}
-}
+} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.h b/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.h
index fa789fcf0b6876ec5d9fa0ff1a3944b4e3bf3fd3..f240779fd3a267a21529dac99efddbd438d92be8 100644
--- a/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.h
+++ b/source/RobotAPI/libraries/armem/core/wm/visitor/Visitor.h
@@ -1,6 +1,5 @@
#pragma once
-
namespace armarx::armem::wm
{
class Memory;
@@ -10,7 +9,6 @@ namespace armarx::armem::wm
class EntitySnapshot;
class EntityInstance;
-
/**
* @brief A visitor for the hierarchical memory data structure.
*/
@@ -18,7 +16,6 @@ namespace armarx::armem::wm
{
public:
-
Visitor();
virtual ~Visitor();
@@ -29,56 +26,72 @@ namespace armarx::armem::wm
bool applyTo(EntitySnapshot& snapshot);
bool applyTo(EntityInstance& instance);
-
- virtual bool visitEnter(Memory& memory)
+ virtual bool
+ visitEnter(Memory& memory)
{
return visitEnter(const_cast<const Memory&>(memory));
}
- virtual bool visitEnter(CoreSegment& coreSegment)
+
+ virtual bool
+ visitEnter(CoreSegment& coreSegment)
{
return visitEnter(const_cast<const CoreSegment&>(coreSegment));
}
- virtual bool visitEnter(ProviderSegment& providerSegment)
+
+ virtual bool
+ visitEnter(ProviderSegment& providerSegment)
{
return visitEnter(const_cast<const ProviderSegment&>(providerSegment));
}
- virtual bool visitEnter(Entity& entity)
+
+ virtual bool
+ visitEnter(Entity& entity)
{
return visitEnter(const_cast<const Entity&>(entity));
}
- virtual bool visitEnter(EntitySnapshot& snapshot)
+
+ virtual bool
+ visitEnter(EntitySnapshot& snapshot)
{
return visitEnter(const_cast<const EntitySnapshot&>(snapshot));
}
- virtual bool visitExit(Memory& memory)
+ virtual bool
+ visitExit(Memory& memory)
{
return visitExit(const_cast<const Memory&>(memory));
}
- virtual bool visitExit(CoreSegment& coreSegment)
+
+ virtual bool
+ visitExit(CoreSegment& coreSegment)
{
return visitExit(const_cast<const CoreSegment&>(coreSegment));
}
- virtual bool visitExit(ProviderSegment& providerSegment)
+
+ virtual bool
+ visitExit(ProviderSegment& providerSegment)
{
return visitExit(const_cast<const ProviderSegment&>(providerSegment));
}
- virtual bool visitExit(Entity& entity)
+
+ virtual bool
+ visitExit(Entity& entity)
{
return visitExit(const_cast<const Entity&>(entity));
}
- virtual bool visitExit(EntitySnapshot& snapshot)
+
+ virtual bool
+ visitExit(EntitySnapshot& snapshot)
{
return visitExit(const_cast<const EntitySnapshot&>(snapshot));
}
- virtual bool visit(EntityInstance& instance)
+ virtual bool
+ visit(EntityInstance& instance)
{
return visit(const_cast<const EntityInstance&>(instance));
}
-
-
// Const versions
bool applyTo(const Memory& memory);
@@ -88,65 +101,82 @@ namespace armarx::armem::wm
bool applyTo(const EntitySnapshot& snapshot);
bool applyTo(const EntityInstance& instance);
-
- virtual bool visitEnter(const Memory& memory)
+ virtual bool
+ visitEnter(const Memory& memory)
{
- (void) memory;
+ (void)memory;
return true;
}
- virtual bool visitEnter(const CoreSegment& coreSegment)
+
+ virtual bool
+ visitEnter(const CoreSegment& coreSegment)
{
- (void) coreSegment;
+ (void)coreSegment;
return true;
}
- virtual bool visitEnter(const ProviderSegment& providerSegment)
+
+ virtual bool
+ visitEnter(const ProviderSegment& providerSegment)
{
- (void) providerSegment;
+ (void)providerSegment;
return true;
}
- virtual bool visitEnter(const Entity& entity)
+
+ virtual bool
+ visitEnter(const Entity& entity)
{
- (void) entity;
+ (void)entity;
return true;
}
- virtual bool visitEnter(const EntitySnapshot& snapshot)
+
+ virtual bool
+ visitEnter(const EntitySnapshot& snapshot)
{
- (void) snapshot;
+ (void)snapshot;
return true;
}
- virtual bool visitExit(const Memory& memory)
+ virtual bool
+ visitExit(const Memory& memory)
{
- (void) memory;
+ (void)memory;
return true;
}
- virtual bool visitExit(const CoreSegment& coreSegment)
+
+ virtual bool
+ visitExit(const CoreSegment& coreSegment)
{
- (void) coreSegment;
+ (void)coreSegment;
return true;
}
- virtual bool visitExit(const ProviderSegment& providerSegment)
+
+ virtual bool
+ visitExit(const ProviderSegment& providerSegment)
{
- (void) providerSegment;
+ (void)providerSegment;
return true;
}
- virtual bool visitExit(const Entity& entity)
+
+ virtual bool
+ visitExit(const Entity& entity)
{
- (void) entity;
+ (void)entity;
return true;
}
- virtual bool visitExit(const EntitySnapshot& snapshot)
+
+ virtual bool
+ visitExit(const EntitySnapshot& snapshot)
{
- (void) snapshot;
+ (void)snapshot;
return true;
}
- virtual bool visit(const EntityInstance& instance)
+ virtual bool
+ visit(const EntityInstance& instance)
{
- (void) instance;
+ (void)instance;
return true;
}
-
};
-}
+} // namespace armarx::armem::wm
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.cpp b/source/RobotAPI/libraries/armem/mns/Registry.cpp
index b11b822285d84ba557e481a0b18ec85469688702..38fe2da405a30b73d0f3114dd464e2bed6a1d8e2 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.cpp
+++ b/source/RobotAPI/libraries/armem/mns/Registry.cpp
@@ -1,8 +1,8 @@
#include "Registry.h"
#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/interface/armem/server/ActionsInterface.h>
+#include <RobotAPI/interface/armem/server/ActionsInterface.h>
namespace armarx::armem::mns
{
@@ -12,14 +12,14 @@ namespace armarx::armem::mns
armarx::Logging::setTag(logTag);
}
-
- bool Registry::hasServer(const std::string& memoryName) const
+ bool
+ Registry::hasServer(const std::string& memoryName) const
{
return servers.count(memoryName) > 0;
}
-
- dto::RegisterServerResult Registry::registerServer(const dto::RegisterServerInput& input)
+ dto::RegisterServerResult
+ Registry::registerServer(const dto::RegisterServerInput& input)
{
dto::RegisterServerResult result;
@@ -60,8 +60,8 @@ namespace armarx::armem::mns
return result;
}
-
- dto::RemoveServerResult Registry::removeServer(const dto::RemoveServerInput& input)
+ dto::RemoveServerResult
+ Registry::removeServer(const dto::RemoveServerInput& input)
{
dto::RemoveServerResult result;
@@ -85,9 +85,9 @@ namespace armarx::armem::mns
return result;
}
-
template <class Prx>
- bool isAvailable(const Prx& proxy)
+ bool
+ isAvailable(const Prx& proxy)
{
if (proxy)
{
@@ -103,7 +103,6 @@ namespace armarx::armem::mns
return false;
}
-
dto::GetAllRegisteredServersResult
Registry::getAllRegisteredServers()
{
@@ -122,8 +121,8 @@ namespace armarx::armem::mns
return result;
}
-
- dto::ResolveServerResult Registry::resolveServer(const dto::ResolveServerInput& input)
+ dto::ResolveServerResult
+ Registry::resolveServer(const dto::ResolveServerInput& input)
{
dto::ResolveServerResult result;
try
@@ -147,4 +146,4 @@ namespace armarx::armem::mns
return result;
}
-}
+} // namespace armarx::armem::mns
diff --git a/source/RobotAPI/libraries/armem/mns/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/mns/plugins/Plugin.cpp
index a2e7c5dbf3fe87d6fdf24fee0181504a5f85abe9..298b933328d8358ed3062b64a8be5be9e0c0ed4d 100644
--- a/source/RobotAPI/libraries/armem/mns/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/mns/plugins/Plugin.cpp
@@ -1,8 +1,6 @@
#include "Plugin.h"
-
namespace armarx::armem::mns::plugins
{
}
-
diff --git a/source/RobotAPI/libraries/armem/mns/plugins/Plugin.h b/source/RobotAPI/libraries/armem/mns/plugins/Plugin.h
index 5caa973a9ebb7025f42978c608c033ce47722074..f9d8ed71c58e8e69d8c4114130e88a496369d362 100644
--- a/source/RobotAPI/libraries/armem/mns/plugins/Plugin.h
+++ b/source/RobotAPI/libraries/armem/mns/plugins/Plugin.h
@@ -1,9 +1,8 @@
#pragma once
-#include <RobotAPI/libraries/armem/mns/MemoryNameSystem.h>
-
#include <ArmarXCore/core/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem/mns/MemoryNameSystem.h>
namespace armarx::armem::mns::plugins
{
@@ -11,14 +10,11 @@ namespace armarx::armem::mns::plugins
class Plugin : public armarx::ComponentPlugin
{
public:
-
using armarx::ComponentPlugin::ComponentPlugin;
public:
-
MemoryNameSystem mns;
-
};
-}
+} // namespace armarx::armem::mns::plugins
diff --git a/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.cpp b/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.cpp
index 4ad29cdeec3c2b732cf1b9074931ab3c116fa42b..471a728c427a84b48caca2ea854b3009daaa51c7 100644
--- a/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.cpp
@@ -1,6 +1,6 @@
#include "PluginUser.h"
-#include "Plugin.h"
+#include "Plugin.h"
namespace armarx::armem::mns::plugins
{
@@ -10,23 +10,22 @@ namespace armarx::armem::mns::plugins
addPlugin(plugin);
}
-
- dto::RegisterServerResult PluginUser::registerServer(const dto::RegisterServerInput& input, const Ice::Current&)
+ dto::RegisterServerResult
+ PluginUser::registerServer(const dto::RegisterServerInput& input, const Ice::Current&)
{
std::scoped_lock lock(mnsMutex);
dto::RegisterServerResult result = plugin->mns.registerServer(input);
return result;
}
-
- dto::RemoveServerResult PluginUser::removeServer(const dto::RemoveServerInput& input, const Ice::Current&)
+ dto::RemoveServerResult
+ PluginUser::removeServer(const dto::RemoveServerInput& input, const Ice::Current&)
{
std::scoped_lock lock(mnsMutex);
dto::RemoveServerResult result = plugin->mns.removeServer(input);
return result;
}
-
dto::GetAllRegisteredServersResult
PluginUser::getAllRegisteredServers(const Ice::Current&)
{
@@ -35,35 +34,34 @@ namespace armarx::armem::mns::plugins
return result;
}
-
- dto::ResolveServerResult PluginUser::resolveServer(const dto::ResolveServerInput& input, const Ice::Current&)
+ dto::ResolveServerResult
+ PluginUser::resolveServer(const dto::ResolveServerInput& input, const Ice::Current&)
{
std::scoped_lock lock(mnsMutex);
dto::ResolveServerResult result = plugin->mns.resolveServer(input);
return result;
}
-
// dto::WaitForServerResult PluginUser::waitForServer(const dto::WaitForServerInput& input, const Ice::Current&)
- void PluginUser::waitForServer_async(
- const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
- const dto::WaitForServerInput& input,
- const Ice::Current&)
+ void
+ PluginUser::waitForServer_async(const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
+ const dto::WaitForServerInput& input,
+ const Ice::Current&)
{
std::scoped_lock lock(mnsMutex);
plugin->mns.waitForServer_async(future, input);
}
-
- MemoryNameSystem& PluginUser::mns()
+ MemoryNameSystem&
+ PluginUser::mns()
{
return plugin->mns;
}
-
- const MemoryNameSystem& PluginUser::mns() const
+ const MemoryNameSystem&
+ PluginUser::mns() const
{
return plugin->mns;
}
-}
+} // namespace armarx::armem::mns::plugins
diff --git a/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.h b/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.h
index 1e8134ad5d372b1d69252cdc0cf2f5916c948b1e..cfecf4bc06627c91f34de67956ea5aaf7e7f6356 100644
--- a/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.h
+++ b/source/RobotAPI/libraries/armem/mns/plugins/PluginUser.h
@@ -1,55 +1,49 @@
#pragma once
-#include <RobotAPI/libraries/armem/mns/MemoryNameSystem.h>
-
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <mutex>
#include <ArmarXCore/core/ManagedIceObject.h>
-#include <mutex>
-
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/mns/MemoryNameSystem.h>
namespace armarx::armem::mns::plugins
{
class Plugin;
-
class PluginUser :
- virtual public ManagedIceObject
- , virtual public mns::MemoryNameSystemInterface
+ virtual public ManagedIceObject,
+ virtual public mns::MemoryNameSystemInterface
{
public:
-
PluginUser();
// mns::MemoryNameSystemInterface interface
public:
-
- dto::RegisterServerResult registerServer(const dto::RegisterServerInput& input, const Ice::Current& = Ice::emptyCurrent) override;
- dto::RemoveServerResult removeServer(const dto::RemoveServerInput& input, const Ice::Current& = Ice::emptyCurrent) override;
+ dto::RegisterServerResult registerServer(const dto::RegisterServerInput& input,
+ const Ice::Current& = Ice::emptyCurrent) override;
+ dto::RemoveServerResult removeServer(const dto::RemoveServerInput& input,
+ const Ice::Current& = Ice::emptyCurrent) override;
dto::GetAllRegisteredServersResult getAllRegisteredServers(const Ice::Current&) override;
- dto::ResolveServerResult resolveServer(const dto::ResolveServerInput& input, const Ice::Current& = Ice::emptyCurrent) override;
+ dto::ResolveServerResult resolveServer(const dto::ResolveServerInput& input,
+ const Ice::Current& = Ice::emptyCurrent) override;
// Uses Asynchronous Method Dispatch (AMD)
- void waitForServer_async(
- const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
- const dto::WaitForServerInput& input,
- const Ice::Current& = Ice::emptyCurrent) override;
+ void waitForServer_async(const AMD_MemoryNameSystemInterface_waitForServerPtr& future,
+ const dto::WaitForServerInput& input,
+ const Ice::Current& = Ice::emptyCurrent) override;
protected:
-
std::mutex mnsMutex;
MemoryNameSystem& mns();
const MemoryNameSystem& mns() const;
private:
-
plugins::Plugin* plugin = nullptr;
-
};
-}
+} // namespace armarx::armem::mns::plugins
diff --git a/source/RobotAPI/libraries/armem/server.h b/source/RobotAPI/libraries/armem/server.h
index 10950a29e308a75b5d30f2e3dc6bd075717ff1ce..4920f1cdeb8c348cadd89ab85c8c83736e62bab6 100644
--- a/source/RobotAPI/libraries/armem/server.h
+++ b/source/RobotAPI/libraries/armem/server.h
@@ -3,9 +3,8 @@
#include "server/ComponentPlugin.h"
#include "server/MemoryRemoteGui.h"
-
namespace armarx::armem
{
using server::MemoryRemoteGui;
using ServerComponentPluginUser = server::ComponentPluginUser;
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
index 1f0014207719d7fa996d66b0fdfde4d0f04ccfec..11aa8a852e4b2f6bd90ec2610e558ed0ae90f0ad 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.cpp
@@ -1,22 +1,22 @@
#include "MemoryRemoteGui.h"
-#include "RemoteGuiAronDataVisitor.h"
+#include <mutex>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include <SimoxUtility/meta/type_name.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <SimoxUtility/meta/type_name.h>
-
-#include <mutex>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include "RemoteGuiAronDataVisitor.h"
namespace armarx::armem::server
{
- template <class ...Args>
- MemoryRemoteGui::GroupBox MemoryRemoteGui::_makeGroupBox(const armem::base::MemoryBase<Args...>& memory) const
+ template <class... Args>
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::_makeGroupBox(const armem::base::MemoryBase<Args...>& memory) const
{
GroupBox group;
group.setLabel(makeGroupLabel("Memory", memory.name(), memory.size()));
@@ -25,15 +25,13 @@ namespace armarx::armem::server
{
group.addChild(Label(makeNoItemsMessage("core segments")));
}
- memory.forEachCoreSegment([this, &group](const auto & coreSegment)
- {
- group.addChild(this->makeGroupBox(coreSegment));
- });
+ memory.forEachCoreSegment([this, &group](const auto& coreSegment)
+ { group.addChild(this->makeGroupBox(coreSegment)); });
return group;
}
-
- static std::string getTypeString(const armem::base::detail::AronTyped& typed)
+ static std::string
+ getTypeString(const armem::base::detail::AronTyped& typed)
{
std::stringstream type;
if (typed.aronType())
@@ -47,46 +45,46 @@ namespace armarx::armem::server
return type.str();
}
- template <class ...Args>
- MemoryRemoteGui::GroupBox MemoryRemoteGui::_makeGroupBox(const armem::base::CoreSegmentBase<Args...>& coreSegment) const
+ template <class... Args>
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::_makeGroupBox(const armem::base::CoreSegmentBase<Args...>& coreSegment) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("Core Segment", coreSegment.name(), coreSegment.size())
- + getTypeString(coreSegment));
+ group.setLabel(makeGroupLabel("Core Segment", coreSegment.name(), coreSegment.size()) +
+ getTypeString(coreSegment));
if (coreSegment.empty())
{
group.addChild(Label(makeNoItemsMessage("provider segments")));
}
- coreSegment.forEachProviderSegment([this, &group](const auto & providerSegment)
- {
- group.addChild(this->makeGroupBox(providerSegment));
- });
+ coreSegment.forEachProviderSegment(
+ [this, &group](const auto& providerSegment)
+ { group.addChild(this->makeGroupBox(providerSegment)); });
return group;
}
-
- template <class ...Args>
- MemoryRemoteGui::GroupBox MemoryRemoteGui::_makeGroupBox(const armem::base::ProviderSegmentBase<Args...>& providerSegment) const
+ template <class... Args>
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::_makeGroupBox(
+ const armem::base::ProviderSegmentBase<Args...>& providerSegment) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("Provider Segment", providerSegment.name(), providerSegment.size())
- + getTypeString(providerSegment));
+ group.setLabel(
+ makeGroupLabel("Provider Segment", providerSegment.name(), providerSegment.size()) +
+ getTypeString(providerSegment));
if (providerSegment.empty())
{
group.addChild(Label(makeNoItemsMessage("entities")));
}
- providerSegment.forEachEntity([this, &group](const auto & entity)
- {
- group.addChild(this->makeGroupBox(entity));
- });
+ providerSegment.forEachEntity([this, &group](const auto& entity)
+ { group.addChild(this->makeGroupBox(entity)); });
return group;
}
-
- template <class ...Args>
- MemoryRemoteGui::GroupBox MemoryRemoteGui::_makeGroupBox(const armem::base::EntityBase<Args...>& entity) const
+ template <class... Args>
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::_makeGroupBox(const armem::base::EntityBase<Args...>& entity) const
{
GroupBox group;
group.setLabel(makeGroupLabel("Entity", entity.name(), entity.size()));
@@ -96,10 +94,8 @@ namespace armarx::armem::server
group.addChild(Label(makeNoItemsMessage("snapshots")));
}
- auto addChild = [this, &group](const armem::wm::EntitySnapshot & snapshot)
- {
- group.addChild(makeGroupBox(snapshot));
- };
+ auto addChild = [this, &group](const armem::wm::EntitySnapshot& snapshot)
+ { group.addChild(makeGroupBox(snapshot)); };
if (int(entity.size()) <= maxHistorySize)
{
@@ -116,79 +112,79 @@ namespace armarx::armem::server
return group;
}
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::server::wm::Memory& memory) const
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::makeGroupBox(const armem::server::wm::Memory& memory) const
{
return this->_makeGroupBox(memory);
}
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::Memory& memory) const
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::makeGroupBox(const armem::wm::Memory& memory) const
{
return this->_makeGroupBox(memory);
}
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::server::wm::CoreSegment& coreSegment) const
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::makeGroupBox(const armem::server::wm::CoreSegment& coreSegment) const
{
return coreSegment.doLocked([this, &coreSegment]()
- {
- return this->_makeGroupBox(coreSegment);
- });
+ { return this->_makeGroupBox(coreSegment); });
}
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::CoreSegment& coreSegment) const
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::makeGroupBox(const armem::wm::CoreSegment& coreSegment) const
{
return this->_makeGroupBox(coreSegment);
}
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::server::wm::ProviderSegment& providerSegment) const
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::makeGroupBox(const armem::server::wm::ProviderSegment& providerSegment) const
{
return this->_makeGroupBox(providerSegment);
}
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::ProviderSegment& providerSegment) const
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::makeGroupBox(const armem::wm::ProviderSegment& providerSegment) const
{
return this->_makeGroupBox(providerSegment);
}
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::Entity& entity) const
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::makeGroupBox(const armem::wm::Entity& entity) const
{
return this->_makeGroupBox(entity);
}
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::server::wm::Entity& entity) const
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::makeGroupBox(const armem::server::wm::Entity& entity) const
{
return this->_makeGroupBox(entity);
}
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::EntitySnapshot& snapshot) const
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::makeGroupBox(const armem::wm::EntitySnapshot& snapshot) const
{
GroupBox group;
- group.setLabel(makeGroupLabel("t", armem::toDateTimeMilliSeconds(snapshot.time()),
- snapshot.size(), " = ", ""));
+ group.setLabel(makeGroupLabel(
+ "t", armem::toDateTimeMilliSeconds(snapshot.time()), snapshot.size(), " = ", ""));
if (snapshot.empty())
{
group.addChild(Label(makeNoItemsMessage("instances")));
}
- snapshot.forEachInstance([this, &group](const armem::wm::EntityInstance & instance)
- {
- group.addChild(makeGroupBox(instance));
- return true;
- });
+ snapshot.forEachInstance(
+ [this, &group](const armem::wm::EntityInstance& instance)
+ {
+ group.addChild(makeGroupBox(instance));
+ return true;
+ });
group.setCollapsed(true);
return group;
}
-
- MemoryRemoteGui::GroupBox MemoryRemoteGui::makeGroupBox(const armem::wm::EntityInstance& instance) const
+ MemoryRemoteGui::GroupBox
+ MemoryRemoteGui::makeGroupBox(const armem::wm::EntityInstance& instance) const
{
GroupBox group;
@@ -211,18 +207,20 @@ namespace armarx::armem::server
return group;
}
-
- std::string MemoryRemoteGui::makeGroupLabel(
- const std::string& term, const std::string& name, size_t size,
- const std::string& namePrefix, const std::string& nameSuffix) const
+ std::string
+ MemoryRemoteGui::makeGroupLabel(const std::string& term,
+ const std::string& name,
+ size_t size,
+ const std::string& namePrefix,
+ const std::string& nameSuffix) const
{
std::stringstream ss;
ss << term << namePrefix << name << nameSuffix << " (" << size << ")";
return ss.str();
}
-
- std::string MemoryRemoteGui::makeNoItemsMessage(const std::string& term) const
+ std::string
+ MemoryRemoteGui::makeNoItemsMessage(const std::string& term) const
{
std::stringstream ss;
ss << "(no " << term << ")";
@@ -230,6 +228,4 @@ namespace armarx::armem::server
}
-
-
-}
+} // namespace armarx::armem::server
diff --git a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h
index c6aef5c51546ac203c50d38f80696b89f3b6f3b9..022688c7dc02db357d2ce6d622dc5d7417b87bba 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h
+++ b/source/RobotAPI/libraries/armem/server/MemoryRemoteGui.h
@@ -4,7 +4,6 @@
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
namespace armarx::armem::server
{
@@ -31,26 +30,30 @@ namespace armarx::armem::server
GroupBox makeGroupBox(const armem::server::wm::Entity& entity) const;
- std::string makeGroupLabel(const std::string& term, const std::string& name, size_t size,
- const std::string& namePrefix = ": '", const std::string& nameSuffix = "'") const;
+ std::string makeGroupLabel(const std::string& term,
+ const std::string& name,
+ size_t size,
+ const std::string& namePrefix = ": '",
+ const std::string& nameSuffix = "'") const;
std::string makeNoItemsMessage(const std::string& term) const;
-
int maxHistorySize = 10;
private:
-
- template <class ...Args>
- MemoryRemoteGui::GroupBox _makeGroupBox(const armem::base::MemoryBase<Args...>& memory) const;
- template <class ...Args>
- MemoryRemoteGui::GroupBox _makeGroupBox(const armem::base::CoreSegmentBase<Args...>& coreSegment) const;
- template <class ...Args>
- MemoryRemoteGui::GroupBox _makeGroupBox(const armem::base::ProviderSegmentBase<Args...>& providerSegment) const;
- template <class ...Args>
- MemoryRemoteGui::GroupBox _makeGroupBox(const armem::base::EntityBase<Args...>& entity) const;
-
+ template <class... Args>
+ MemoryRemoteGui::GroupBox
+ _makeGroupBox(const armem::base::MemoryBase<Args...>& memory) const;
+ template <class... Args>
+ MemoryRemoteGui::GroupBox
+ _makeGroupBox(const armem::base::CoreSegmentBase<Args...>& coreSegment) const;
+ template <class... Args>
+ MemoryRemoteGui::GroupBox
+ _makeGroupBox(const armem::base::ProviderSegmentBase<Args...>& providerSegment) const;
+ template <class... Args>
+ MemoryRemoteGui::GroupBox
+ _makeGroupBox(const armem::base::EntityBase<Args...>& entity) const;
};
-}
+} // namespace armarx::armem::server
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index d878781a7022485802e86620ec06abef6300c8e8..f5d0a57fb298492283af3e544e91cb2454403c6d 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -1,9 +1,9 @@
#include "MemoryToIceAdapter.h"
-#include <thread>
-#include <list>
#include <iostream>
+#include <list>
#include <sstream>
+#include <thread>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
@@ -239,7 +239,9 @@ namespace armarx::armem::server
}
// Consolidate to ltm(s) if recording mode is CLONE_WM
- if (longtermMemory->isRecording() && longtermMemory->getRecordingMode() == server::ltm::RecordingMode::CONSOLIDATE_ALL)
+ if (longtermMemory->isRecording() &&
+ longtermMemory->getRecordingMode() ==
+ server::ltm::RecordingMode::CONSOLIDATE_ALL)
{
// convert removedSnapshots to Memory
armem::wm::Memory m(longtermMemory->name());
@@ -252,7 +254,9 @@ namespace armarx::armem::server
// Consolidate to ltm(s) if recording mode is CONSOLIDATE_REMOVED
- if (longtermMemory->isRecording() && longtermMemory->getRecordingMode() == server::ltm::RecordingMode::CONSOLIDATE_REMOVED)
+ if (longtermMemory->isRecording() &&
+ longtermMemory->getRecordingMode() ==
+ server::ltm::RecordingMode::CONSOLIDATE_REMOVED)
{
// convert removedSnapshots to Memory
armem::wm::Memory m(longtermMemory->name());
@@ -263,7 +267,9 @@ namespace armarx::armem::server
longtermMemory->store(m);
}
- if (longtermMemory->isRecording() && longtermMemory->getRecordingMode() == server::ltm::RecordingMode::CONSOLIDATE_LATEST)
+ if (longtermMemory->isRecording() &&
+ longtermMemory->getRecordingMode() ==
+ server::ltm::RecordingMode::CONSOLIDATE_LATEST)
{
ARMARX_WARNING << deactivateSpam() << "THIS IS NOT IMPLEMENTED YET!!!";
}
@@ -401,10 +407,11 @@ namespace armarx::armem::server
}
// WM LOADING FROM LTM
- armem::CommitResult
+ armem::CommitResult
MemoryToIceAdapter::reloadFromLTM()
{
- if(this->longtermMemory->p.importOnStartUp){
+ if (this->longtermMemory->p.importOnStartUp)
+ {
ARMARX_INFO << "Reloading of data from LTM into WM triggered";
//define which core segments to load and how many snapshots:
auto coreNames = this->longtermMemory->p.coreSegmentsToLoad;
@@ -413,7 +420,8 @@ namespace armarx::armem::server
std::stringstream ss(coreNames);
std::string item;
- while (std::getline(ss, item, ',')) {
+ while (std::getline(ss, item, ','))
+ {
names.push_back(item);
}
int maxAmountOfSnapshots = this->longtermMemory->p.maxAmountOfSnapshotsLoaded;
@@ -427,12 +435,14 @@ namespace armarx::armem::server
ARMARX_DEBUG << "After commit";
//the CommitResult contains some information which might be helpful:
return res;
- } else {
- ARMARX_INFO << "Not loading initial data from LTM due to importOnStartup being " << this->longtermMemory->p.importOnStartUp;
+ }
+ else
+ {
+ ARMARX_INFO << "Not loading initial data from LTM due to importOnStartup being "
+ << this->longtermMemory->p.importOnStartUp;
armem::CommitResult r;
return r;
}
-
}
// LTM STORING AND RECORDING
@@ -473,22 +483,28 @@ namespace armarx::armem::server
ARMARX_CHECK_NOT_NULL(workingMemory);
ARMARX_IMPORTANT << "Disabling the recording of memory " << longtermMemory->id().str();
- if(longtermMemory->p.storeOnStop){ //if true this means when stopping LTM recording leftover snapshots are transferred to WM using the simulated consolidation
+ if (longtermMemory->p.storeOnStop)
+ { //if true this means when stopping LTM recording leftover snapshots are transferred to WM using the simulated consolidation
ARMARX_INFO << "Starting to save left-over WM data into LTM";
longtermMemory->directlyStore(*workingMemory, true);
ARMARX_INFO << "Stored leftover WM data into LTM";
- } else {
- ARMARX_INFO << "Not storing WM data into LTM on stop, because storeOnStop is " << longtermMemory->p.storeOnStop;
+ }
+ else
+ {
+ ARMARX_INFO << "Not storing WM data into LTM on stop, because storeOnStop is "
+ << longtermMemory->p.storeOnStop;
}
//put calling stopRecording into a seperate thread and detach to make sure GUI does not freeze
auto ltm = longtermMemory;
- std::thread stopRecordingThread([<m](){
- ltm->stopRecording();
- ltm->bufferFinished();
- ARMARX_INFO << "Storing finished";
- });
+ std::thread stopRecordingThread(
+ [<m]()
+ {
+ ltm->stopRecording();
+ ltm->bufferFinished();
+ ARMARX_INFO << "Storing finished";
+ });
stopRecordingThread.detach();
ARMARX_IMPORTANT
diff --git a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp
index 4ce73be809be81f4a3ccc6febcb75eb7b103a58d..a6d23c4936dc8e533699b4e0877a992bf160121f 100644
--- a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.cpp
@@ -4,7 +4,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::armem::server
{
@@ -14,16 +13,20 @@ namespace armarx::armem::server
groupBox.addChild(layout);
}
- bool RemoteGuiAronDataVisitor::visitEnter(const std::string& key, const std::string& type, size_t size)
+ bool
+ RemoteGuiAronDataVisitor::visitEnter(const std::string& key,
+ const std::string& type,
+ size_t size)
{
std::stringstream label;
label << key << " (" << type << " of size " << size << ")";
Group& group = groups.emplace(label.str());
- (void) group;
+ (void)group;
return true;
}
- bool RemoteGuiAronDataVisitor::visitExit()
+ bool
+ RemoteGuiAronDataVisitor::visitExit()
{
Group group = groups.top();
groups.pop();
@@ -38,23 +41,23 @@ namespace armarx::armem::server
return true;
}
- void RemoteGuiAronDataVisitor::streamValueText(aron::data::String& n, std::stringstream& ss)
+ void
+ RemoteGuiAronDataVisitor::streamValueText(aron::data::String& n, std::stringstream& ss)
{
ss << "'" << n.getValue() << "'";
}
- void RemoteGuiAronDataVisitor::streamValueText(aron::data::NDArray& n, std::stringstream& ss)
+ void
+ RemoteGuiAronDataVisitor::streamValueText(aron::data::NDArray& n, std::stringstream& ss)
{
ss << "shape (" << simox::alg::join(simox::alg::multi_to_string(n.getShape()), ", ") << ")";
}
- void RemoteGuiAronDataVisitor::checkGroupsNotEmpty() const
+ void
+ RemoteGuiAronDataVisitor::checkGroupsNotEmpty() const
{
ARMARX_CHECK_POSITIVE(groups.size()) << "Groups must not be empty.";
}
-
-
-
-}
+} // namespace armarx::armem::server
diff --git a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
index d77f34eb719b46c117c64a2e9ec09cd43780dad9..b988066dec4908dbf62b522b4dd234dcbe4dd6d7 100644
--- a/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
+++ b/source/RobotAPI/libraries/armem/server/RemoteGuiAronDataVisitor.h
@@ -10,18 +10,15 @@
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
-
namespace armarx::armem::server
{
- struct RemoteGuiAronDataVisitor :
- public aron::data::RecursiveConstVariantVisitor
+ struct RemoteGuiAronDataVisitor : public aron::data::RecursiveConstVariantVisitor
{
using GroupBox = armarx::RemoteGui::Client::GroupBox;
using GridLayout = armarx::RemoteGui::Client::GridLayout;
using Label = armarx::RemoteGui::Client::Label;
-
struct Group
{
Group(const std::string& label = {});
@@ -37,24 +34,31 @@ namespace armarx::armem::server
virtual ~RemoteGuiAronDataVisitor() = default;
- void visitDictOnEnter(const aron::data::VariantPtr& n) override
+ void
+ visitDictOnEnter(const aron::data::VariantPtr& n) override
{
ARMARX_CHECK_NOT_NULL(n);
- const std::string key = n->getPath().size() > 0 ? n->getPath().getLastElement() : ""; // check if root of object
+ const std::string key = n->getPath().size() > 0 ? n->getPath().getLastElement()
+ : ""; // check if root of object
visitEnter(key, "dict", n->childrenSize());
}
- void visitDictOnExit(const aron::data::VariantPtr&) override
+
+ void
+ visitDictOnExit(const aron::data::VariantPtr&) override
{
visitExit();
}
- void visitListOnEnter(const aron::data::VariantPtr& n) override
+ void
+ visitListOnEnter(const aron::data::VariantPtr& n) override
{
ARMARX_CHECK_NOT_NULL(n);
const std::string key = n->getPath().getLastElement();
visitEnter(key, "list", n->childrenSize());
}
- void visitListOnExit(const aron::data::VariantPtr&) override
+
+ void
+ visitListOnExit(const aron::data::VariantPtr&) override
{
visitExit();
}
@@ -63,37 +67,48 @@ namespace armarx::armem::server
bool visitEnter(const std::string& key, const std::string& type, size_t size);
bool visitExit();
- void visitBool(const aron::data::VariantPtr& b) override
+ void
+ visitBool(const aron::data::VariantPtr& b) override
{
ARMARX_CHECK_NOT_NULL(b);
const std::string key = b->getPath().getLastElement();
this->addValueLabel(key, *aron::data::Bool::DynamicCastAndCheck(b), "bool");
}
- void visitDouble(const aron::data::VariantPtr& d) override
+
+ void
+ visitDouble(const aron::data::VariantPtr& d) override
{
ARMARX_CHECK_NOT_NULL(d);
const std::string key = d->getPath().getLastElement();
this->addValueLabel(key, *aron::data::Double::DynamicCastAndCheck(d), "double");
}
- void visitFloat(const aron::data::VariantPtr& f) override
+
+ void
+ visitFloat(const aron::data::VariantPtr& f) override
{
ARMARX_CHECK_NOT_NULL(f);
const std::string key = f->getPath().getLastElement();
this->addValueLabel(key, *aron::data::Float::DynamicCastAndCheck(f), "float");
}
- void visitInt(const aron::data::VariantPtr& i) override
+
+ void
+ visitInt(const aron::data::VariantPtr& i) override
{
ARMARX_CHECK_NOT_NULL(i);
const std::string key = i->getPath().getLastElement();
this->addValueLabel(key, *aron::data::Int::DynamicCastAndCheck(i), "int");
}
- void visitLong(const aron::data::VariantPtr& l) override
+
+ void
+ visitLong(const aron::data::VariantPtr& l) override
{
ARMARX_CHECK_NOT_NULL(l);
const std::string key = l->getPath().getLastElement();
this->addValueLabel(key, *aron::data::Long::DynamicCastAndCheck(l), "long");
}
- void visitString(const aron::data::VariantPtr& string) override
+
+ void
+ visitString(const aron::data::VariantPtr& string) override
{
ARMARX_CHECK_NOT_NULL(string);
auto s = aron::data::String::DynamicCastAndCheck(string);
@@ -101,14 +116,16 @@ namespace armarx::armem::server
this->addValueLabel(key, *s, "string");
}
- void visitNDArray(const aron::data::VariantPtr& array) override
+ void
+ visitNDArray(const aron::data::VariantPtr& array) override
{
ARMARX_CHECK_NOT_NULL(array);
const std::string key = array->getPath().getLastElement();
this->addValueLabel(key, *aron::data::NDArray::DynamicCastAndCheck(array), "ND Array");
}
- void visitUnknown(const aron::data::VariantPtr& unknown) override
+ void
+ visitUnknown(const aron::data::VariantPtr& unknown) override
{
ARMARX_IMPORTANT << "Unknown data encountered: "
<< (unknown ? unknown->getFullName() : "nullptr");
@@ -117,20 +134,20 @@ namespace armarx::armem::server
private:
template <class DataT>
- void addValueLabel(const std::string& key, DataT& n, const std::string& typeName)
+ void
+ addValueLabel(const std::string& key, DataT& n, const std::string& typeName)
{
checkGroupsNotEmpty();
Group& g = groups.top();
- g.layout
- .add(Label(key), {g.nextRow, 0})
- .add(Label("(" + typeName + ")"), {g.nextRow, 1})
- .add(Label("= " + getValueText(n)), {g.nextRow, 2})
- ;
+ g.layout.add(Label(key), {g.nextRow, 0})
+ .add(Label("(" + typeName + ")"), {g.nextRow, 1})
+ .add(Label("= " + getValueText(n)), {g.nextRow, 2});
++g.nextRow;
}
template <class DataT>
- std::string getValueText(DataT& n)
+ std::string
+ getValueText(DataT& n)
{
std::stringstream ss;
streamValueText(n, ss);
@@ -138,16 +155,17 @@ namespace armarx::armem::server
}
template <class DataT>
- void streamValueText(DataT& n, std::stringstream& ss)
+ void
+ streamValueText(DataT& n, std::stringstream& ss)
{
ss << n.getValue();
}
+
void streamValueText(aron::data::String& n, std::stringstream& ss);
void streamValueText(aron::data::NDArray& n, std::stringstream& ss);
void checkGroupsNotEmpty() const;
-
};
-}
+} // namespace armarx::armem::server
diff --git a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
index 7fc46a57672e594333de3d178655c326bffce5da..27223eb060f039b912b7a3b71789588d7114427c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/CoreSegment.cpp
@@ -194,7 +194,7 @@ namespace armarx::armem::server::ltm
else
{
ARMARX_DEBUG << "CoreSegment does not have aron type, so aron type information "
- "cannot be exported";
+ "cannot be exported";
/*writeForeignKeyToPreviousDocument();*/
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp b/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
index 7e4bdc4eef7809a7361c01d3b414809734a968fb..1b188c4c32c4385619fee4bbb60a927916b30445 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Entity.cpp
@@ -371,12 +371,7 @@ namespace armarx::armem::server::ltm
int count = 0;
//this is a little bit ugly, TODO: find an easier way to count the snapshots
- forEachSnapshot(
- [&e, &count](auto& x)
- {
- count++;
- }
- );
+ forEachSnapshot([&e, &count](auto& x) { count++; });
int current = 0;
@@ -387,14 +382,19 @@ namespace armarx::armem::server::ltm
x.id()
.timestamp)) // we only load the references if the snapshot is not existant
{
- if (current >= (count - n)){
- ARMARX_INFO << e.id().coreSegmentName << ": count: " << count << ", n: " << n << ", current: " << current;
- ARMARX_DEBUG << "Loading snapshot with timestamp " << x.id().timestamp << " into WM";
+ if (current >= (count - n))
+ {
+ ARMARX_INFO << e.id().coreSegmentName << ": count: " << count
+ << ", n: " << n << ", current: " << current;
+ ARMARX_DEBUG << "Loading snapshot with timestamp " << x.id().timestamp
+ << " into WM";
armem::wm::EntitySnapshot s;
x.loadAllReferences(s);
x.resolve(s);
e.addSnapshot(s);
- } else {
+ }
+ else
+ {
ARMARX_DEBUG << "Skipping snapshot with timestamp " << x.id().timestamp;
}
current++;
@@ -457,7 +457,7 @@ namespace armarx::armem::server::ltm
if (hasSnapshot(snap.id().timestamp))
{
- ARMARX_INFO << "Ignoring to put an EntitiySnapshot into the LTM because "
+ ARMARX_INFO << "Ignoring to put an EntitiySnapshot into the LTM because "
"the timestamp already existed (we assume snapshots are "
"const and do not change outside the ltm).";
return;
@@ -466,17 +466,21 @@ namespace armarx::armem::server::ltm
for (auto& f : processors->snapFilters)
{
bool accepted = f->accept(snap, simulatedVersion);
-
+
if (!accepted)
{
- ARMARX_DEBUG << "Ignoring to put an EntitySnapshot into the LTM because it got filtered.";
+ ARMARX_DEBUG << "Ignoring to put an EntitySnapshot into the LTM because it "
+ "got filtered.";
return;
- } else {
+ }
+ else
+ {
//ARMARX_INFO << "Storing EntitySnapshot";
}
}
- ARMARX_DEBUG << "Snapshot: " << c.id().timestamp << " of coreSegment " << c.id().coreSegmentName;
+ ARMARX_DEBUG << "Snapshot: " << c.id().timestamp << " of coreSegment "
+ << c.id().coreSegmentName;
c.store(snap);
statistics.recordedSnapshots++;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
index ce8932e546b096d2ee30bdc47244fcb59b77f920..e5e944b367e0752c97016f7270bc7947bf4e43d6 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntityInstance.cpp
@@ -149,7 +149,8 @@ namespace armarx::armem::server::ltm
std::shared_ptr<aron::data::Dict> source;
- bool saveAndExtract = false; //if true the data is saved in extracted form and in data.aron.json
+ bool saveAndExtract =
+ false; //if true the data is saved in extracted form and in data.aron.json
// this is needed if several LTMs are recorded at once because otherwise the data from the data.aron.json
// is not there anymore to extract from
diff --git a/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
index 175840a0c29a0b2ab96b24172c4b92ddf289a234..f2c5cc6703c7c02c96088d1c1ae51338d9912128 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/EntitySnapshot.cpp
@@ -111,9 +111,7 @@ namespace armarx::armem::server::ltm
e.id() = id().getEntitySnapshotID().cleanID();
- forEachInstance([&e](armem::server::ltm::EntityInstance& x) {
- x.loadAllReferences(e);
- });
+ forEachInstance([&e](armem::server::ltm::EntityInstance& x) { x.loadAllReferences(e); });
}
void
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
index a950f15410ed4a4283895e896d42856f835c5575..47817858b662ed655482969a24336471edb16eaa 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/Memory.cpp
@@ -100,11 +100,8 @@ namespace armarx::armem::server::ltm
}
else
{
- ARMARX_WARNING << "Could not load the core segments of LTM "
- << id().str()
- << " as the path "
- << getFullPath().string()
- << " does not exist.";
+ ARMARX_WARNING << "Could not load the core segments of LTM " << id().str()
+ << " as the path " << getFullPath().string() << " does not exist.";
}
ARMARX_DEBUG << "All CoreSegments handeled";
@@ -214,17 +211,21 @@ namespace armarx::armem::server::ltm
forEachCoreSegment(
[&m, &n, &coreSegNames](auto& x)
{
- bool loadCoreSeg = (std::find(coreSegNames.begin(), coreSegNames.end(), x.id().coreSegmentName) != coreSegNames.end());
- if(loadCoreSeg){
+ bool loadCoreSeg =
+ (std::find(coreSegNames.begin(), coreSegNames.end(), x.id().coreSegmentName) !=
+ coreSegNames.end());
+ if (loadCoreSeg)
+ {
armem::wm::CoreSegment s;
x.loadLatestNReferences(n, s);
m.addCoreSegment(s);
- } else {
- ARMARX_DEBUG << "Skipping loading CoreSegment with name "
- << x.id().coreSegmentName
+ }
+ else
+ {
+ ARMARX_DEBUG << "Skipping loading CoreSegment with name "
+ << x.id().coreSegmentName
<< " from LTM into WM as it is not in the defined list";
}
-
});
}
@@ -248,8 +249,10 @@ namespace armarx::armem::server::ltm
processors);
c.resolve(e);
});
- } else {
- ARMARX_WARNING << "You are trying to resolve an LTM from a path that does not exist: "
+ }
+ else
+ {
+ ARMARX_WARNING << "You are trying to resolve an LTM from a path that does not exist: "
<< getFullPath();
}
}
@@ -293,7 +296,7 @@ namespace armarx::armem::server::ltm
// 2. store data
c.store(core, simulatedVersion);
-
+
// 3. update statistics
statistics.recordedCoreSegments++;
@@ -304,9 +307,9 @@ namespace armarx::armem::server::ltm
}
void
- Memory::_loadOnStartup(){
+ Memory::_loadOnStartup()
+ {
//not used any more
-
}
void
diff --git a/source/RobotAPI/libraries/armem/server/ltm/Memory.h b/source/RobotAPI/libraries/armem/server/ltm/Memory.h
index bb022c1e2e4e196215eb7288f5dc0a28d70b458a..62dafa951d02ceb2834690c94d8794ab2a221e8c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/Memory.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/Memory.h
@@ -48,7 +48,8 @@ namespace armarx::armem::server::ltm
std::unique_ptr<CoreSegment>
findCoreSegment(const std::string& coreSegmentName) const final;
- void createPropertyDefinitions(PropertyDefinitionsPtr& defs, const std::string& prefix) override;
+ void createPropertyDefinitions(PropertyDefinitionsPtr& defs,
+ const std::string& prefix) override;
/**
* @brief getAndSaveStatistics generates and saves statistics for a LTM recording
@@ -58,7 +59,9 @@ namespace armarx::armem::server::ltm
protected:
void _loadAllReferences(armem::wm::Memory&) final;
void _loadLatestNReferences(int n, armem::wm::Memory& m) final;
- void _loadLatestNReferences(int n, armem::wm::Memory& m, std::list<std::string> coreSegNames) final;
+ void _loadLatestNReferences(int n,
+ armem::wm::Memory& m,
+ std::list<std::string> coreSegNames) final;
void _resolve(armem::wm::Memory&) final;
void _store(const armem::wm::Memory& m) final;
void _directlyStore(const armem::wm::Memory& m, bool simulatedVersion) final;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
index 7d2cabfc613dea511df6bf91e8b76a12a8a770cd..d104e2398cfa57d0be3e22293f850c78f75fedc7 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/ProviderSegment.cpp
@@ -193,7 +193,8 @@ namespace armarx::armem::server::ltm
}
else
{
- ARMARX_DEBUG << "ProviderSegment does not seem to have an aron type, so aron type information connot be exported";
+ ARMARX_DEBUG << "ProviderSegment does not seem to have an aron type, so aron type "
+ "information connot be exported";
//writeForeignKeyToPreviousDocument();
}
@@ -209,6 +210,6 @@ namespace armarx::armem::server::ltm
c.store(e, simulatedVersion);
statistics.recordedEntities++;
});
- }
+ }
} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.cpp
index bb0860e4228ac731fb03e67f9b05befcab0c5059..d350786c4bd394c439d4afcf686d0f4ab24e2e9a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.cpp
@@ -3,4 +3,4 @@
namespace armarx::armem::server::ltm::detail
{
-} // namespace armarx::armem::server::ltm
+} // namespace armarx::armem::server::ltm::detail
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.h
index ce70f089d74cb7beae72b037449676f087c4befd..d8155579a1852736226f16b8880952103dd9b91c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/CoreSegmentBase.h
@@ -95,7 +95,7 @@ namespace armarx::armem::server::ltm::detail
virtual void _loadAllReferences(armem::wm::CoreSegment&) = 0;
virtual void _loadLatestNReferences(int n, armem::wm::CoreSegment& c) = 0;
virtual void _resolve(armem::wm::CoreSegment&) = 0;
- virtual void _store(const armem::wm::CoreSegment& c, bool simulatedVersion=false) = 0;
+ virtual void _store(const armem::wm::CoreSegment& c, bool simulatedVersion = false) = 0;
protected:
mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.cpp
index 8b2d05c28d61ede50f8bbe9643d4fc3d05c56be1..8b035aceea7a1918e56175dbb5c29b49349839e0 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.cpp
@@ -7,4 +7,4 @@
namespace armarx::armem::server::ltm::detail
{
-} // namespace armarx::armem::server::ltm
+} // namespace armarx::armem::server::ltm::detail
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.h
index 3de97348618f77988bf87f92d5c97c2508f08fb5..5ae045c8459feb3ba634b52a5e1fd6cfe024905f 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/EntityBase.h
@@ -108,7 +108,7 @@ namespace armarx::armem::server::ltm::detail
virtual void _loadAllReferences(armem::wm::Entity&) = 0;
virtual void _loadLatestNReferences(int n, armem::wm::Entity& e) = 0;
virtual void _resolve(armem::wm::Entity&) = 0;
- virtual void _store(const armem::wm::Entity& e, bool simulatedVersion=false) = 0;
+ virtual void _store(const armem::wm::Entity& e, bool simulatedVersion = false) = 0;
protected:
mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
index 708926591ad0bc91eec9b259100dad43eba08dee..039aaefd930a5c9ce439f38ca07a39f3601d0c64 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryBase.h
@@ -30,7 +30,7 @@ namespace armarx::armem::server::ltm
CONSOLIDATE_ALL, // store whats in the WM (on commit)
CONSOLIDATE_LATEST // store the latest snapshots at a fixed frequency (for now 1Hz)
};
-}
+} // namespace armarx::armem::server::ltm
namespace armarx::armem::server::ltm::detail
{
@@ -39,8 +39,6 @@ namespace armarx::armem::server::ltm::detail
class MemoryBase : public MemoryItem
{
public:
-
-
struct Statistics
{
armarx::core::time::DateTime lastEnabled = armarx::core::time::DateTime::Invalid();
@@ -61,7 +59,8 @@ namespace armarx::armem::server::ltm::detail
{
}
- void setRecordingMode(const std::string& m)
+ void
+ setRecordingMode(const std::string& m)
{
if (m == "CONSOLIDATE_REMOVED")
{
@@ -81,12 +80,14 @@ namespace armarx::armem::server::ltm::detail
}
}
- void setRecordingMode(const RecordingMode m)
+ void
+ setRecordingMode(const RecordingMode m)
{
this->recordingMode = m;
}
- RecordingMode getRecordingMode() const
+ RecordingMode
+ getRecordingMode() const
{
return recordingMode;
}
@@ -181,7 +182,8 @@ namespace armarx::armem::server::ltm::detail
//return the newest part of the ltm as a wm::Memory with only references
//will return the newest n snapshots of each entity
armem::wm::Memory
- loadLatestNReferences(int n){
+ loadLatestNReferences(int n)
+ {
armem::wm::Memory ret;
loadLatestNReferences(n, ret);
return ret;
@@ -190,21 +192,21 @@ namespace armarx::armem::server::ltm::detail
//return the newest part of the ltm as a wm::Memory with only references
//will return the newest n snapshots of each entity for only the CoreSegments matching one of the name sin the list
void
- loadLatestNReferences(int n, armem::wm::Memory& memory, std::list<std::string> coreSegNames){
+ loadLatestNReferences(int n, armem::wm::Memory& memory, std::list<std::string> coreSegNames)
+ {
TIMING_START(LTM_Memory_loadNewestn_someCoreSegs);
_loadLatestNReferences(n, memory, coreSegNames);
TIMING_END_STREAM(LTM_Memory_loadNewestn_someCoreSegs, ARMARX_DEBUG);
}
- void
- loadLatestNReferences(int n, armem::wm::Memory& memory){
+ void
+ loadLatestNReferences(int n, armem::wm::Memory& memory)
+ {
TIMING_START(LTM_Memory_loadNewestn);
_loadLatestNReferences(n, memory);
TIMING_END_STREAM(LTM_Memory_loadNewestn, ARMARX_DEBUG);
}
-
-
/// return the full ltm as a wm::Memory and resolves the references
/// the ltm may be huge, use with caution
armem::wm::Memory
@@ -351,7 +353,8 @@ namespace armarx::armem::server::ltm::detail
}
void
- loadOnStartup(){
+ loadOnStartup()
+ {
_loadOnStartup();
}
@@ -381,10 +384,13 @@ namespace armarx::armem::server::ltm::detail
virtual void _loadAllReferences(armem::wm::Memory& memory) = 0;
virtual void _resolve(armem::wm::Memory& memory) = 0;
virtual void _store(const armem::wm::Memory& memory) = 0;
- virtual void _directlyStore(const armem::wm::Memory& memory, bool simulatedVersion=false) = 0;
+ virtual void _directlyStore(const armem::wm::Memory& memory,
+ bool simulatedVersion = false) = 0;
virtual void _loadOnStartup() = 0;
virtual void _loadLatestNReferences(int n, armem::wm::Memory& memory) = 0;
- virtual void _loadLatestNReferences(int n, armem::wm::Memory& memory, std::list<std::string> coreSegNames) = 0;
+ virtual void _loadLatestNReferences(int n,
+ armem::wm::Memory& memory,
+ std::list<std::string> coreSegNames) = 0;
public:
// stuff for scenario parameters
@@ -399,16 +405,17 @@ namespace armarx::armem::server::ltm::detail
// Name and export path (TODO: Should this be part of the base class? Export path is a disk memory thing)
std::string export_name = "MemoryExport";
std::string export_path = "/tmp/ltm";
- bool storeOnStop = false;
+ bool storeOnStop = false;
- // TODO: belong more to WM, but no good solution for that currently:
+ // TODO: belong more to WM, but no good solution for that currently:
bool importOnStartUp = false;
int maxAmountOfSnapshotsLoaded = 1;
std::string coreSegmentsToLoad = "";
// Other configuration
std::string configuration_on_startup =
- "{ \"SnapshotFrequencyFilter\": {\"WaitingTimeInMsForFilter\" : 50}}"; //record with 20 fps as standard
+ "{ \"SnapshotFrequencyFilter\": "
+ "{\"WaitingTimeInMsForFilter\" : 50}}"; //record with 20 fps as standard
} p;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.cpp
index 29061d0eed8efc5ab8298daa5f84bb4f572bfd8e..99dfcfee345d5d2015baffa7ef389a0c6879b3f9 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.cpp
@@ -26,8 +26,6 @@ namespace armarx::armem::server::ltm::detail
_setExportName(id);
}
-
-
void
MemoryItem::setMemoryID(const MemoryID& id)
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h
index 821f131bb10b628380359b601aac4d02ff5dbcb7..e82c29aa99ab4f9a89d30831516340c4f3a40d1e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/MemoryItem.h
@@ -23,12 +23,12 @@ namespace armarx::armem::server::ltm::detail
void setMemoryID(const MemoryID&);
void setMemoryName(const std::string& memoryName);
- virtual std::string getExportName() const
+ virtual std::string
+ getExportName() const
{
return exportName;
}
-
MemoryID
getMemoryID() const
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
index 91f853f92a44cf227f11569bca4442837d11ac8a..2a6b2121a3c8c083c724ff3b9c456305fab7230e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/ProviderSegmentBase.h
@@ -34,7 +34,8 @@ namespace armarx::armem::server::ltm::detail
_loadAllReferences(provSeg);
}
- void loadLatestNReferences(int n, armem::wm::ProviderSegment& provSeg)
+ void
+ loadLatestNReferences(int n, armem::wm::ProviderSegment& provSeg)
{
_loadLatestNReferences(n, provSeg);
}
@@ -76,7 +77,8 @@ namespace armarx::armem::server::ltm::detail
virtual std::shared_ptr<EntityT> findEntity(const std::string&) const = 0;
///get aron type
- aron::type::ObjectPtr aronType() const
+ aron::type::ObjectPtr
+ aronType() const
{
return nullptr;
}
@@ -91,7 +93,7 @@ namespace armarx::armem::server::ltm::detail
virtual void _loadAllReferences(armem::wm::ProviderSegment&) = 0;
virtual void _loadLatestNReferences(int n, armem::wm::ProviderSegment& p) = 0;
virtual void _resolve(armem::wm::ProviderSegment&) = 0;
- virtual void _store(const armem::wm::ProviderSegment& p, bool simulatedVersion=false) = 0;
+ virtual void _store(const armem::wm::ProviderSegment& p, bool simulatedVersion = false) = 0;
protected:
mutable std::recursive_mutex ltm_mutex;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.cpp
index 80982baf62c2bd7e20e0e7960deac373831c9614..c674d997b4d03eef2670ffabdac44c31474b201a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.cpp
@@ -4,4 +4,4 @@ namespace armarx::armem::server::ltm::detail::mixin
{
-} // namespace armarx::armem::server::ltm
+} // namespace armarx::armem::server::ltm::detail::mixin
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
index 2f3ab8022851c7fa5a66ef3d263b6d4b0189e3c6..d1f8f60388ae11e4f23b0a8888da609e8bc4e232 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/BufferedMemoryMixin.h
@@ -28,7 +28,7 @@ namespace armarx::armem::server::ltm::detail::mixin
virtual ~BufferedMemoryMixin() = default;
void
- directlyStore(const armem::wm::Memory& memory, bool simulatedVersion=false)
+ directlyStore(const armem::wm::Memory& memory, bool simulatedVersion = false)
{
std::lock_guard l(storeMutex);
@@ -38,7 +38,7 @@ namespace armarx::armem::server::ltm::detail::mixin
}
void
- directlyStore(const armem::server::wm::Memory& serverMemory, bool simulatedVersion=false)
+ directlyStore(const armem::server::wm::Memory& serverMemory, bool simulatedVersion = false)
{
armem::wm::Memory memory;
memory.setName(serverMemory.name());
@@ -86,7 +86,6 @@ namespace armarx::armem::server::ltm::detail::mixin
task->stop();
task = nullptr;
}
-
}
armem::wm::Memory
@@ -138,7 +137,8 @@ namespace armarx::armem::server::ltm::detail::mixin
defs->optional(storeFrequency, prefix + "storeFrequency");
}
- virtual void _directlyStore(const armem::wm::Memory& memory, bool simulatedVersion=false) = 0;
+ virtual void _directlyStore(const armem::wm::Memory& memory,
+ bool simulatedVersion = false) = 0;
void
addToBuffer(const armem::wm::Memory& memory)
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
index a7fbaa4700d3b6908940cfa7185236254ec71ff7..83f1e87f6aa53e9dd56891eaf8d05bb88b719d75 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/DiskStorageMixin.cpp
@@ -26,7 +26,7 @@ namespace armarx::armem::server::ltm::detail::mixin
Path
DiskMemoryItemMixin::addDateToMemoryBasePath(const Path& n) const
- {
+ {
//set path to include date of creation as prefix:
auto current_date = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
std::tm* localTime = std::localtime(¤t_date);
@@ -70,11 +70,13 @@ namespace armarx::armem::server::ltm::detail::mixin
Path
DiskMemoryItemMixin::getMemoryBasePath() const
{
- std::string p = [&](){
- if(memoryBasePathString.empty()){
+ std::string p = [&]()
+ {
+ if (memoryBasePathString.empty())
+ {
return memoryBasePath.string();
- }
-
+ }
+
return memoryBasePathString;
}();
@@ -90,7 +92,8 @@ namespace armarx::armem::server::ltm::detail::mixin
//p = this->addDateToMemoryBasePath(p);
//ARMARX_INFO << VAROUT(_id);
//ARMARX_INFO << VAROUT(_id.cleanID());
- auto cleanID = _id.cleanID(); //somehow, the iDs are jumbled when loading the LTM from disk, this solves it for now
+ auto cleanID =
+ _id.cleanID(); //somehow, the iDs are jumbled when loading the LTM from disk, this solves it for now
auto fullPath = util::fs::toPath(p, cleanID);
@@ -170,7 +173,9 @@ namespace armarx::armem::server::ltm::detail::mixin
return util::fs::getAllFiles(p);
}
- void DiskMemoryItemMixin::createPropertyDefinitions(PropertyDefinitionsPtr &defs, const std::string &prefix)
+ void
+ DiskMemoryItemMixin::createPropertyDefinitions(PropertyDefinitionsPtr& defs,
+ const std::string& prefix)
{
defs->optional(memoryBasePathString, prefix + "exportPath");
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.cpp b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.cpp
index bb8f36bb24159e90bbef87825d5a0b80e97cf3f7..d2bafc90f4807223266577ba2dac5aff66b7be76 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/filesystem.cpp
@@ -40,14 +40,17 @@ namespace armarx::armem::server::ltm ::util::fs
return ret;
}
- std::string
+ std::string
extractLastDirectoryFromPath(const std::string& path)
{
size_t pos = path.rfind('/');
-
- if (pos != std::string::npos) {
+
+ if (pos != std::string::npos)
+ {
return path.substr(pos + 1);
- } else {
+ }
+ else
+ {
return path;
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h
index 5cb419cc05ccc2475bc1ae414b9303dbef25647c..9908e6c5b7f379be2ab7207a28610183c87395f5 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/detail/mixins/util/mongodb.h
@@ -8,16 +8,15 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
+#include <bsoncxx/builder/stream/array.hpp>
+#include <bsoncxx/builder/stream/document.hpp>
+#include <bsoncxx/builder/stream/helpers.hpp>
+#include <bsoncxx/json.hpp>
#include <mongocxx/client.hpp>
#include <mongocxx/instance.hpp>
#include <mongocxx/pool.hpp>
#include <mongocxx/stdx.hpp>
#include <mongocxx/uri.hpp>
-#include <bsoncxx/builder/stream/array.hpp>
-#include <bsoncxx/builder/stream/document.hpp>
-#include <bsoncxx/builder/stream/helpers.hpp>
-#include <bsoncxx/json.hpp>
namespace armarx::armem::server::ltm::util::mongodb
{
diff --git a/source/RobotAPI/libraries/armem/server/ltm/io/Recording.cpp b/source/RobotAPI/libraries/armem/server/ltm/io/Recording.cpp
index 265a4f0f94c7463223985277f20e337b281220cc..085a352b3f91fec5762351e7cc7a13db8038c127 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/io/Recording.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/io/Recording.cpp
@@ -2,8 +2,8 @@
#include <ArmarXCore/core/ice_conversions.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
-
#include <ArmarXCore/core/time/ice_conversions.h>
+
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
namespace armarx::armem::server::ltm
@@ -14,19 +14,26 @@ namespace armarx::armem::server::ltm
/* ===========================
* DirectlyStoreInput
* =========================== */
- DirectlyStoreInput DirectlyStoreInput::fromIce(const dto::DirectlyStoreInput& ice)
+ DirectlyStoreInput
+ DirectlyStoreInput::fromIce(const dto::DirectlyStoreInput& ice)
{
return armarx::fromIce<DirectlyStoreInput>(ice);
}
- dto::DirectlyStoreInput DirectlyStoreInput::toIce() const
+
+ dto::DirectlyStoreInput
+ DirectlyStoreInput::toIce() const
{
return armarx::toIce<dto::DirectlyStoreInput>(*this);
}
- void toIce(dto::DirectlyStoreInput& ice, const DirectlyStoreInput& result)
+
+ void
+ toIce(dto::DirectlyStoreInput& ice, const DirectlyStoreInput& result)
{
toIce(ice.memory, result.memory);
}
- void fromIce(const dto::DirectlyStoreInput& ice, DirectlyStoreInput& result)
+
+ void
+ fromIce(const dto::DirectlyStoreInput& ice, DirectlyStoreInput& result)
{
fromIce(ice.memory, result.memory);
}
@@ -34,19 +41,26 @@ namespace armarx::armem::server::ltm
/* ===========================
* DirectlyStoreResult
* =========================== */
- DirectlyStoreResult DirectlyStoreResult::fromIce(const dto::DirectlyStoreResult& ice)
+ DirectlyStoreResult
+ DirectlyStoreResult::fromIce(const dto::DirectlyStoreResult& ice)
{
return armarx::fromIce<DirectlyStoreResult>(ice);
}
- dto::DirectlyStoreResult DirectlyStoreResult::toIce() const
+
+ dto::DirectlyStoreResult
+ DirectlyStoreResult::toIce() const
{
return armarx::toIce<dto::DirectlyStoreResult>(*this);
}
- void toIce(dto::DirectlyStoreResult& ice, const DirectlyStoreResult& result)
+
+ void
+ toIce(dto::DirectlyStoreResult& ice, const DirectlyStoreResult& result)
{
toIce(ice, dynamic_cast<const detail::SuccessHeader&>(result));
}
- void fromIce(const dto::DirectlyStoreResult& ice, DirectlyStoreResult& result)
+
+ void
+ fromIce(const dto::DirectlyStoreResult& ice, DirectlyStoreResult& result)
{
fromIce(ice, dynamic_cast<detail::SuccessHeader&>(result));
}
@@ -54,15 +68,20 @@ namespace armarx::armem::server::ltm
/* ===========================
* DirectlyStoreInput
* =========================== */
- StartRecordInput StartRecordInput::fromIce(const dto::StartRecordInput& ice)
+ StartRecordInput
+ StartRecordInput::fromIce(const dto::StartRecordInput& ice)
{
return armarx::fromIce<StartRecordInput>(ice);
}
- dto::StartRecordInput StartRecordInput::toIce() const
+
+ dto::StartRecordInput
+ StartRecordInput::toIce() const
{
return armarx::toIce<dto::StartRecordInput>(*this);
}
- void toIce(dto::StartRecordInput& ice, const StartRecordInput& result)
+
+ void
+ toIce(dto::StartRecordInput& ice, const StartRecordInput& result)
{
toIce(ice.executionTime, result.executionTime);
ice.recordingID = result.recordingID;
@@ -79,7 +98,9 @@ namespace armarx::armem::server::ltm
ice.configuration[id] = c;
}
}
- void fromIce(const dto::StartRecordInput& ice, StartRecordInput& result)
+
+ void
+ fromIce(const dto::StartRecordInput& ice, StartRecordInput& result)
{
fromIce(ice.executionTime, result.executionTime);
result.recordingID = ice.recordingID;
@@ -100,19 +121,26 @@ namespace armarx::armem::server::ltm
/* ===========================
* StartRecordResult
* =========================== */
- StartRecordResult StartRecordResult::fromIce(const dto::StartRecordResult& ice)
+ StartRecordResult
+ StartRecordResult::fromIce(const dto::StartRecordResult& ice)
{
return armarx::fromIce<StartRecordResult>(ice);
}
- dto::StartRecordResult StartRecordResult::toIce() const
+
+ dto::StartRecordResult
+ StartRecordResult::toIce() const
{
return armarx::toIce<dto::StartRecordResult>(*this);
}
- void toIce(dto::StartRecordResult& ice, const StartRecordResult& result)
+
+ void
+ toIce(dto::StartRecordResult& ice, const StartRecordResult& result)
{
toIce(ice, dynamic_cast<const detail::SuccessHeader&>(result));
}
- void fromIce(const dto::StartRecordResult& ice, StartRecordResult& result)
+
+ void
+ fromIce(const dto::StartRecordResult& ice, StartRecordResult& result)
{
fromIce(ice, dynamic_cast<detail::SuccessHeader&>(result));
}
@@ -120,19 +148,26 @@ namespace armarx::armem::server::ltm
/* ===========================
* StopRecordResult
* =========================== */
- StopRecordResult StopRecordResult::fromIce(const dto::StopRecordResult& ice)
+ StopRecordResult
+ StopRecordResult::fromIce(const dto::StopRecordResult& ice)
{
return armarx::fromIce<StopRecordResult>(ice);
}
- dto::StopRecordResult StopRecordResult::toIce() const
+
+ dto::StopRecordResult
+ StopRecordResult::toIce() const
{
return armarx::toIce<dto::StopRecordResult>(*this);
}
- void toIce(dto::StopRecordResult& ice, const StopRecordResult& result)
+
+ void
+ toIce(dto::StopRecordResult& ice, const StopRecordResult& result)
{
toIce(ice, dynamic_cast<const detail::SuccessHeader&>(result));
}
- void fromIce(const dto::StopRecordResult& ice, StopRecordResult& result)
+
+ void
+ fromIce(const dto::StopRecordResult& ice, StopRecordResult& result)
{
fromIce(ice, dynamic_cast<detail::SuccessHeader&>(result));
}
@@ -140,19 +175,26 @@ namespace armarx::armem::server::ltm
/* ===========================
* RecordStatusResult
* =========================== */
- RecordStatusResult RecordStatusResult::fromIce(const dto::RecordStatusResult& ice)
+ RecordStatusResult
+ RecordStatusResult::fromIce(const dto::RecordStatusResult& ice)
{
return armarx::fromIce<RecordStatusResult>(ice);
}
- dto::RecordStatusResult RecordStatusResult::toIce() const
+
+ dto::RecordStatusResult
+ RecordStatusResult::toIce() const
{
return armarx::toIce<dto::RecordStatusResult>(*this);
}
- void toIce(dto::RecordStatusResult& ice, const RecordStatusResult& result)
+
+ void
+ toIce(dto::RecordStatusResult& ice, const RecordStatusResult& result)
{
toIce(ice, dynamic_cast<const detail::SuccessHeader&>(result));
}
- void fromIce(const dto::RecordStatusResult& ice, RecordStatusResult& result)
+
+ void
+ fromIce(const dto::RecordStatusResult& ice, RecordStatusResult& result)
{
fromIce(ice, dynamic_cast<detail::SuccessHeader&>(result));
}
@@ -178,4 +220,4 @@ namespace armarx::armem::server::ltm
fromIce(ice, dynamic_cast<detail::SuccessHeader&>(result));
fromIce(ice.serverStructure, result.serverStructure);
}*/
-}
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/io/Recording.h b/source/RobotAPI/libraries/armem/server/ltm/io/Recording.h
index 1bdd4d0586c3bafc7806852868380ecd30692be3..e240592cd15ec8e23730bf749f71a132548ff8df 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/io/Recording.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/io/Recording.h
@@ -4,7 +4,6 @@
#include <RobotAPI/libraries/armem/core/SuccessHeader.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-
namespace armarx::armem::server::ltm
{
@@ -18,11 +17,10 @@ namespace armarx::armem::server::ltm
static DirectlyStoreInput fromIce(const dto::DirectlyStoreInput& ice);
dto::DirectlyStoreInput toIce() const;
};
+
void toIce(dto::DirectlyStoreInput& ice, const DirectlyStoreInput& input);
void fromIce(const dto::DirectlyStoreInput& ice, DirectlyStoreInput& input);
-
-
/* ===========================
* DirectlyStoreResult
* =========================== */
@@ -35,11 +33,10 @@ namespace armarx::armem::server::ltm
static DirectlyStoreResult fromIce(const dto::DirectlyStoreResult& ice);
dto::DirectlyStoreResult toIce() const;
};
+
void toIce(dto::DirectlyStoreResult& ice, const DirectlyStoreResult& input);
void fromIce(const dto::DirectlyStoreResult& ice, DirectlyStoreResult& input);
-
-
/* ===========================
* StartRecordInput
* =========================== */
@@ -58,11 +55,10 @@ namespace armarx::armem::server::ltm
static StartRecordInput fromIce(const dto::StartRecordInput& ice);
dto::StartRecordInput toIce() const;
};
+
void toIce(dto::StartRecordInput& ice, const StartRecordInput& input);
void fromIce(const dto::StartRecordInput& ice, StartRecordInput& input);
-
-
/* ===========================
* StartRecordResult
* =========================== */
@@ -71,11 +67,10 @@ namespace armarx::armem::server::ltm
static StartRecordResult fromIce(const dto::StartRecordResult& ice);
dto::StartRecordResult toIce() const;
};
+
void toIce(dto::StartRecordResult& ice, const StartRecordResult& input);
void fromIce(const dto::StartRecordResult& ice, StartRecordResult& input);
-
-
/* ===========================
* StopRecordResult
* =========================== */
@@ -84,11 +79,10 @@ namespace armarx::armem::server::ltm
static StopRecordResult fromIce(const dto::StopRecordResult& ice);
dto::StopRecordResult toIce() const;
};
+
void toIce(dto::StopRecordResult& ice, const StopRecordResult& input);
void fromIce(const dto::StopRecordResult& ice, StopRecordResult& input);
-
-
/* ===========================
* RecordStatusResult
* =========================== */
@@ -103,6 +97,7 @@ namespace armarx::armem::server::ltm
static RecordStatusResult fromIce(const dto::RecordStatusResult& ice);
dto::RecordStatusResult toIce() const;
};
+
void toIce(dto::RecordStatusResult& ice, const RecordStatusResult& input);
void fromIce(const dto::RecordStatusResult& ice, RecordStatusResult& input);
-}
+} // namespace armarx::armem::server::ltm
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.cpp b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.cpp
index 11414c6a21e518a17b7bb1e2925ba60efbf65dfe..ca426e5f94630fa09ec1b0e78d0f197cbdd96ca1 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.cpp
@@ -1,8 +1,8 @@
#include "ConnectionManager.h"
-#include <iostream>
-#include <fstream>
#include <algorithm>
+#include <fstream>
+#include <iostream>
namespace armarx::armem::server::ltm::mongodb
{
@@ -17,14 +17,14 @@ namespace armarx::armem::server::ltm::mongodb
{
armarx_home = getenv("ARMARX_DEFAULTS_DIR");
}
- std::ifstream cFile (armarx_home + "/default.cfg");
+ std::ifstream cFile(armarx_home + "/default.cfg");
if (cFile.is_open())
{
std::string line;
- while(getline(cFile, line))
+ while (getline(cFile, line))
{
line.erase(std::remove_if(line.begin(), line.end(), isspace), line.end());
- if(line[0] == '#' || line.empty())
+ if (line[0] == '#' || line.empty())
{
continue;
}
@@ -37,7 +37,7 @@ namespace armarx::armem::server::ltm::mongodb
}
if (name == "ArmarX.MongoPort")
{
- port = (unsigned int) std::stoi(value);
+ port = (unsigned int)std::stoi(value);
}
if (name == "ArmarX.MongoUser")
{
@@ -51,13 +51,15 @@ namespace armarx::armem::server::ltm::mongodb
}
}
- bool ConnectionManager::MongoDBSettings::isSet() const
+ bool
+ ConnectionManager::MongoDBSettings::isSet() const
{
// we always need a host and a port
return !host.empty() and port != 0;
}
- std::string ConnectionManager::MongoDBSettings::baseUri() const
+ std::string
+ ConnectionManager::MongoDBSettings::baseUri() const
{
std::stringstream ss;
ss << "mongodb://";
@@ -75,25 +77,32 @@ namespace armarx::armem::server::ltm::mongodb
return ss.str();
}
- std::string ConnectionManager::MongoDBSettings::key() const
+ std::string
+ ConnectionManager::MongoDBSettings::key() const
{
// TODO: What happens if a connection exists and you would like to open another one with a different user (e.g. that sees different things)?
return "mongodb://" + host + ":" + std::to_string(port);
}
- std::string ConnectionManager::MongoDBSettings::uri() const
+ std::string
+ ConnectionManager::MongoDBSettings::uri() const
{
- return baseUri() + ":" + std::to_string(port) + "/?minPoolSize=" + std::to_string(minPoolSize) + "&maxPoolSize=" + std::to_string(maxPoolSize);
+ return baseUri() + ":" + std::to_string(port) +
+ "/?minPoolSize=" + std::to_string(minPoolSize) +
+ "&maxPoolSize=" + std::to_string(maxPoolSize);
}
- std::string ConnectionManager::MongoDBSettings::toString() const
+ std::string
+ ConnectionManager::MongoDBSettings::toString() const
{
return uri() + "&database=" + database;
}
- void ConnectionManager::initialize_if()
+ void
+ ConnectionManager::initialize_if()
{
- std::lock_guard l(initializationMutex); // all others have to wait until the initialization is complete
+ std::lock_guard l(
+ initializationMutex); // all others have to wait until the initialization is complete
if (!initialized)
{
initialized = true;
@@ -101,7 +110,8 @@ namespace armarx::armem::server::ltm::mongodb
}
}
- mongocxx::pool& ConnectionManager::Connect(const MongoDBSettings& settings)
+ mongocxx::pool&
+ ConnectionManager::Connect(const MongoDBSettings& settings)
{
initialize_if();
@@ -121,7 +131,8 @@ namespace armarx::armem::server::ltm::mongodb
}
}
- bool ConnectionManager::ConnectionIsValid(const MongoDBSettings& settings, bool forceNewConnection)
+ bool
+ ConnectionManager::ConnectionIsValid(const MongoDBSettings& settings, bool forceNewConnection)
{
initialize_if();
@@ -134,7 +145,8 @@ namespace armarx::armem::server::ltm::mongodb
{
auto client = it->second->acquire();
auto admin = client->database("admin");
- auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
+ auto result = admin.run_command(bsoncxx::builder::basic::make_document(
+ bsoncxx::builder::basic::kvp("isMaster", 1)));
return true;
}
}
@@ -142,7 +154,8 @@ namespace armarx::armem::server::ltm::mongodb
mongocxx::uri uri(settings.uri());
auto client = mongocxx::client(uri);
auto admin = client["admin"];
- auto result = admin.run_command(bsoncxx::builder::basic::make_document(bsoncxx::builder::basic::kvp("isMaster", 1)));
+ auto result = admin.run_command(bsoncxx::builder::basic::make_document(
+ bsoncxx::builder::basic::kvp("isMaster", 1)));
return true;
}
catch (const std::exception& xcp)
@@ -151,11 +164,12 @@ namespace armarx::armem::server::ltm::mongodb
}
}
- bool ConnectionManager::ConnectionExists(const MongoDBSettings& settings)
+ bool
+ ConnectionManager::ConnectionExists(const MongoDBSettings& settings)
{
initialize_if();
auto it = Connections.find(settings.key());
return it != Connections.end();
}
-}
+} // namespace armarx::armem::server::ltm::mongodb
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.h b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.h
index 51acb7a0c7848b944beef878b969f54d5c141f1a..ee5cde45111be2772f148965691708d9e21dada4 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/ConnectionManager.h
@@ -1,21 +1,20 @@
#pragma once
-#include <string>
-#include <mutex>
#include <map>
#include <memory>
+#include <mutex>
#include <sstream>
+#include <string>
+#include <bsoncxx/builder/stream/array.hpp>
+#include <bsoncxx/builder/stream/document.hpp>
+#include <bsoncxx/builder/stream/helpers.hpp>
#include <bsoncxx/json.hpp>
#include <mongocxx/client.hpp>
+#include <mongocxx/instance.hpp>
#include <mongocxx/pool.hpp>
#include <mongocxx/stdx.hpp>
#include <mongocxx/uri.hpp>
-#include <mongocxx/instance.hpp>
-#include <bsoncxx/builder/stream/helpers.hpp>
-#include <bsoncxx/builder/stream/document.hpp>
-#include <bsoncxx/builder/stream/array.hpp>
-
namespace armarx::armem::server::ltm::mongodb
{
@@ -54,7 +53,8 @@ namespace armarx::armem::server::ltm::mongodb
};
static mongocxx::pool& Connect(const MongoDBSettings& settings);
- static bool ConnectionIsValid(const MongoDBSettings& settings, bool forceNewConnection = false);
+ static bool ConnectionIsValid(const MongoDBSettings& settings,
+ bool forceNewConnection = false);
static bool ConnectionExists(const MongoDBSettings& settings);
private:
@@ -65,6 +65,5 @@ namespace armarx::armem::server::ltm::mongodb
static std::mutex initializationMutex;
static bool initialized;
static std::map<std::string, std::unique_ptr<mongocxx::pool>> Connections;
-
};
-}
+} // namespace armarx::armem::server::ltm::mongodb
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.cpp b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.cpp
index 2381297ee3a8311497b175c09372d6776cd1bf3b..63eaec4d5f706f7d54f5fc8cb6b99ac0472cad3a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.cpp
@@ -5,12 +5,12 @@
#include <SimoxUtility/json.h>
// ArmarX
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include "operations.h"
@@ -19,13 +19,15 @@ namespace armarx::armem::server::ltm::mongodb
namespace bsoncxxbuilder = bsoncxx::builder::stream;
namespace bsoncxxdoc = bsoncxx::document;
- PoolClientPtr Memory::checkConnection() const
+ PoolClientPtr
+ Memory::checkConnection() const
{
// Check connection:
if (!ConnectionManager::ConnectionIsValid(dbsettings))
{
ARMARX_WARNING << deactivateSpam("LTM_ConnectionError_" + cache.name())
- << "The connection to mongocxx for ltm '" << cache.name() << "' is not valid. Settings are: " << dbsettings.toString()
+ << "The connection to mongocxx for ltm '" << cache.name()
+ << "' is not valid. Settings are: " << dbsettings.toString()
<< "\nTo start it, run e.g.: \n"
<< "armarx memory start"
<< "\n\n";
@@ -38,7 +40,8 @@ namespace armarx::armem::server::ltm::mongodb
return client;
}
- void Memory::reload()
+ void
+ Memory::reload()
{
TIMING_START(LTM_Reload);
ARMARX_DEBUG << "(Re)Establishing connection to: " << dbsettings.toString();
@@ -69,7 +72,8 @@ namespace armarx::armem::server::ltm::mongodb
TIMING_END_STREAM(LTM_Reload, ARMARX_DEBUG);
}
- void Memory::convert(armem::wm::Memory& m)
+ void
+ Memory::convert(armem::wm::Memory& m)
{
TIMING_START(LTM_Convert);
@@ -87,7 +91,8 @@ namespace armarx::armem::server::ltm::mongodb
TIMING_END_STREAM(LTM_Convert, ARMARX_DEBUG);
}
- void Memory::encodeAndStore()
+ void
+ Memory::encodeAndStore()
{
TIMING_START(LTM_Encode);
@@ -109,4 +114,4 @@ namespace armarx::armem::server::ltm::mongodb
TIMING_END_STREAM(LTM_Encode, ARMARX_DEBUG);
}
-}
+} // namespace armarx::armem::server::ltm::mongodb
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.h b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.h
index 6809bb596692fae617e45ee4eb16368ccd905366..d554b253d2326e0ac1dc87c28f9b7f923c8127f4 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/MemoryManager.h
@@ -8,19 +8,18 @@
#include "../MemoryBase.h"
// Data
-# include "ConnectionManager.h"
+#include "ConnectionManager.h"
namespace armarx::armem::server::ltm::mongodb
{
/// @brief A memory storing data in mongodb (needs 'armarx memory start' to start the mongod instance)
- class Memory :
- public MemoryBase
+ class Memory : public MemoryBase
{
using Base = MemoryBase;
public:
- using Base::MemoryBase;
using Base::convert;
+ using Base::MemoryBase;
Memory() = default;
@@ -35,6 +34,5 @@ namespace armarx::armem::server::ltm::mongodb
ConnectionManager::MongoDBSettings dbsettings;
private:
-
};
-}
+} // namespace armarx::armem::server::ltm::mongodb
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.cpp b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.cpp
index f4169a858fadd55757b04055b80d8836dad4a131..a9845ca27df02a169143f7851371c8da56c5000f 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.cpp
@@ -3,9 +3,10 @@
// Simox
#include <SimoxUtility/json.h>
-#include "../operations.h"
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
#include <RobotAPI/libraries/armem/core/wm/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+
+#include "../operations.h"
namespace armarx::armem::server::ltm::mongodb::util
{
@@ -14,30 +15,37 @@ namespace armarx::armem::server::ltm::mongodb::util
namespace
{
- void mongodbInsertForeignKey(mongocxx::collection& coll, const std::string& key)
+ void
+ mongodbInsertForeignKey(mongocxx::collection& coll, const std::string& key)
{
- auto q = bsoncxxbuilder::document{} << std::string(constantes::FOREIGN_KEY) << key << bsoncxxbuilder::finalize;
+ auto q = bsoncxxbuilder::document{} << std::string(constantes::FOREIGN_KEY) << key
+ << bsoncxxbuilder::finalize;
coll.insert_one(q.view());
}
- bool mongodbContainsForeignKey(mongocxx::collection& coll, const std::string& key)
+ bool
+ mongodbContainsForeignKey(mongocxx::collection& coll, const std::string& key)
{
// check mongodb
- auto q = bsoncxxbuilder::document{} << std::string(constantes::FOREIGN_KEY) << key << bsoncxxbuilder::finalize;
+ auto q = bsoncxxbuilder::document{} << std::string(constantes::FOREIGN_KEY) << key
+ << bsoncxxbuilder::finalize;
auto res = coll.find_one(q.view());
- return (bool) res;
+ return (bool)res;
}
- bool mongodbContainsTimestamp(mongocxx::collection& coll, const long ts)
+ bool
+ mongodbContainsTimestamp(mongocxx::collection& coll, const long ts)
{
// check mongodb
- auto q = bsoncxxbuilder::document{} << std::string(constantes::TIMESTAMP) << ts << bsoncxxbuilder::finalize;
+ auto q = bsoncxxbuilder::document{} << std::string(constantes::TIMESTAMP) << ts
+ << bsoncxxbuilder::finalize;
auto res = coll.find_one(q.view());
- return (bool) res;
+ return (bool)res;
}
- }
+ } // namespace
- void load(const mongocxx::database& db, armem::wm::Memory& m)
+ void
+ load(const mongocxx::database& db, armem::wm::Memory& m)
{
if (!db.has_collection(m.id().str()))
{
@@ -50,16 +58,19 @@ namespace armarx::armem::server::ltm::mongodb::util
auto el = doc[constantes::FOREIGN_KEY];
auto foreignKey = el.get_utf8().value;
- MemoryID i((std::string) foreignKey);
+ MemoryID i((std::string)foreignKey);
if (i.memoryName != m.id().memoryName)
{
- throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
+ throw error::InvalidMemoryID(
+ i,
+ "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
}
std::string k = i.coreSegmentName;
if (m.hasCoreSegment(k))
{
- throw error::ArMemError("Somehow the container already contains the key k = " + k + ". Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow the container already contains the key k = " + k +
+ ". Do you have double entries in mongodb?");
}
else
{
@@ -69,7 +80,8 @@ namespace armarx::armem::server::ltm::mongodb::util
}
}
- void load(const mongocxx::database& db, armem::wm::CoreSegment& c)
+ void
+ load(const mongocxx::database& db, armem::wm::CoreSegment& c)
{
if (!db.has_collection(c.id().str()))
{
@@ -82,17 +94,19 @@ namespace armarx::armem::server::ltm::mongodb::util
auto el = doc[constantes::FOREIGN_KEY];
auto foreignKey = el.get_utf8().value;
- MemoryID i((std::string) foreignKey);
- if (i.coreSegmentName != c.id().coreSegmentName ||
- i.memoryName != c.id().memoryName)
+ MemoryID i((std::string)foreignKey);
+ if (i.coreSegmentName != c.id().coreSegmentName || i.memoryName != c.id().memoryName)
{
- throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
+ throw error::InvalidMemoryID(
+ i,
+ "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
}
std::string k = i.providerSegmentName;
if (c.hasProviderSegment(k))
{
- throw error::ArMemError("Somehow the container already contains the key k = " + k + ". Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow the container already contains the key k = " + k +
+ ". Do you have double entries in mongodb?");
}
else
{
@@ -102,7 +116,8 @@ namespace armarx::armem::server::ltm::mongodb::util
}
}
- void load(const mongocxx::database& db, armem::wm::ProviderSegment& p)
+ void
+ load(const mongocxx::database& db, armem::wm::ProviderSegment& p)
{
if (!db.has_collection(p.id().str()))
{
@@ -115,18 +130,20 @@ namespace armarx::armem::server::ltm::mongodb::util
auto el = doc[constantes::FOREIGN_KEY];
auto foreignKey = el.get_utf8().value;
- MemoryID i((std::string) foreignKey);
+ MemoryID i((std::string)foreignKey);
if (i.providerSegmentName != p.id().providerSegmentName ||
- i.coreSegmentName != p.id().coreSegmentName ||
- i.memoryName != p.id().memoryName)
+ i.coreSegmentName != p.id().coreSegmentName || i.memoryName != p.id().memoryName)
{
- throw error::InvalidMemoryID(i, "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
+ throw error::InvalidMemoryID(
+ i,
+ "A MemoryID in mongodb was invalid. Found the wrong memory name: " + i.str());
}
std::string k = i.entityName;
if (p.hasEntity(k))
{
- throw error::ArMemError("Somehow the container already contains the key k = " + k + ". Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow the container already contains the key k = " + k +
+ ". Do you have double entries in mongodb?");
}
else
{
@@ -136,7 +153,8 @@ namespace armarx::armem::server::ltm::mongodb::util
}
}
- void load(const mongocxx::database& db, armem::wm::Entity& e)
+ void
+ load(const mongocxx::database& db, armem::wm::Entity& e)
{
if (!db.has_collection(e.id().str()))
{
@@ -150,7 +168,8 @@ namespace armarx::armem::server::ltm::mongodb::util
auto& newSnapshot = e.addSnapshot(ts);
auto i = doc[constantes::INSTANCES];
- unsigned long length = std::distance(i.get_array().value.begin(), i.get_array().value.end());
+ unsigned long length =
+ std::distance(i.get_array().value.begin(), i.get_array().value.end());
for (unsigned long i = 0; i < length; ++i)
{
@@ -163,152 +182,167 @@ namespace armarx::armem::server::ltm::mongodb::util
}
}
- void convert(const mongocxx::database& db, armem::wm::Memory& m)
+ void
+ convert(const mongocxx::database& db, armem::wm::Memory& m)
{
- m.forEachCoreSegment([&db](armem::wm::CoreSegment & e)
- {
- convert(db, e);
- });
+ m.forEachCoreSegment([&db](armem::wm::CoreSegment& e) { convert(db, e); });
}
- void convert(const mongocxx::database& db, armem::wm::CoreSegment& c)
+ void
+ convert(const mongocxx::database& db, armem::wm::CoreSegment& c)
{
- c.forEachProviderSegment([&db](armem::wm::ProviderSegment & e)
- {
- convert(db, e);
- });
+ c.forEachProviderSegment([&db](armem::wm::ProviderSegment& e) { convert(db, e); });
}
- void convert(const mongocxx::database& db, armem::wm::ProviderSegment& p)
+ void
+ convert(const mongocxx::database& db, armem::wm::ProviderSegment& p)
{
- p.forEachEntity([&db](armem::wm::Entity & e)
- {
- convert(db, e);
- });
+ p.forEachEntity([&db](armem::wm::Entity& e) { convert(db, e); });
}
- void convert(const mongocxx::database& db, armem::wm::Entity& e)
+ void
+ convert(const mongocxx::database& db, armem::wm::Entity& e)
{
- e.forEachSnapshot([&db](armem::wm::EntitySnapshot & e)
- {
- if (!ltm::util::entityHasData(e))
+ e.forEachSnapshot(
+ [&db](armem::wm::EntitySnapshot& e)
{
- // Get data from mongodb
- auto eColl = db.collection(e.id().getEntityID().str());
- auto q = bsoncxxbuilder::document{} << std::string(constantes::TIMESTAMP) << e.id().timestamp.toMicroSeconds() << bsoncxxbuilder::finalize;
- auto res = eColl.find_one(q.view());
-
- if (!res)
- {
- // break if the data could not be found in ltm storage
- // perhaps its a not-set maybe type from the cache (not yet consollidated to ltm)
- return false;
- }
-
- // convert full json of this entry
- nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
- nlohmann::json instances = json[constantes::INSTANCES];
-
- if (instances.size() != e.size())
+ if (!ltm::util::entityHasData(e))
{
- throw error::ArMemError("The size of the mongodb entity entry at id " + e.id().getEntitySnapshotID().str() + " has wrong size. Expected: " + std::to_string(e.size()) + " but got: " + std::to_string(instances.size()));
- }
-
- for (unsigned int i = 0; i < e.size(); ++i)
- {
- auto& ins = e.getInstance(e.id().withInstanceIndex(i));
-
- // get ionstance json
- nlohmann::json doc = instances[i];
- auto aron = aron::converter::AronNlohmannJSONConverter::ConvertFromNlohmannJSONObject(doc);
-
- // remove metadata
- wm::EntityInstance tmp(e.id().withInstanceIndex(i));
- from_aron(aron, tmp);
-
- // set data
- ins.data() = tmp.data();
+ // Get data from mongodb
+ auto eColl = db.collection(e.id().getEntityID().str());
+ auto q = bsoncxxbuilder::document{} << std::string(constantes::TIMESTAMP)
+ << e.id().timestamp.toMicroSeconds()
+ << bsoncxxbuilder::finalize;
+ auto res = eColl.find_one(q.view());
+
+ if (!res)
+ {
+ // break if the data could not be found in ltm storage
+ // perhaps its a not-set maybe type from the cache (not yet consollidated to ltm)
+ return false;
+ }
+
+ // convert full json of this entry
+ nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
+ nlohmann::json instances = json[constantes::INSTANCES];
+
+ if (instances.size() != e.size())
+ {
+ throw error::ArMemError(
+ "The size of the mongodb entity entry at id " +
+ e.id().getEntitySnapshotID().str() +
+ " has wrong size. Expected: " + std::to_string(e.size()) +
+ " but got: " + std::to_string(instances.size()));
+ }
+
+ for (unsigned int i = 0; i < e.size(); ++i)
+ {
+ auto& ins = e.getInstance(e.id().withInstanceIndex(i));
+
+ // get ionstance json
+ nlohmann::json doc = instances[i];
+ auto aron = aron::converter::AronNlohmannJSONConverter::
+ ConvertFromNlohmannJSONObject(doc);
+
+ // remove metadata
+ wm::EntityInstance tmp(e.id().withInstanceIndex(i));
+ from_aron(aron, tmp);
+
+ // set data
+ ins.data() = tmp.data();
+ }
}
- }
- return true;
- });
+ return true;
+ });
}
- void store(const mongocxx::database& db, const armem::wm::Memory& m)
+ void
+ store(const mongocxx::database& db, const armem::wm::Memory& m)
{
auto coll = db.collection(m.id().str());
- m.forEachCoreSegment([&db, &coll](armem::wm::CoreSegment & e)
- {
- if (!mongodbContainsForeignKey(coll, e.id().str()))
+ m.forEachCoreSegment(
+ [&db, &coll](armem::wm::CoreSegment& e)
{
- mongodbInsertForeignKey(coll, e.id().str());
- }
+ if (!mongodbContainsForeignKey(coll, e.id().str()))
+ {
+ mongodbInsertForeignKey(coll, e.id().str());
+ }
- store(db, e);
- });
+ store(db, e);
+ });
}
- void store(const mongocxx::database& db, const armem::wm::CoreSegment& c)
+ void
+ store(const mongocxx::database& db, const armem::wm::CoreSegment& c)
{
auto coll = db.collection(c.id().str());
- c.forEachProviderSegment([&db, &coll](armem::wm::ProviderSegment & e)
- {
- if (!mongodbContainsForeignKey(coll, e.id().str()))
+ c.forEachProviderSegment(
+ [&db, &coll](armem::wm::ProviderSegment& e)
{
- mongodbInsertForeignKey(coll, e.id().str());
- }
+ if (!mongodbContainsForeignKey(coll, e.id().str()))
+ {
+ mongodbInsertForeignKey(coll, e.id().str());
+ }
- store(db, e);
- });
+ store(db, e);
+ });
}
- void store(const mongocxx::database& db, const armem::wm::ProviderSegment& p)
+ void
+ store(const mongocxx::database& db, const armem::wm::ProviderSegment& p)
{
auto coll = db.collection(p.id().str());
- p.forEachEntity([&db, &coll](armem::wm::Entity & e)
- {
- if (!mongodbContainsForeignKey(coll, e.id().str()))
+ p.forEachEntity(
+ [&db, &coll](armem::wm::Entity& e)
{
- mongodbInsertForeignKey(coll, e.id().str());
- }
+ if (!mongodbContainsForeignKey(coll, e.id().str()))
+ {
+ mongodbInsertForeignKey(coll, e.id().str());
+ }
- store(db, e);
- });
+ store(db, e);
+ });
}
- void store(const mongocxx::database& db, const armem::wm::Entity& e)
+ void
+ store(const mongocxx::database& db, const armem::wm::Entity& e)
{
auto coll = db.collection(e.id().str());
- e.forEachSnapshot([&coll](armem::wm::EntitySnapshot & e)
- {
- if (!mongodbContainsTimestamp(coll, e.id().timestamp.toMilliSeconds()))
+ e.forEachSnapshot(
+ [&coll](armem::wm::EntitySnapshot& e)
{
- // timestamp not found in mongodb ==> new entry
- bsoncxxbuilder::document builder{};
- auto in_array = builder
- << std::string(constantes::ID) << e.id().str()
- << std::string(constantes::TIMESTAMP) << e.id().timestamp.toMicroSeconds()
- << std::string(constantes::INSTANCES);
- auto array_builder = bsoncxx::builder::basic::array{};
-
- e.forEachInstance([&array_builder](const wm::EntityInstance & e)
+ if (!mongodbContainsTimestamp(coll, e.id().timestamp.toMilliSeconds()))
{
- auto aron = std::make_shared<aron::data::Dict>();
- to_aron(aron, e);
- nlohmann::json j = aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(aron);
-
- auto doc_value = bsoncxx::from_json(j.dump(2));
- array_builder.append(doc_value);
- });
-
- auto after_array = in_array << array_builder;
- bsoncxx::document::value doc = after_array << bsoncxx::builder::stream::finalize;
- coll.insert_one(doc.view());
-
- // check to update lastSnapshot map TODO
- //checkUpdateLatestSnapshot(e);
- }
- });
+ // timestamp not found in mongodb ==> new entry
+ bsoncxxbuilder::document builder{};
+ auto in_array = builder << std::string(constantes::ID) << e.id().str()
+ << std::string(constantes::TIMESTAMP)
+ << e.id().timestamp.toMicroSeconds()
+ << std::string(constantes::INSTANCES);
+ auto array_builder = bsoncxx::builder::basic::array{};
+
+ e.forEachInstance(
+ [&array_builder](const wm::EntityInstance& e)
+ {
+ auto aron = std::make_shared<aron::data::Dict>();
+ to_aron(aron, e);
+ nlohmann::json j =
+ aron::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(
+ aron);
+
+ auto doc_value = bsoncxx::from_json(j.dump(2));
+ array_builder.append(doc_value);
+ });
+
+ auto after_array = in_array << array_builder;
+ bsoncxx::document::value doc = after_array
+ << bsoncxx::builder::stream::finalize;
+ coll.insert_one(doc.view());
+
+ // check to update lastSnapshot map TODO
+ //checkUpdateLatestSnapshot(e);
+ }
+ });
}
-} // namespace armarx::armem::server::ltm
+} // namespace armarx::armem::server::ltm::mongodb::util
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.h b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.h
index c81c2312e3e4083e1323465cb5b92ee54bb75818..746d1245507d402a72561279480d8fe9981525dc 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/mongodb/operations.h
@@ -4,7 +4,7 @@
#include <RobotAPI/libraries/armem/server/forward_declarations.h>
// Data
-# include "ConnectionManager.h"
+#include "ConnectionManager.h"
namespace armarx::armem::server::ltm::mongodb
{
@@ -15,7 +15,7 @@ namespace armarx::armem::server::ltm::mongodb
const std::string ID = "id";
const std::string TIMESTAMP = "timestamp";
const std::string INSTANCES = "instances";
- }
+ } // namespace constantes
namespace util
{
@@ -33,6 +33,6 @@ namespace armarx::armem::server::ltm::mongodb
void store(const mongocxx::database& db, const armem::wm::CoreSegment& memory);
void store(const mongocxx::database& db, const armem::wm::ProviderSegment& memory);
void store(const mongocxx::database& db, const armem::wm::Entity& memory);
- }
+ } // namespace util
} // namespace armarx::armem::server::ltm::mongodb
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/util/filesystem_util.cpp b/source/RobotAPI/libraries/armem/server/ltm/legacy/util/filesystem_util.cpp
index d3b307eab0c5bb8cc9b8487d7429fc46ab2a76a6..fd8a47a4d93d6126e9466c242a1dba7c065eb72f 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/legacy/util/filesystem_util.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/util/filesystem_util.cpp
@@ -4,12 +4,12 @@
#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
-
namespace armarx::armem::server::ltm::disk
{
namespace filesystem::util
{
- size_t getSizeOfDirectory(const std::filesystem::path& p)
+ size_t
+ getSizeOfDirectory(const std::filesystem::path& p)
{
if (!std::filesystem::exists(p) || !std::filesystem::is_directory(p))
{
@@ -20,8 +20,9 @@ namespace armarx::armem::server::ltm::disk
std::string cmd = "du -sb " + p.string() + " | cut -f1 2>&1";
// execute above command and get the output
- FILE *stream = popen(cmd.c_str(), "r");
- if (stream) {
+ FILE* stream = popen(cmd.c_str(), "r");
+ if (stream)
+ {
const int max_size = 256;
char readbuf[max_size];
if (fgets(readbuf, max_size, stream) != NULL)
@@ -35,12 +36,14 @@ namespace armarx::armem::server::ltm::disk
return 0;
}
- bool checkIfFolderInFilesystemExists(const std::filesystem::path& p)
+ bool
+ checkIfFolderInFilesystemExists(const std::filesystem::path& p)
{
return std::filesystem::exists(p) and std::filesystem::is_directory(p);
}
- void ensureFolderInFilesystemExists(const std::filesystem::path& p, bool createIfNotExistent)
+ void
+ ensureFolderInFilesystemExists(const std::filesystem::path& p, bool createIfNotExistent)
{
if (!std::filesystem::exists(p))
{
@@ -55,12 +58,14 @@ namespace armarx::armem::server::ltm::disk
}
}
- bool checkIfFileInFilesystemExists(const std::filesystem::path& p)
+ bool
+ checkIfFileInFilesystemExists(const std::filesystem::path& p)
{
return std::filesystem::exists(p) and std::filesystem::is_regular_file(p);
}
- void ensureFileInFilesystemExists(const std::filesystem::path& p)
+ void
+ ensureFileInFilesystemExists(const std::filesystem::path& p)
{
if (!std::filesystem::exists(p) || !std::filesystem::is_regular_file(p))
{
@@ -69,26 +74,32 @@ namespace armarx::armem::server::ltm::disk
}
}
- void writeDataInFilesystemFile(const std::filesystem::path& p, const std::vector<unsigned char>& data)
+ void
+ writeDataInFilesystemFile(const std::filesystem::path& p,
+ const std::vector<unsigned char>& data)
{
std::ofstream dataofs;
dataofs.open(p);
if (!dataofs)
{
- throw error::ArMemError("Could not write data to filesystem file '" + p.string() + "'. Skipping this file.");
+ throw error::ArMemError("Could not write data to filesystem file '" + p.string() +
+ "'. Skipping this file.");
}
dataofs.write(reinterpret_cast<const char*>(data.data()), data.size());
dataofs.close();
}
- std::vector<unsigned char> readDataFromFilesystemFile(const std::filesystem::path path)
+ std::vector<unsigned char>
+ readDataFromFilesystemFile(const std::filesystem::path path)
{
std::ifstream dataifs(path);
- std::vector<unsigned char> datafilecontent((std::istreambuf_iterator<char>(dataifs)), (std::istreambuf_iterator<char>()));
+ std::vector<unsigned char> datafilecontent((std::istreambuf_iterator<char>(dataifs)),
+ (std::istreambuf_iterator<char>()));
return datafilecontent;
}
- std::vector<std::string> getAllFilesystemDirectories(const std::filesystem::path path)
+ std::vector<std::string>
+ getAllFilesystemDirectories(const std::filesystem::path path)
{
std::vector<std::string> ret;
for (const auto& subdir : std::filesystem::directory_iterator(path))
@@ -103,7 +114,8 @@ namespace armarx::armem::server::ltm::disk
return ret;
}
- std::vector<std::string> getAllFilesystemFiles(const std::filesystem::path path)
+ std::vector<std::string>
+ getAllFilesystemFiles(const std::filesystem::path path)
{
std::vector<std::string> ret;
for (const auto& subdir : std::filesystem::directory_iterator(path))
@@ -117,6 +129,5 @@ namespace armarx::armem::server::ltm::disk
std::sort(ret.begin(), ret.end());
return ret;
}
- }
-} // namespace armarx::armem::server::ltm::diskfile:///home/fabian/code/armarx/RobotAPI/source/RobotAPI/libraries/armem/server/ltm/legacy/util/util.h
-
+ } // namespace filesystem::util
+} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/util/filesystem_util.h b/source/RobotAPI/libraries/armem/server/ltm/legacy/util/filesystem_util.h
index 78a8f4b1399b8112e204fb891277ed7d52d75ba6..3f21b0edf4fdfb04d55ba440878af99724d489f0 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/legacy/util/filesystem_util.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/util/filesystem_util.h
@@ -5,7 +5,6 @@
#include <iostream>
#include <vector>
-
namespace armarx::armem::server::ltm::disk
{
namespace filesystem::util
@@ -14,18 +13,20 @@ namespace armarx::armem::server::ltm::disk
bool checkIfFolderInFilesystemExists(const std::filesystem::path& p);
- void ensureFolderInFilesystemExists(const std::filesystem::path& p, bool createIfNotExistent = true);
+ void ensureFolderInFilesystemExists(const std::filesystem::path& p,
+ bool createIfNotExistent = true);
bool checkIfFileInFilesystemExists(const std::filesystem::path& p);
void ensureFileInFilesystemExists(const std::filesystem::path& p);
- void writeDataInFilesystemFile(const std::filesystem::path& p, const std::vector<unsigned char>& data);
+ void writeDataInFilesystemFile(const std::filesystem::path& p,
+ const std::vector<unsigned char>& data);
std::vector<unsigned char> readDataFromFilesystemFile(const std::filesystem::path path);
std::vector<std::string> getAllFilesystemDirectories(const std::filesystem::path path);
std::vector<std::string> getAllFilesystemFiles(const std::filesystem::path path);
- }
+ } // namespace filesystem::util
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/util/util.cpp b/source/RobotAPI/libraries/armem/server/ltm/legacy/util/util.cpp
index 8828dc595166c90f7a7217e247428ba0792bf0c4..c90761fdef7368339fb8a87338368e08e81f8fab 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/legacy/util/util.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/util/util.cpp
@@ -1,14 +1,15 @@
#include "util.h"
-#include <thread>
#include <chrono>
+#include <thread>
namespace armarx::armem::server::ltm::disk
{
namespace util
{
// check whether a string is a number (e.g. timestamp folders)
- bool isNumber(const std::string& s)
+ bool
+ isNumber(const std::string& s)
{
for (char const& ch : s)
{
@@ -20,7 +21,8 @@ namespace armarx::armem::server::ltm::disk
return true;
}
- bool checkIfBasePathExists(const std::filesystem::path& mPath)
+ bool
+ checkIfBasePathExists(const std::filesystem::path& mPath)
{
if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
@@ -29,7 +31,8 @@ namespace armarx::armem::server::ltm::disk
return filesystem::util::checkIfFolderInFilesystemExists(mPath);
}
- void ensureBasePathExists(const std::filesystem::path& mPath, bool createIfNotExistent)
+ void
+ ensureBasePathExists(const std::filesystem::path& mPath, bool createIfNotExistent)
{
if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
@@ -40,7 +43,8 @@ namespace armarx::armem::server::ltm::disk
return filesystem::util::ensureFolderInFilesystemExists(mPath, createIfNotExistent);
}
- bool checkIfFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ bool
+ checkIfFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
{
if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
@@ -50,7 +54,10 @@ namespace armarx::armem::server::ltm::disk
return filesystem::util::checkIfFolderInFilesystemExists(mPath / p);
}
- void ensureFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p, bool createIfNotExistent)
+ void
+ ensureFolderExists(const std::filesystem::path& mPath,
+ const std::filesystem::path& p,
+ bool createIfNotExistent)
{
if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
@@ -60,7 +67,8 @@ namespace armarx::armem::server::ltm::disk
return filesystem::util::ensureFolderInFilesystemExists(mPath / p, createIfNotExistent);
}
- bool checkIfFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ bool
+ checkIfFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
{
if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
@@ -70,7 +78,8 @@ namespace armarx::armem::server::ltm::disk
return filesystem::util::checkIfFileInFilesystemExists(mPath / p);
}
- void ensureFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ void
+ ensureFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p)
{
if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
@@ -80,7 +89,10 @@ namespace armarx::armem::server::ltm::disk
return filesystem::util::ensureFileInFilesystemExists(mPath / p);
}
- void writeDataToFile(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data)
+ void
+ writeDataToFile(const std::filesystem::path& mPath,
+ const std::filesystem::path& p,
+ const std::vector<unsigned char>& data)
{
if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
@@ -90,7 +102,12 @@ namespace armarx::armem::server::ltm::disk
return filesystem::util::writeDataInFilesystemFile(mPath / p, data);
}
- void writeDataToFileRepeated(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data, const unsigned int maxTries, const unsigned int sleepTimeMs)
+ void
+ writeDataToFileRepeated(const std::filesystem::path& mPath,
+ const std::filesystem::path& p,
+ const std::vector<unsigned char>& data,
+ const unsigned int maxTries,
+ const unsigned int sleepTimeMs)
{
for (unsigned int i = 0; i < maxTries; ++i)
{
@@ -107,10 +124,14 @@ namespace armarx::armem::server::ltm::disk
}
// even after all the tries we did not succeeded. This is very bad!
- throw error::ArMemError("ATTENTION! Even after " + std::to_string(maxTries) + " tries, the memory was not able to store the instance at path '" + p.string() + "'. This means this instance will be lost!");
+ throw error::ArMemError(
+ "ATTENTION! Even after " + std::to_string(maxTries) +
+ " tries, the memory was not able to store the instance at path '" + p.string() +
+ "'. This means this instance will be lost!");
}
- std::vector<unsigned char> readDataFromFile(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ std::vector<unsigned char>
+ readDataFromFile(const std::filesystem::path& mPath, const std::filesystem::path& p)
{
if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
@@ -120,7 +141,8 @@ namespace armarx::armem::server::ltm::disk
return filesystem::util::readDataFromFilesystemFile(mPath / p);
}
- std::vector<std::string> getAllDirectories(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ std::vector<std::string>
+ getAllDirectories(const std::filesystem::path& mPath, const std::filesystem::path& p)
{
if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
@@ -130,7 +152,8 @@ namespace armarx::armem::server::ltm::disk
return filesystem::util::getAllFilesystemDirectories(mPath / p);
}
- std::vector<std::string> getAllFiles(const std::filesystem::path& mPath, const std::filesystem::path& p)
+ std::vector<std::string>
+ getAllFiles(const std::filesystem::path& mPath, const std::filesystem::path& p)
{
if (mPath.extension() == minizip::util::MINIZIP_SUFFIX)
{
@@ -139,5 +162,5 @@ namespace armarx::armem::server::ltm::disk
return filesystem::util::getAllFilesystemFiles(mPath / p);
}
- }
+ } // namespace util
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/legacy/util/util.h b/source/RobotAPI/libraries/armem/server/ltm/legacy/util/util.h
index e260d70923c7eb36bf0a78ab6e0c3a7d3bf98c52..fc5d9dbb45c5723826b8010775f599d99e5b48be 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/legacy/util/util.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/legacy/util/util.h
@@ -1,7 +1,6 @@
#pragma once
#include "../../../../../core/error.h"
-
#include "filesystem_util.h"
#include "minizip_util.h"
@@ -12,7 +11,7 @@ namespace armarx::armem::server::ltm::disk
const std::string TYPE_FILENAME = "type.aron";
const std::string DATA_FILENAME = "data.aron";
const std::string METADATA_FILENAME = "metadata.aron";
- }
+ } // namespace constantes
namespace util
{
@@ -21,24 +20,37 @@ namespace armarx::armem::server::ltm::disk
bool checkIfBasePathExists(const std::filesystem::path& mPath);
- void ensureBasePathExists(const std::filesystem::path& mPath, bool createIfNotExistent = true);
+ void ensureBasePathExists(const std::filesystem::path& mPath,
+ bool createIfNotExistent = true);
- bool checkIfFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p);
+ bool checkIfFolderExists(const std::filesystem::path& mPath,
+ const std::filesystem::path& p);
- void ensureFolderExists(const std::filesystem::path& mPath, const std::filesystem::path& p, bool createIfNotExistent = true);
+ void ensureFolderExists(const std::filesystem::path& mPath,
+ const std::filesystem::path& p,
+ bool createIfNotExistent = true);
bool checkIfFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p);
void ensureFileExists(const std::filesystem::path& mPath, const std::filesystem::path& p);
- void writeDataToFile(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data);
+ void writeDataToFile(const std::filesystem::path& mPath,
+ const std::filesystem::path& p,
+ const std::vector<unsigned char>& data);
- void writeDataToFileRepeated(const std::filesystem::path& mPath, const std::filesystem::path& p, const std::vector<unsigned char>& data, const unsigned int maxTries = 100, const unsigned int sleepTimeMs = 10);
+ void writeDataToFileRepeated(const std::filesystem::path& mPath,
+ const std::filesystem::path& p,
+ const std::vector<unsigned char>& data,
+ const unsigned int maxTries = 100,
+ const unsigned int sleepTimeMs = 10);
- std::vector<unsigned char> readDataFromFile(const std::filesystem::path& mPath, const std::filesystem::path& p);
+ std::vector<unsigned char> readDataFromFile(const std::filesystem::path& mPath,
+ const std::filesystem::path& p);
- std::vector<std::string> getAllDirectories(const std::filesystem::path& mPath, const std::filesystem::path& p);
+ std::vector<std::string> getAllDirectories(const std::filesystem::path& mPath,
+ const std::filesystem::path& p);
- std::vector<std::string> getAllFiles(const std::filesystem::path& mPath, const std::filesystem::path& p);
- }
+ std::vector<std::string> getAllFiles(const std::filesystem::path& mPath,
+ const std::filesystem::path& p);
+ } // namespace util
} // namespace armarx::armem::server::ltm::disk
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
index 83413578f58480e7d19517980623c88159b5985f..a2405db2d40275f47a6932cf18199e48b06e0956 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.cpp
@@ -82,10 +82,12 @@ namespace armarx::armem::server::ltm
return stats;
}
- void Processors::resetFilterStatisticsForNewEpisode()
+ void
+ Processors::resetFilterStatisticsForNewEpisode()
{
ARMARX_DEBUG << "Resetting statistics for filters";
- for(const auto& filter_ptr: this->snapFilters){
+ for (const auto& filter_ptr : this->snapFilters)
+ {
filter_ptr->resetStatisticsForNewEpisode();
}
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h
index 9bdf419dd8649affe14a34e7e2b46d6e47e9e366..8c8ef67d549a6646ea8e304ee35720ae1eedfb1e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/Processors.h
@@ -23,7 +23,8 @@ namespace armarx::armem::server::ltm
void configure(const nlohmann::json& config);
- std::map<std::string, processor::SnapshotFilter::FilterStatistics> getSnapshotFilterStatistics();
+ std::map<std::string, processor::SnapshotFilter::FilterStatistics>
+ getSnapshotFilterStatistics();
/**
* @brief resetFilterStatisticsForNewEpisode runs resetFilterStatisticsForNewEpisode on all
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h
index b65d01925c32dd8d45345c6dba78195713a83be5..b9d651dbbbaa96a5714d1115d43f1e7eab5fa5ab 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/converter/data/image/exr/ExrConverter.h
@@ -9,7 +9,6 @@ namespace armarx::armem::server::ltm::processor::converter::data::image
class ExrConverter : public ImageConverter
{
public:
-
static const constexpr char* NAME = "ExrConverter";
ExrConverter() :
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/extractor/imageExtractor/DepthImageExtractor.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/extractor/imageExtractor/DepthImageExtractor.cpp
index 2222482857e3148659f3c00bdb978aa0e3125381..00a520067fb52b40af99950aea6904db8192b356 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/extractor/imageExtractor/DepthImageExtractor.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/extractor/imageExtractor/DepthImageExtractor.cpp
@@ -1,9 +1,9 @@
#include "DepthImageExtractor.h"
-
namespace armarx::armem::server::ltm::processor::extractor
{
- void DepthImageExtractorVisitor::visitDictOnEnter(Input& data)
+ void
+ DepthImageExtractorVisitor::visitDictOnEnter(Input& data)
{
ARMARX_CHECK_NOT_NULL(data);
@@ -14,7 +14,9 @@ namespace armarx::armem::server::ltm::processor::extractor
{
auto ndarray = aron::data::NDArray::DynamicCastAndCheck(child);
auto shape = ndarray->getShape();
- if (shape.size() == 3 && shape[2] == 4 && std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>()) > 200) // must be big enough to assume an image (instead of 4x4x4 poses)
+ if (shape.size() == 3 && shape[2] == 4 &&
+ std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>()) >
+ 200) // must be big enough to assume an image (instead of 4x4x4 poses)
{
depthImages[key] = ndarray;
dict->setElement(key, nullptr);
@@ -23,12 +25,14 @@ namespace armarx::armem::server::ltm::processor::extractor
}
}
- void DepthImageExtractorVisitor::visitUnknown(Input&)
+ void
+ DepthImageExtractorVisitor::visitUnknown(Input&)
{
// A member is null. Simply ignore...
}
- Extractor::ExtractionResult DepthImageExtractor::extract(aron::data::DictPtr& data)
+ Extractor::ExtractionResult
+ DepthImageExtractor::extract(aron::data::DictPtr& data)
{
DepthImageExtractorVisitor visitor;
aron::data::VariantPtr var = std::static_pointer_cast<aron::data::Variant>(data);
@@ -41,8 +45,9 @@ namespace armarx::armem::server::ltm::processor::extractor
return encoding;
}
- aron::data::DictPtr DepthImageExtractor::merge(ExtractionResult& encoding)
+ aron::data::DictPtr
+ DepthImageExtractor::merge(ExtractionResult& encoding)
{
return encoding.dataWithoutExtraction;
}
-}
+} // namespace armarx::armem::server::ltm::processor::extractor
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/extractor/imageExtractor/ImageExtractor.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/extractor/imageExtractor/ImageExtractor.cpp
index b1fd4650973c0aaebd97c08fb32a533672756fa3..7926483fb7650c5126869a820f34dbd77877a93a 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/extractor/imageExtractor/ImageExtractor.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/extractor/imageExtractor/ImageExtractor.cpp
@@ -51,4 +51,4 @@ namespace armarx::armem::server::ltm::processor::extractor
{
return encoding.dataWithoutExtraction;
}
-} // namespace armarx::armem::server::ltm::extractor
+} // namespace armarx::armem::server::ltm::processor::extractor
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp
index 4aa00b6f3fc673a7ad89b393b9eba9b7f6523991..10eeba41ca0ff2eb50218eb77e7876ea49523dbc 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.cpp
@@ -12,17 +12,20 @@ namespace armarx::armem::server::ltm::processor
{
}
- SnapshotFilter::FilterStatistics SnapshotFilter::getFilterStatistics()
+ SnapshotFilter::FilterStatistics
+ SnapshotFilter::getFilterStatistics()
{
return stats;
}
- std::string SnapshotFilter::getName()
+ std::string
+ SnapshotFilter::getName()
{
return "Base_Filter";
}
- void SnapshotFilter::resetStatisticsForNewEpisode()
+ void
+ SnapshotFilter::resetStatisticsForNewEpisode()
{
stats.accepted = 0;
stats.rejected = 0;
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
index 4bf8ec0493bf70a31a402ef7e196e1463c4de515..7e7791ccd90643f09cf09ba16c2effb2f986402c 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/Filter.h
@@ -32,7 +32,8 @@ namespace armarx::armem::server::ltm::processor
virtual bool accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion = false) = 0;
virtual void configure(const nlohmann::json& json);
- struct FilterStatistics {
+ struct FilterStatistics
+ {
double accepted = 0;
double rejected = 0;
std::chrono::duration<double> additional_time = std::chrono::duration<double>::zero();
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
index 6f55b623a9fdc92a5ba518d63e2ace9c8003826b..0d5090c7ab8acdc63fa1f3533c74956ef8eafaf8 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.cpp
@@ -1,11 +1,12 @@
#include "EqualityFilter.h"
+
#include <list>
#include <IceUtil/Time.h>
+
#include "RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Float.h"
-
namespace armarx::armem::server::ltm::processor::filter
{
bool
@@ -18,33 +19,47 @@ namespace armarx::armem::server::ltm::processor::filter
std::vector<armarx::aron::data::FloatPtr> floats_snapshot;
std::vector<float> distances;
- e.forEachInstance([&num_instances, &images_snapshot, &floats_snapshot](armem::wm::EntityInstance& i){
- auto data = aron::data::Dict::DynamicCastAndCheck(i.data());
- for(auto key: data->getAllKeys()){
- aron::data::Descriptor img_desc;
- try{
- auto d = data->at(key);
-; img_desc = data->at(key)->getDescriptor();
- } catch (...){
- ARMARX_INFO << "Problem with accessing image description";
- img_desc = aron::data::Descriptor::NDARRAY;
- }
- if (img_desc == aron::data::Descriptor::NDARRAY){
- auto img_nd = aron::data::NDArray::DynamicCastAndCheck(data->at(key));
- images_snapshot.insert(images_snapshot.end(), img_nd);
- num_instances++;
- } else if (img_desc == aron::data::Descriptor::FLOAT){
- auto fl = aron::data::Float::DynamicCastAndCheck(data->at(key));
- floats_snapshot.push_back(fl);
- num_instances++;
- } else {
- ARMARX_INFO << "data-type not yet supported. \n"
- << "Only ndarray and float data types are supported for equality filters yet.";
- }
- }
- });
-
- if(images.size() < 2){
+ e.forEachInstance(
+ [&num_instances, &images_snapshot, &floats_snapshot](armem::wm::EntityInstance& i)
+ {
+ auto data = aron::data::Dict::DynamicCastAndCheck(i.data());
+ for (auto key : data->getAllKeys())
+ {
+ aron::data::Descriptor img_desc;
+ try
+ {
+ auto d = data->at(key);
+ ;
+ img_desc = data->at(key)->getDescriptor();
+ }
+ catch (...)
+ {
+ ARMARX_INFO << "Problem with accessing image description";
+ img_desc = aron::data::Descriptor::NDARRAY;
+ }
+ if (img_desc == aron::data::Descriptor::NDARRAY)
+ {
+ auto img_nd = aron::data::NDArray::DynamicCastAndCheck(data->at(key));
+ images_snapshot.insert(images_snapshot.end(), img_nd);
+ num_instances++;
+ }
+ else if (img_desc == aron::data::Descriptor::FLOAT)
+ {
+ auto fl = aron::data::Float::DynamicCastAndCheck(data->at(key));
+ floats_snapshot.push_back(fl);
+ num_instances++;
+ }
+ else
+ {
+ ARMARX_INFO << "data-type not yet supported. \n"
+ << "Only ndarray and float data types are supported for "
+ "equality filters yet.";
+ }
+ }
+ });
+
+ if (images.size() < 2)
+ {
ARMARX_INFO << "Adding first images, because nothing to compare";
images.push_back(images_snapshot);
this->stats.accepted += 1;
@@ -52,7 +67,9 @@ namespace armarx::armem::server::ltm::processor::filter
stats.end_time = end;
stats.additional_time += (end - start);
return true;
- } else if(images.size() < max_images){
+ }
+ else if (images.size() < max_images)
+ {
ARMARX_INFO << "Not enough elements yet to do full comparison of last " << max_images
<< " elements";
images.push_back(images_snapshot);
@@ -66,10 +83,13 @@ namespace armarx::armem::server::ltm::processor::filter
std::vector<armarx::aron::data::NDArrayPtr> lastCommittedImages;
int sizeOfCommited = 0;
- for(int i = 0; i < max_images; i++){
- std::vector<armarx::aron::data::NDArrayPtr> lastCommitImages = images.at(images.size() - i - 1);
+ for (int i = 0; i < max_images; i++)
+ {
+ std::vector<armarx::aron::data::NDArrayPtr> lastCommitImages =
+ images.at(images.size() - i - 1);
sizeOfCommited = lastCommitImages.size();
- for(int j = 0; j < lastCommitImages.size(); j++){
+ for (int j = 0; j < lastCommitImages.size(); j++)
+ {
lastCommittedImages.push_back(lastCommitImages.at(j));
}
// we just concatenate all images (instances)
@@ -77,16 +97,19 @@ namespace armarx::armem::server::ltm::processor::filter
ARMARX_CHECK(sizeOfCommited == images_snapshot.size()); //make sure we have enough instances
- for(int i= 0; i < images_snapshot.size(); i++){
+ for (int i = 0; i < images_snapshot.size(); i++)
+ {
armarx::aron::data::NDArrayPtr new_image = images_snapshot.at(i);
std::vector<armarx::aron::data::NDArrayPtr> commited_images;
- for(int j = 0; j < max_images; j++){
- int index = i + 2*j;
+ for (int j = 0; j < max_images; j++)
+ {
+ int index = i + 2 * j;
auto image = lastCommittedImages.at(index);
commited_images.emplace_back(image);
}
- float distance = aron::similarity::NDArraySimilarity::calculate_similarity_multi(commited_images, new_image, this->similarity_type);
+ float distance = aron::similarity::NDArraySimilarity::calculate_similarity_multi(
+ commited_images, new_image, this->similarity_type);
distances.insert(distances.end(), distance);
}
@@ -94,23 +117,30 @@ namespace armarx::armem::server::ltm::processor::filter
//check for criterion:
float sum_distances = 0;
float max_distance = 0;
- for(auto d: distances){
+ for (auto d : distances)
+ {
sum_distances += d;
- if(d > max_distance){
+ if (d > max_distance)
+ {
max_distance = d;
}
}
- bool max = true; //set true if only maximum distance value is important and false if sum of distances is important
+ bool max =
+ true; //set true if only maximum distance value is important and false if sum of distances is important
bool accept = false;
- if(max){
+ if (max)
+ {
accept = (max_distance > this->threshold);
- } else {
+ }
+ else
+ {
accept = (sum_distances > this->threshold);
}
- if(accept){
+ if (accept)
+ {
images.pop_front(); //delete first element
images.push_back(images_snapshot);
this->stats.accepted += 1;
@@ -118,7 +148,9 @@ namespace armarx::armem::server::ltm::processor::filter
stats.additional_time += (end - start);
stats.end_time = end;
return true;
- } else {
+ }
+ else
+ {
this->stats.rejected += 1;
auto end = std::chrono::high_resolution_clock::now();
stats.additional_time += (end - start);
@@ -127,7 +159,8 @@ namespace armarx::armem::server::ltm::processor::filter
}
}
- void SnapshotSimilarityFilter::configure(const nlohmann::json &json)
+ void
+ SnapshotSimilarityFilter::configure(const nlohmann::json& json)
{
if (json.find(PARAM_THRESHOLD) != json.end())
{
@@ -136,32 +169,43 @@ namespace armarx::armem::server::ltm::processor::filter
stats.additional_info += "Threshold-Parameter: ";
stats.additional_info += std::to_string(threshold);
}
- if(json.find(PARAM_SIM_MEASURE) != json.end()){
+ if (json.find(PARAM_SIM_MEASURE) != json.end())
+ {
std::string type_string = json.at(PARAM_SIM_MEASURE);
- if(type_string == "MSE"){
+ if (type_string == "MSE")
+ {
this->similarity_type = aron::similarity::NDArraySimilarity::Type::MSE;
this->float_similarity_type = aron::similarity::FloatSimilarity::Type::MSE;
stats.similarity_type = aron::similarity::NDArraySimilarity::Type::MSE;
- } else if (type_string == "MAE"){
+ }
+ else if (type_string == "MAE")
+ {
this->similarity_type = aron::similarity::NDArraySimilarity::Type::MAE;
this->float_similarity_type = aron::similarity::FloatSimilarity::Type::MAE;
stats.similarity_type = aron::similarity::NDArraySimilarity::Type::MAE;
- } else if (type_string == "Chernoff"){
+ }
+ else if (type_string == "Chernoff")
+ {
this->similarity_type = aron::similarity::NDArraySimilarity::Type::CHERNOFF;
this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
stats.similarity_type = aron::similarity::NDArraySimilarity::Type::CHERNOFF;
- } else if (type_string == "Cosine"){
+ }
+ else if (type_string == "Cosine")
+ {
this->similarity_type = aron::similarity::NDArraySimilarity::Type::COSINE;
this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
stats.similarity_type = aron::similarity::NDArraySimilarity::Type::COSINE;
- } else {
+ }
+ else
+ {
ARMARX_WARNING << "Undefined similarity measure detected in JSON file";
stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE;
this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
}
ARMARX_INFO << VAROUT(this->similarity_type);
}
- if(json.find(PARAM_MAX_OBJECTS) != json.end()){
+ if (json.find(PARAM_MAX_OBJECTS) != json.end())
+ {
max_images = json.at(PARAM_MAX_OBJECTS);
ARMARX_INFO << VAROUT(max_images);
stats.number_of_compared_objects = max_images;
@@ -170,14 +214,16 @@ namespace armarx::armem::server::ltm::processor::filter
stats.start_time = std::chrono::high_resolution_clock::now();
}
- SnapshotFilter::FilterStatistics SnapshotSimilarityFilter::getFilterStatistics()
+ SnapshotFilter::FilterStatistics
+ SnapshotSimilarityFilter::getFilterStatistics()
{
return stats;
}
- std::string SnapshotSimilarityFilter::getName()
+ std::string
+ SnapshotSimilarityFilter::getName()
{
return this->NAME;
}
-} // namespace armarx::armem::server::ltm::filter
+} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
index 407290de8f18931e71b79a97efe49cd28930405c..e621f7121e892a4284cb4edd27caf96bda91c37f 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/equalityFilter/EqualityFilter.h
@@ -7,12 +7,12 @@
#include "../Filter.h"
// Aron
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include "RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h"
-#include "RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h"
-
#include <chrono>
+#include "RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h"
+#include "RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h"
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+
namespace armarx::armem::server::ltm::processor::filter
{
class SnapshotSimilarityFilter : public SnapshotFilter
@@ -41,6 +41,5 @@ namespace armarx::armem::server::ltm::processor::filter
std::size_t max_images = 2;
aron::similarity::NDArraySimilarity::Type similarity_type;
aron::similarity::FloatSimilarity::Type float_similarity_type;
-
};
} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
index 066272e170c89c44e3bbc8bf3d7dc1d4177d13ca..6bc53b69ebf0995304370fc632305590d491efa5 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.cpp
@@ -6,7 +6,7 @@ namespace armarx::armem::server::ltm::processor::filter
{
bool
- SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot &e, bool simulatedVersion)
+ SnapshotFrequencyFilter::accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion)
{
//accepting to many elements makes the filter slow and brings problems with the buffer with itself!
@@ -18,46 +18,56 @@ namespace armarx::armem::server::ltm::processor::filter
auto memID = e.id().getEntityID();
//find out if timestamps are corrupted:
- if(this->nonCorruptedType == TimestampType::NOT_SET){
+ if (this->nonCorruptedType == TimestampType::NOT_SET)
+ {
ARMARX_DEBUG << "Setting timestamp type";
this->setNonCorruptedTimestampType(e);
ARMARX_DEBUG << VAROUT(this->nonCorruptedType);
}
- if(this->nonCorruptedType == TimestampType::ALL_CORRUPTED) {
+ if (this->nonCorruptedType == TimestampType::ALL_CORRUPTED)
+ {
return true;
}
- if(this->lastTimesPerEntity.end() == this->lastTimesPerEntity.find(memID)){
- //this happens if the key is not in the map, which means this is the first time this
- //entity tries to save a snapshot
- ARMARX_DEBUG << "First time this entity is saved";
- auto firstIndex = e.getInstanceIndices()[0];
- auto firstInsance = e.getInstance(firstIndex);
- auto lastT = this->getNonCorruptedTimestamp(firstInsance, simulatedVersion);
- //for statistics sake:
- auto end = std::chrono::high_resolution_clock::now();
- stats.end_time = end;
- stats.additional_time += (end - start);
- stats.accepted += 1;
- //add this last time to the map:
- this->lastTimesPerEntity[memID] = lastT;
- return true; //the first one is always accepted
- } else {
+ if (this->lastTimesPerEntity.end() == this->lastTimesPerEntity.find(memID))
+ {
+ //this happens if the key is not in the map, which means this is the first time this
+ //entity tries to save a snapshot
+ ARMARX_DEBUG << "First time this entity is saved";
+ auto firstIndex = e.getInstanceIndices()[0];
+ auto firstInsance = e.getInstance(firstIndex);
+ auto lastT = this->getNonCorruptedTimestamp(firstInsance, simulatedVersion);
+ //for statistics sake:
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
+ stats.accepted += 1;
+ //add this last time to the map:
+ this->lastTimesPerEntity[memID] = lastT;
+ return true; //the first one is always accepted
+ }
+ else
+ {
auto lastTime = this->lastTimesPerEntity.at(memID);
// check if any one of the instances for this snapshot were sent in the last x
// milliseconds since a snapshot was accepted last for this entity:
- e.forEachInstance([this, &instances_accepted, ¤t, &lastTime, &simulatedVersion](armem::wm::EntityInstance& i){
- auto t = this->getNonCorruptedTimestamp(i, simulatedVersion);
- int difference = std::abs(t - lastTime);
- if(difference > this->maxDifference){ //at least one instance is older than the last saved instance
- instances_accepted = true;
- current = this->getNonCorruptedTimestamp(i, simulatedVersion);
- }
- });
+ e.forEachInstance(
+ [this, &instances_accepted, ¤t, &lastTime, &simulatedVersion](
+ armem::wm::EntityInstance& i)
+ {
+ auto t = this->getNonCorruptedTimestamp(i, simulatedVersion);
+ int difference = std::abs(t - lastTime);
+ if (difference > this->maxDifference)
+ { //at least one instance is older than the last saved instance
+ instances_accepted = true;
+ current = this->getNonCorruptedTimestamp(i, simulatedVersion);
+ }
+ });
- if(instances_accepted){ //if one of the instances was accepted the time when an
+ if (instances_accepted)
+ { //if one of the instances was accepted the time when an
//instance was accepted last needs to be changed
this->lastTimesPerEntity[memID] = current;
}
@@ -68,9 +78,12 @@ namespace armarx::armem::server::ltm::processor::filter
stats.end_time = end;
stats.additional_time += (end - start);
- if(instances_accepted){
+ if (instances_accepted)
+ {
this->stats.accepted += 1;
- } else {
+ }
+ else
+ {
this->stats.rejected += 1;
}
@@ -78,7 +91,8 @@ namespace armarx::armem::server::ltm::processor::filter
}
void
- SnapshotFrequencyFilter::setNonCorruptedTimestampType(const armem::wm::EntitySnapshot &e){
+ SnapshotFrequencyFilter::setNonCorruptedTimestampType(const armem::wm::EntitySnapshot& e)
+ {
auto firstIndex = e.getInstanceIndices()[0];
auto firstInsance = e.getInstance(firstIndex);
auto sentTime = firstInsance.metadata().sentTime;
@@ -90,34 +104,49 @@ namespace armarx::armem::server::ltm::processor::filter
ARMARX_DEBUG << VAROUT(referencedTime.toMilliSecondsSinceEpoch());
//we want the referenced time, if not set correctly we take sent time, then arrvived time:
- if(referencedTime.toMilliSecondsSinceEpoch() < 946688400000){
+ if (referencedTime.toMilliSecondsSinceEpoch() < 946688400000)
+ {
//timestamp corrupted:
- if(sentTime.toMilliSecondsSinceEpoch() < 946688400000){
+ if (sentTime.toMilliSecondsSinceEpoch() < 946688400000)
+ {
//sent also corrupted:
- if(arrivedTime.toMilliSecondsSinceEpoch() < 946688400000){
+ if (arrivedTime.toMilliSecondsSinceEpoch() < 946688400000)
+ {
//arrived also corrupted:
- if(!corruptedWarningGiven){
- ARMARX_WARNING << "LTM recording does not work correctly, as frequency filter is used, but "
- << "time sent, arrived and referenced are all corrupted. \n"
- << "Accepting all snapshots.";
+ if (!corruptedWarningGiven)
+ {
+ ARMARX_WARNING << "LTM recording does not work correctly, as frequency "
+ "filter is used, but "
+ << "time sent, arrived and referenced are all corrupted. \n"
+ << "Accepting all snapshots.";
this->nonCorruptedType = TimestampType::ALL_CORRUPTED;
corruptedWarningGiven = true;
}
- } else {
- if(!corruptedWarningGiven){ //only print this warning once
- ARMARX_INFO << "Time referenced and sent for snapshot corrupted, using time arrived for filtering";
+ }
+ else
+ {
+ if (!corruptedWarningGiven)
+ { //only print this warning once
+ ARMARX_INFO << "Time referenced and sent for snapshot corrupted, using "
+ "time arrived for filtering";
corruptedWarningGiven = true;
}
this->nonCorruptedType = TimestampType::ARRIVED;
}
- } else {
- if(!corruptedWarningGiven){ //only print this warning once
- ARMARX_INFO << "Time referenced snapshot corrupted, using time sent for filtering";
- corruptedWarningGiven = true;
- }
+ }
+ else
+ {
+ if (!corruptedWarningGiven)
+ { //only print this warning once
+ ARMARX_INFO
+ << "Time referenced snapshot corrupted, using time sent for filtering";
+ corruptedWarningGiven = true;
+ }
this->nonCorruptedType = TimestampType::SENT;
}
- } else {
+ }
+ else
+ {
this->nonCorruptedType = TimestampType::REFERENCED;
}
@@ -153,12 +182,18 @@ namespace armarx::armem::server::ltm::processor::filter
}
int
- SnapshotFrequencyFilter::getNonCorruptedTimestamp(const armem::wm::EntityInstance &i, bool simulatedVersion){
- switch(this->nonCorruptedType) {
+ SnapshotFrequencyFilter::getNonCorruptedTimestamp(const armem::wm::EntityInstance& i,
+ bool simulatedVersion)
+ {
+ switch (this->nonCorruptedType)
+ {
case TimestampType::SENT:
- if(!simulatedVersion){
+ if (!simulatedVersion)
+ {
return i.metadata().sentTime.toMilliSecondsSinceEpoch();
- } else {
+ }
+ else
+ {
[[fallthrough]];
}
case TimestampType::ARRIVED:
@@ -169,7 +204,7 @@ namespace armarx::armem::server::ltm::processor::filter
return -1;
}
}
-
+
void
SnapshotFrequencyFilter::configure(const nlohmann::json& json)
{
@@ -181,16 +216,19 @@ namespace armarx::armem::server::ltm::processor::filter
}
stats.start_time = std::chrono::high_resolution_clock::now();
stats.number_of_compared_objects = 1;
- stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE; //information for statistics export
+ stats.similarity_type =
+ aron::similarity::NDArraySimilarity::Type::NONE; //information for statistics export
}
- SnapshotFilter::FilterStatistics SnapshotFrequencyFilter::getFilterStatistics()
+ SnapshotFilter::FilterStatistics
+ SnapshotFrequencyFilter::getFilterStatistics()
{
stats.end_time = std::chrono::high_resolution_clock::now();
return this->stats;
}
- std::string SnapshotFrequencyFilter::getName()
+ std::string
+ SnapshotFrequencyFilter::getName()
{
return this->NAME;
}
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
index 63e4493764e33d009b00b7b1de97001c27c49776..9f9ebccb0289195a3f3c9cbf5dbee31d67d26001 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/frequencyFilter/FrequencyFilter.h
@@ -5,7 +5,8 @@
namespace armarx::armem::server::ltm::processor::filter
{
- enum TimestampType{
+ enum TimestampType
+ {
NOT_SET = 0,
SENT = 1,
ARRIVED = 2,
@@ -21,20 +22,19 @@ namespace armarx::armem::server::ltm::processor::filter
SnapshotFrequencyFilter() = default;
- virtual bool accept(const armem::wm::EntitySnapshot &e, bool simulatedVersion) override;
+ virtual bool accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion) override;
void configure(const nlohmann::json& json) override;
FilterStatistics getFilterStatistics() override;
std::string getName() override;
private:
- void setNonCorruptedTimestampType(const armem::wm::EntitySnapshot &e);
- int getNonCorruptedTimestamp(const armem::wm::EntityInstance &i, bool simulatedVersion);
+ void setNonCorruptedTimestampType(const armem::wm::EntitySnapshot& e);
+ int getNonCorruptedTimestamp(const armem::wm::EntityInstance& i, bool simulatedVersion);
std::map<MemoryID, std::int64_t> lastTimesPerEntity;
int maxDifference = 0;
bool corruptedWarningGiven = false;
TimestampType nonCorruptedType = TimestampType::NOT_SET;
-
};
} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
index 06516bd7894d0974f2b9341952cfff1b6bc467de..27c4aacd38c61ea1b9da3f10900bcc4e9cfe755e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.cpp
@@ -1,84 +1,104 @@
#include "ImportanceFilter.h"
-
namespace armarx::armem::server::ltm::processor::filter
{
-bool SnapshotImportanceFilter::accept(const armem::wm::EntitySnapshot &e, bool simulatedVersion)
-{
- auto start = std::chrono::high_resolution_clock::now();
- bool instances_accepted = false;
+ bool
+ SnapshotImportanceFilter::accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion)
+ {
+ auto start = std::chrono::high_resolution_clock::now();
+ bool instances_accepted = false;
- e.forEachInstance([this, &instances_accepted](armem::wm::EntityInstance& i){
- if(!instances_accepted){ //if no instance was accepted yet
- instances_accepted = important(i); //see if another instance triggers acceptance
+ e.forEachInstance(
+ [this, &instances_accepted](armem::wm::EntityInstance& i)
+ {
+ if (!instances_accepted)
+ { //if no instance was accepted yet
+ instances_accepted = important(i); //see if another instance triggers acceptance
+ }
+ });
+
+ //set stats:
+ auto end = std::chrono::high_resolution_clock::now();
+ stats.end_time = end;
+ stats.additional_time += (end - start);
+ if (instances_accepted)
+ {
+ stats.accepted += 1;
+ }
+ else
+ {
+ stats.rejected += 1;
}
- });
- //set stats:
- auto end = std::chrono::high_resolution_clock::now();
- stats.end_time = end;
- stats.additional_time += (end - start);
- if(instances_accepted){
- stats.accepted += 1;
- } else {
- stats.rejected += 1;
+ return instances_accepted;
}
- return instances_accepted;
-}
-
-void SnapshotImportanceFilter::configure(const nlohmann::json &json)
-{
- if(json.find(PARAM_THRESHOLD) != json.end()){
- this->threshold = json.at(PARAM_THRESHOLD);
- ARMARX_INFO << VAROUT(this->threshold);
- stats.additional_info += "Threshold for importance: ";
- stats.additional_info += std::to_string(this->threshold);
- }
- if(json.find(PARAM_TYPE) != json.end()){
- auto t = json.at(PARAM_TYPE);
- if(t == "Confidence"){
- this->type = ImportanceType::CONFIDENCE;
+ void
+ SnapshotImportanceFilter::configure(const nlohmann::json& json)
+ {
+ if (json.find(PARAM_THRESHOLD) != json.end())
+ {
+ this->threshold = json.at(PARAM_THRESHOLD);
+ ARMARX_INFO << VAROUT(this->threshold);
+ stats.additional_info += "Threshold for importance: ";
+ stats.additional_info += std::to_string(this->threshold);
}
- if(t == "Accesses"){
- this->type = ImportanceType::ACCESSES;
+ if (json.find(PARAM_TYPE) != json.end())
+ {
+ auto t = json.at(PARAM_TYPE);
+ if (t == "Confidence")
+ {
+ this->type = ImportanceType::CONFIDENCE;
+ }
+ if (t == "Accesses")
+ {
+ this->type = ImportanceType::ACCESSES;
+ }
+ stats.importance_type = t;
+ ARMARX_INFO << VAROUT(stats.importance_type);
}
- stats.importance_type = t;
- ARMARX_INFO << VAROUT(stats.importance_type);
- }
- stats.start_time = std::chrono::high_resolution_clock::now();
- stats.number_of_compared_objects = 1;
- stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE; //information for statistics export
-}
+ stats.start_time = std::chrono::high_resolution_clock::now();
+ stats.number_of_compared_objects = 1;
+ stats.similarity_type =
+ aron::similarity::NDArraySimilarity::Type::NONE; //information for statistics export
+ }
-SnapshotFilter::FilterStatistics SnapshotImportanceFilter::getFilterStatistics()
-{
- return this->stats;
-}
+ SnapshotFilter::FilterStatistics
+ SnapshotImportanceFilter::getFilterStatistics()
+ {
+ return this->stats;
+ }
-std::string SnapshotImportanceFilter::getName()
-{
- return this->NAME;
-}
+ std::string
+ SnapshotImportanceFilter::getName()
+ {
+ return this->NAME;
+ }
-bool SnapshotImportanceFilter::important(armem::wm::EntityInstance &i)
-{
- if(this->type == ImportanceType::CONFIDENCE){
- auto c = i.metadata().confidence;
- if ( c > this->threshold){
- //for now the whole entity snapshot is accepted if at least one instance has a confidence higher than the threshold
- return true;
+ bool
+ SnapshotImportanceFilter::important(armem::wm::EntityInstance& i)
+ {
+ if (this->type == ImportanceType::CONFIDENCE)
+ {
+ auto c = i.metadata().confidence;
+ if (c > this->threshold)
+ {
+ //for now the whole entity snapshot is accepted if at least one instance has a confidence higher than the threshold
+ return true;
+ }
}
- } else if (this->type == ImportanceType::ACCESSES){
- auto a = i.metadata().numAccessed;
- if (a > this->threshold){
- return true;
+ else if (this->type == ImportanceType::ACCESSES)
+ {
+ auto a = i.metadata().numAccessed;
+ if (a > this->threshold)
+ {
+ return true;
+ }
}
+ return false;
}
- return false;
-}
-}
+} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h
index 5dcc76b57947a3a208dc1c3bf85ab190fce47dc3..f7e86d83dead2ec454ad023ce3376b0f35cb477e 100644
--- a/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h
+++ b/source/RobotAPI/libraries/armem/server/ltm/processors/filter/importanceFilter/ImportanceFilter.h
@@ -1,41 +1,40 @@
#pragma once
// Base Class
-#include "../Filter.h"
-
#include <chrono>
-namespace armarx::armem::server::ltm::processor::filter
-{
-
-enum ImportanceType{
- CONFIDENCE,
- ACCESSES
-};
+#include "../Filter.h"
-class SnapshotImportanceFilter : public SnapshotFilter
+namespace armarx::armem::server::ltm::processor::filter
{
-public:
- static const constexpr char* NAME = "SnapshotImportanceFilter";
- static const constexpr char* PARAM_THRESHOLD = "Threshold";
- static const constexpr char* PARAM_TYPE = "Type";
+ enum ImportanceType
+ {
+ CONFIDENCE,
+ ACCESSES
+ };
- SnapshotImportanceFilter() = default;
+ class SnapshotImportanceFilter : public SnapshotFilter
+ {
+ public:
+ static const constexpr char* NAME = "SnapshotImportanceFilter";
+ static const constexpr char* PARAM_THRESHOLD = "Threshold";
+ static const constexpr char* PARAM_TYPE = "Type";
- virtual bool accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion) override;
- void configure(const nlohmann::json& json) override;
+ SnapshotImportanceFilter() = default;
- FilterStatistics getFilterStatistics() override;
- std::string getName() override;
+ virtual bool accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion) override;
+ void configure(const nlohmann::json& json) override;
-private:
- double threshold = 1.0;
- FilterStatistics stats;
- ImportanceType type;
+ FilterStatistics getFilterStatistics() override;
+ std::string getName() override;
- bool important(armem::wm::EntityInstance& i);
+ private:
+ double threshold = 1.0;
+ FilterStatistics stats;
+ ImportanceType type;
-};
+ bool important(armem::wm::EntityInstance& i);
+ };
-}
+} // namespace armarx::armem::server::ltm::processor::filter
diff --git a/source/RobotAPI/libraries/armem/server/plugins.h b/source/RobotAPI/libraries/armem/server/plugins.h
index 9f1255994daba045756a8c90dcbcfd57a9c63a11..adb8dfe21ec3b573570147707e804c9d378a704e 100644
--- a/source/RobotAPI/libraries/armem/server/plugins.h
+++ b/source/RobotAPI/libraries/armem/server/plugins.h
@@ -4,17 +4,16 @@
#include "plugins/ReadOnlyPluginUser.h"
#include "plugins/ReadWritePluginUser.h"
-
namespace armarx::armem::server
{
using plugins::Plugin;
using plugins::ReadOnlyPluginUser;
using plugins::ReadWritePluginUser;
-}
+} // namespace armarx::armem::server
namespace armarx::armem
{
using ServerPlugin = server::Plugin;
using ReadOnlyServerPluginUser = server::ReadOnlyPluginUser;
using ReadWriteServerPluginUser = server::ReadWritePluginUser;
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index ddc51ddbc9b9699302041fc37718983b6c808b4f..2b03b5f81b923b6c37b575361e316e293f1074a7 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -83,7 +83,7 @@ namespace armarx::armem::server::plugins
// register new server info to MNS
if (clientPlugin->isMemoryNameSystemEnabled() and clientPlugin->getMemoryNameSystemClient())
{
- registerServer(parent);
+ registerServer(parent);
}
}
@@ -103,15 +103,25 @@ namespace armarx::armem::server::plugins
Plugin::preOnDisconnectComponent()
{
//Storing rest of WM into LTM when component is shut down:
- try{
- if(longtermMemory.p.storeOnStop){
+ try
+ {
+ if (longtermMemory.p.storeOnStop)
+ {
longtermMemory.directlyStore(workingMemory);
- ARMARX_INFO << "Stored working memory contents into long-term memory on component stop";
- } else {
- ARMARX_DEBUG << "Not storing WM into LTM on stop, as longtermMemory.p.storeOnstop is " << longtermMemory.p.storeOnStop;
+ ARMARX_INFO
+ << "Stored working memory contents into long-term memory on component stop";
+ }
+ else
+ {
+ ARMARX_DEBUG
+ << "Not storing WM into LTM on stop, as longtermMemory.p.storeOnstop is "
+ << longtermMemory.p.storeOnStop;
}
- } catch(...){
- ARMARX_WARNING << "Could not store working memory into the long-term memory on component stop.";
+ }
+ catch (...)
+ {
+ ARMARX_WARNING
+ << "Could not store working memory into the long-term memory on component stop.";
}
//Storing statistics aout LTM recording:
try
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.cpp
index e8805d52528f5352830bb537336abbb5be5de484..1827614664251c7c322eb95525dc20119f72bd5d 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.cpp
@@ -1,13 +1,12 @@
#include "ReadOnlyPluginUser.h"
-#include "Plugin.h"
-
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include "Plugin.h"
namespace armarx::armem::server::plugins
{
@@ -17,46 +16,46 @@ namespace armarx::armem::server::plugins
addPlugin(plugin);
}
-
ReadOnlyPluginUser::~ReadOnlyPluginUser()
{
}
-
- void ReadOnlyPluginUser::setMemoryName(const std::string& memoryName)
+ void
+ ReadOnlyPluginUser::setMemoryName(const std::string& memoryName)
{
plugin->setMemoryName(memoryName);
}
-
- armem::query::data::Result ReadOnlyPluginUser::query(const armem::query::data::Input& input, const Ice::Current&)
+ armem::query::data::Result
+ ReadOnlyPluginUser::query(const armem::query::data::Input& input, const Ice::Current&)
{
ARMARX_TRACE;
return iceAdapter().query(input);
}
- structure::data::GetServerStructureResult ReadOnlyPluginUser::getServerStructure(const Ice::Current&)
+ structure::data::GetServerStructureResult
+ ReadOnlyPluginUser::getServerStructure(const Ice::Current&)
{
ARMARX_TRACE;
return iceAdapter().getServerStructure();
}
-
- Plugin& ReadOnlyPluginUser::memoryServerPlugin()
+ Plugin&
+ ReadOnlyPluginUser::memoryServerPlugin()
{
return *plugin;
}
-
- wm::Memory& ReadOnlyPluginUser::workingMemory()
+ wm::Memory&
+ ReadOnlyPluginUser::workingMemory()
{
return plugin->workingMemory;
}
-
- MemoryToIceAdapter& ReadOnlyPluginUser::iceAdapter()
+ MemoryToIceAdapter&
+ ReadOnlyPluginUser::iceAdapter()
{
return plugin->iceAdapter;
}
-}
+} // namespace armarx::armem::server::plugins
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.h b/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.h
index d234819b931794bb0b562f4a33d462baf16a9cfe..570eed7931e1ebde6bc0f543f6cbb1835baa651d 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadOnlyPluginUser.h
@@ -1,31 +1,27 @@
#pragma once
-#include <RobotAPI/libraries/armem/server/forward_declarations.h>
-
-#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
-
#include <ArmarXCore/core/ManagedIceObject.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
namespace armarx::armem::server::plugins
{
class Plugin;
-
/**
* @brief Base class of memory server components.
*
* Implements the server ice interfaces using the ice adapter of the plugin.
*/
class ReadOnlyPluginUser :
- virtual public ManagedIceObject
- , virtual public ReadingMemoryInterface
- , virtual public client::plugins::PluginUser
+ virtual public ManagedIceObject,
+ virtual public ReadingMemoryInterface,
+ virtual public client::plugins::PluginUser
{
public:
-
ReadOnlyPluginUser();
virtual ~ReadOnlyPluginUser() override;
@@ -34,9 +30,8 @@ namespace armarx::armem::server::plugins
// ReadingInterface interface
- virtual armem::query::data::Result query(
- const armem::query::data::Input& input,
- const Ice::Current& = Ice::emptyCurrent) override;
+ virtual armem::query::data::Result query(const armem::query::data::Input& input,
+ const Ice::Current& = Ice::emptyCurrent) override;
virtual armem::structure::data::GetServerStructureResult
@@ -44,7 +39,6 @@ namespace armarx::armem::server::plugins
public:
-
Plugin& memoryServerPlugin();
server::wm::Memory& workingMemory();
@@ -52,12 +46,10 @@ namespace armarx::armem::server::plugins
private:
-
plugins::Plugin* plugin = nullptr;
-
};
-}
+} // namespace armarx::armem::server::plugins
namespace armarx::armem::server
{
diff --git a/source/RobotAPI/libraries/armem/server/query_proc.h b/source/RobotAPI/libraries/armem/server/query_proc.h
index ad2cdb26de85b67951a9bbe26715218f09760c0f..8c92585ec2381b91f882bde7243be61c5631567e 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc.h
@@ -1,8 +1,6 @@
#pragma once
-#include "query_proc/MemoryQueryProcessor.h"
#include "query_proc/CoreSegmentQueryProcessor.h"
-#include "query_proc/ProviderSegmentQueryProcessor.h"
#include "query_proc/EntityQueryProcessor.h"
-
-
+#include "query_proc/MemoryQueryProcessor.h"
+#include "query_proc/ProviderSegmentQueryProcessor.h"
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base.h b/source/RobotAPI/libraries/armem/server/query_proc/base.h
index 1d826798cecac7371882ce0783ddc05414a4f237..e77694af44941e9a88b70b8b4dfd4aa15d7b1186 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base.h
@@ -1,10 +1,9 @@
#pragma once
-#include <RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h>
-#include <RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h>
#include <RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h>
+#include <RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h>
#include <RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h>
-
+#include <RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h>
namespace armarx::armem::server::query_proc::base
{
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
index ae1dedc1427790bf40a0df8a661942dad9b79668..5c57cf54add3c602acf2814f1489a9d71fb07acb 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.cpp
@@ -2,7 +2,6 @@
#include <ArmarXCore/core/logging/Logging.h>
-
namespace armarx::armem::server::query_proc::base
{
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
index 709457fb57b8d03b4c624c69199fae6812ee50d0..9bf271cc7221789967a408fac290a3161e84345d 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
@@ -1,19 +1,18 @@
#pragma once
-#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/query/QueryTarget.h>
+#include <set>
+#include <vector>
#include <Ice/Handle.h>
-#include <set>
-#include <vector>
+#include <RobotAPI/interface/armem/query.h>
+#include <RobotAPI/libraries/armem/core/query/QueryTarget.h>
namespace armarx::armem::server::query_proc::base
{
using QueryTarget = armem::query::data::QueryTarget::QueryTargetEnum;
-
/**
* @brief Base class for memory query processors.
*/
@@ -21,35 +20,37 @@ namespace armarx::armem::server::query_proc::base
class BaseQueryProcessorBase
{
public:
-
using QueryPtrT = ::IceInternal::Handle<QueryT>;
using QuerySeqT = std::vector<QueryPtrT>;
public:
-
virtual ~BaseQueryProcessorBase() = default;
- ResultT process(const QueryT& query, const DataT& data) const
+ ResultT
+ process(const QueryT& query, const DataT& data) const
{
- ResultT result { data.id() };
+ ResultT result{data.id()};
this->process(result, query, data);
return result;
}
- ResultT process(const QueryPtrT& query, const DataT& data) const
+ ResultT
+ process(const QueryPtrT& query, const DataT& data) const
{
return this->process(*query, *data);
}
- ResultT process(const QuerySeqT& queries, const DataT& data) const
+ ResultT
+ process(const QuerySeqT& queries, const DataT& data) const
{
- ResultT result { data.id() };
+ ResultT result{data.id()};
this->process(result, queries, data);
return result;
}
- void process(ResultT& result, const QuerySeqT& queries, const DataT& data) const
+ void
+ process(ResultT& result, const QuerySeqT& queries, const DataT& data) const
{
if (queries.empty())
{
@@ -62,7 +63,6 @@ namespace armarx::armem::server::query_proc::base
}
}
-
/**
* @brief Process the query and populate `result`.
*
@@ -71,8 +71,7 @@ namespace armarx::armem::server::query_proc::base
* @param data The source container.
*/
virtual void process(ResultT& result, const QueryT& query, const DataT& data) const = 0;
-
};
-}
+} // namespace armarx::armem::server::query_proc::base
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
index 70506550c02b9091a48186b04f772bb607ccf29f..1baa704ea7cb3e3ad6d4e022480dcdd9f1c586aa 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/CoreSegmentQueryProcessorBase.h
@@ -1,12 +1,11 @@
#pragma once
-#include "BaseQueryProcessorBase.h"
+#include <regex>
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/interface/armem/query.h>
+#include <RobotAPI/libraries/armem/core/error.h>
-#include <regex>
-
+#include "BaseQueryProcessorBase.h"
namespace armarx::armem::server::query_proc::base
{
@@ -16,14 +15,16 @@ namespace armarx::armem::server::query_proc::base
*/
template <class _CoreSegmentT, class _ResultCoreSegmentT, class _ChildProcessorT>
class CoreSegmentQueryProcessorBase :
- public BaseQueryProcessorBase<_CoreSegmentT, _ResultCoreSegmentT, armem::query::data::CoreSegmentQuery>
+ public BaseQueryProcessorBase<_CoreSegmentT,
+ _ResultCoreSegmentT,
+ armem::query::data::CoreSegmentQuery>
{
protected:
-
- using Base = BaseQueryProcessorBase<_CoreSegmentT, _ResultCoreSegmentT, armem::query::data::CoreSegmentQuery>;
+ using Base = BaseQueryProcessorBase<_CoreSegmentT,
+ _ResultCoreSegmentT,
+ armem::query::data::CoreSegmentQuery>;
public:
-
using CoreSegmentT = _CoreSegmentT;
using ProviderSegmentT = typename CoreSegmentT::ProviderSegmentT;
@@ -34,20 +35,21 @@ namespace armarx::armem::server::query_proc::base
public:
-
CoreSegmentQueryProcessorBase()
{
}
+
CoreSegmentQueryProcessorBase(ChildProcessorT&& childProcessor) :
childProcessor(childProcessor)
{
}
-
using Base::process;
- virtual void process(ResultCoreSegmentT& result,
- const armem::query::data::CoreSegmentQuery& query,
- const CoreSegmentT& coreSegment) const override
+
+ virtual void
+ process(ResultCoreSegmentT& result,
+ const armem::query::data::CoreSegmentQuery& query,
+ const CoreSegmentT& coreSegment) const override
{
if (auto q = dynamic_cast<const armem::query::data::core::All*>(&query))
{
@@ -67,19 +69,20 @@ namespace armarx::armem::server::query_proc::base
}
}
- virtual void process(ResultCoreSegmentT& result,
- const armem::query::data::core::All& query,
- const CoreSegmentT& coreSegment) const
+ virtual void
+ process(ResultCoreSegmentT& result,
+ const armem::query::data::core::All& query,
+ const CoreSegmentT& coreSegment) const
{
- coreSegment.forEachProviderSegment([this, &query, &result](const ProviderSegmentT & providerSegment)
- {
- this->_processResult(result, providerSegment, query);
- });
+ coreSegment.forEachProviderSegment(
+ [this, &query, &result](const ProviderSegmentT& providerSegment)
+ { this->_processResult(result, providerSegment, query); });
}
- virtual void process(ResultCoreSegmentT& result,
- const armem::query::data::core::Single& query,
- const CoreSegmentT& coreSegment) const
+ virtual void
+ process(ResultCoreSegmentT& result,
+ const armem::query::data::core::Single& query,
+ const CoreSegmentT& coreSegment) const
{
if (auto providerSegment = coreSegment.findProviderSegment(query.providerSegmentName))
{
@@ -87,40 +90,40 @@ namespace armarx::armem::server::query_proc::base
}
}
- virtual void process(ResultCoreSegmentT& result,
- const armem::query::data::core::Regex& query,
- const CoreSegmentT& coreSegment) const
+ virtual void
+ process(ResultCoreSegmentT& result,
+ const armem::query::data::core::Regex& query,
+ const CoreSegmentT& coreSegment) const
{
const std::regex regex(query.providerSegmentNameRegex);
coreSegment.forEachProviderSegment(
- [this, &result, &query, ®ex](const ProviderSegmentT & providerSegment)
- {
- if (std::regex_search(providerSegment.name(), regex))
+ [this, &result, &query, ®ex](const ProviderSegmentT& providerSegment)
{
- this->_processResult(result, providerSegment, query);
- }
- });
+ if (std::regex_search(providerSegment.name(), regex))
+ {
+ this->_processResult(result, providerSegment, query);
+ }
+ });
}
protected:
-
- void _processResult(ResultCoreSegmentT& result,
- const ProviderSegmentT& providerSegment,
- const armem::query::data::CoreSegmentQuery& query) const
+ void
+ _processResult(ResultCoreSegmentT& result,
+ const ProviderSegmentT& providerSegment,
+ const armem::query::data::CoreSegmentQuery& query) const
{
ResultProviderSegmentT* child = result.findProviderSegment(providerSegment.name());
if (child == nullptr)
{
- child = &result.addProviderSegment(providerSegment.name(), providerSegment.aronType());
+ child =
+ &result.addProviderSegment(providerSegment.name(), providerSegment.aronType());
}
childProcessor.process(*child, query.providerSegmentQueries, providerSegment);
}
protected:
-
ChildProcessorT childProcessor;
-
};
-}
+} // namespace armarx::armem::server::query_proc::base
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
index 645b8aaa33fa6669135caeb49031e11ca2d8bb6c..89ec2bc88a793ae63527d93c4addc80b23c5e3bd 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.cpp
@@ -2,11 +2,12 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::armem::server::query_proc::base
{
- void detail::checkReferenceTimestampNonNegative(const Time& timestamp)
+ void
+ detail::checkReferenceTimestampNonNegative(const Time& timestamp)
{
- ARMARX_CHECK_NONNEGATIVE(timestamp.toMicroSecondsSinceEpoch()) << "Reference timestamp must be non-negative.";
+ ARMARX_CHECK_NONNEGATIVE(timestamp.toMicroSecondsSinceEpoch())
+ << "Reference timestamp must be non-negative.";
}
-}
+} // namespace armarx::armem::server::query_proc::base
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index e5578ace37104b5435bb0953c3f1912b79219b84..a1ea37f906e6ab25e9332b6a9fa2575cbd6b7808 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -1,24 +1,23 @@
#pragma once
-#include "BaseQueryProcessorBase.h"
+#include <cstdint>
+#include <iterator>
-#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <RobotAPI/interface/armem/query.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-#include <RobotAPI/interface/armem/query.h>
-
-#include <cstdint>
-#include <iterator>
-
+#include "BaseQueryProcessorBase.h"
namespace armarx::armem::server::query_proc::base::detail
{
void checkReferenceTimestampNonNegative(const Time& timestamp);
}
+
namespace armarx::armem::server::query_proc::base
{
@@ -27,11 +26,10 @@ namespace armarx::armem::server::query_proc::base
public BaseQueryProcessorBase<_EntityT, _ResultEntityT, armem::query::data::EntityQuery>
{
protected:
-
- using Base = BaseQueryProcessorBase<_EntityT, _ResultEntityT, armem::query::data::EntityQuery>;
+ using Base =
+ BaseQueryProcessorBase<_EntityT, _ResultEntityT, armem::query::data::EntityQuery>;
public:
-
using EntityT = _EntityT;
using EntitySnapshotT = typename EntityT::EntitySnapshotT;
@@ -40,11 +38,12 @@ namespace armarx::armem::server::query_proc::base
public:
-
using Base::process;
- virtual void process(ResultEntityT& result,
- const armem::query::data::EntityQuery& query,
- const EntityT& entity) const override
+
+ virtual void
+ process(ResultEntityT& result,
+ const armem::query::data::EntityQuery& query,
+ const EntityT& entity) const override
{
if (auto q = dynamic_cast<const armem::query::data::entity::All*>(&query))
{
@@ -66,7 +65,8 @@ namespace armarx::armem::server::query_proc::base
{
process(result, *q, entity);
}
- else if (auto q = dynamic_cast<const armem::query::data::entity::BeforeOrAtTime*>(&query))
+ else if (auto q =
+ dynamic_cast<const armem::query::data::entity::BeforeOrAtTime*>(&query))
{
process(result, *q, entity);
}
@@ -80,24 +80,22 @@ namespace armarx::armem::server::query_proc::base
}
}
-
- virtual void process(ResultEntityT& result,
- const armem::query::data::entity::All& query,
- const EntityT& entity) const
+ virtual void
+ process(ResultEntityT& result,
+ const armem::query::data::entity::All& query,
+ const EntityT& entity) const
{
- (void) query;
+ (void)query;
// Copy this entitiy and its contents.
entity.forEachSnapshot([this, &result](const EntitySnapshotT& snapshot)
- {
- this->addResultSnapshot(result, snapshot);
- });
+ { this->addResultSnapshot(result, snapshot); });
}
-
- virtual void process(ResultEntityT& result,
- const armem::query::data::entity::Single& query,
- const EntityT& entity) const
+ virtual void
+ process(ResultEntityT& result,
+ const armem::query::data::entity::Single& query,
+ const EntityT& entity) const
{
if (query.timestamp.timeSinceEpoch.microSeconds < 0)
{
@@ -132,14 +130,15 @@ namespace armarx::armem::server::query_proc::base
}
}
-
- virtual void process(ResultEntityT& result,
- const armem::query::data::entity::TimeRange& query,
- const EntityT& entity) const
+ virtual void
+ process(ResultEntityT& result,
+ const armem::query::data::entity::TimeRange& query,
+ const EntityT& entity) const
{
- if (query.minTimestamp.timeSinceEpoch.microSeconds <= query.maxTimestamp.timeSinceEpoch.microSeconds
- || query.minTimestamp.timeSinceEpoch.microSeconds < 0
- || query.maxTimestamp.timeSinceEpoch.microSeconds < 0)
+ if (query.minTimestamp.timeSinceEpoch.microSeconds <=
+ query.maxTimestamp.timeSinceEpoch.microSeconds ||
+ query.minTimestamp.timeSinceEpoch.microSeconds < 0 ||
+ query.maxTimestamp.timeSinceEpoch.microSeconds < 0)
{
const Time min = armarx::fromIce<Time>(query.minTimestamp);
const Time max = armarx::fromIce<Time>(query.maxTimestamp);
@@ -147,39 +146,35 @@ namespace armarx::armem::server::query_proc::base
}
}
-
- virtual void process(ResultEntityT& result,
- const armem::query::data::entity::IndexRange& query,
- const EntityT& entity) const
+ virtual void
+ process(ResultEntityT& result,
+ const armem::query::data::entity::IndexRange& query,
+ const EntityT& entity) const
{
- entity.forEachSnapshotInIndexRange(
- query.first, query.last,
- [this, &result](const EntitySnapshotT & snapshot)
- {
- this->addResultSnapshot(result, snapshot);
- });
+ entity.forEachSnapshotInIndexRange(query.first,
+ query.last,
+ [this, &result](const EntitySnapshotT& snapshot)
+ { this->addResultSnapshot(result, snapshot); });
}
-
- virtual void process(ResultEntityT& result,
- const Time& min,
- const Time& max,
- const EntityT& entity,
- const armem::query::data::EntityQuery& query) const
+ virtual void
+ process(ResultEntityT& result,
+ const Time& min,
+ const Time& max,
+ const EntityT& entity,
+ const armem::query::data::EntityQuery& query) const
{
- (void) query;
- entity.forEachSnapshotInTimeRange(
- min, max,
- [this, &result](const EntitySnapshotT & snapshot)
- {
- this->addResultSnapshot(result, snapshot);
- });
+ (void)query;
+ entity.forEachSnapshotInTimeRange(min,
+ max,
+ [this, &result](const EntitySnapshotT& snapshot)
+ { this->addResultSnapshot(result, snapshot); });
}
-
- virtual void process(ResultEntityT& result,
- const armem::query::data::entity::BeforeOrAtTime& query,
- const EntityT& entity) const
+ virtual void
+ process(ResultEntityT& result,
+ const armem::query::data::entity::BeforeOrAtTime& query,
+ const EntityT& entity) const
{
const Time referenceTimestamp = armarx::fromIce<Time>(query.timestamp);
base::detail::checkReferenceTimestampNonNegative(referenceTimestamp);
@@ -190,19 +185,18 @@ namespace armarx::armem::server::query_proc::base
}
}
-
- virtual void process(ResultEntityT& result,
- const armem::query::data::entity::BeforeTime& query,
- const EntityT& entity) const
+ virtual void
+ process(ResultEntityT& result,
+ const armem::query::data::entity::BeforeTime& query,
+ const EntityT& entity) const
{
const Time referenceTimestamp = armarx::fromIce<Time>(query.timestamp);
base::detail::checkReferenceTimestampNonNegative(referenceTimestamp);
std::vector<const EntitySnapshotT*> befores;
- entity.forEachSnapshotBefore(referenceTimestamp, [&befores](const EntitySnapshotT & s)
- {
- befores.push_back(&s);
- });
+ entity.forEachSnapshotBefore(referenceTimestamp,
+ [&befores](const EntitySnapshotT& s)
+ { befores.push_back(&s); });
size_t num = 0;
if (query.maxEntries < 0)
@@ -221,19 +215,20 @@ namespace armarx::armem::server::query_proc::base
}
}
-
- virtual void process(ResultEntityT& result,
- const armem::query::data::entity::TimeApprox& query,
- const EntityT& entity) const
+ virtual void
+ process(ResultEntityT& result,
+ const armem::query::data::entity::TimeApprox& query,
+ const EntityT& entity) const
{
const Time referenceTimestamp = armarx::fromIce<Time>(query.timestamp);
base::detail::checkReferenceTimestampNonNegative(referenceTimestamp);
// elements have to be in range [t_ref - eps, t_ref + eps] if eps is positive
- const auto isInRange = [&](const Time & t) -> bool
+ const auto isInRange = [&](const Time& t) -> bool
{
- return query.eps.microSeconds <= 0
- or std::abs((t - referenceTimestamp).toMicroSeconds()) <= query.eps.microSeconds;
+ return query.eps.microSeconds <= 0 or
+ std::abs((t - referenceTimestamp).toMicroSeconds()) <=
+ query.eps.microSeconds;
};
// last element before or at timestamp
@@ -267,8 +262,7 @@ namespace armarx::armem::server::query_proc::base
protected:
-
- virtual void addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const = 0;
-
+ virtual void addResultSnapshot(ResultEntityT& result,
+ const EntitySnapshotT& snapshot) const = 0;
};
-}
+} // namespace armarx::armem::server::query_proc::base
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
index 240e38a375ef7c2a438d7169b72234e63a54e10f..28194bc1aa8a92b5d630ea0e0cfeb4bf1bc7406f 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/ProviderSegmentQueryProcessorBase.h
@@ -1,28 +1,28 @@
#pragma once
-#include "BaseQueryProcessorBase.h"
-
-#include <RobotAPI/libraries/armem/core/error.h>
+#include <regex>
#include <RobotAPI/interface/armem/query.h>
+#include <RobotAPI/libraries/armem/core/error.h>
-#include <regex>
-
+#include "BaseQueryProcessorBase.h"
namespace armarx::armem::server::query_proc::base
{
template <class _ProviderSegmentT, class _ResultProviderSegmentT, class _ChildProcessorT>
class ProviderSegmentQueryProcessorBase :
- public BaseQueryProcessorBase<_ProviderSegmentT, _ResultProviderSegmentT, armem::query::data::ProviderSegmentQuery>
+ public BaseQueryProcessorBase<_ProviderSegmentT,
+ _ResultProviderSegmentT,
+ armem::query::data::ProviderSegmentQuery>
{
protected:
-
- using Base = BaseQueryProcessorBase<_ProviderSegmentT, _ResultProviderSegmentT, armem::query::data::ProviderSegmentQuery>;
+ using Base = BaseQueryProcessorBase<_ProviderSegmentT,
+ _ResultProviderSegmentT,
+ armem::query::data::ProviderSegmentQuery>;
public:
-
using ProviderSegmentT = _ProviderSegmentT;
using EntityT = typename ProviderSegmentT::EntityT;
@@ -33,19 +33,21 @@ namespace armarx::armem::server::query_proc::base
public:
-
ProviderSegmentQueryProcessorBase()
{
}
+
ProviderSegmentQueryProcessorBase(ChildProcessorT&& childProcessor) :
childProcessor(childProcessor)
{
}
using Base::process;
- virtual void process(ResultProviderSegmentT& result,
- const armem::query::data::ProviderSegmentQuery& query,
- const ProviderSegmentT& providerSegment) const override
+
+ virtual void
+ process(ResultProviderSegmentT& result,
+ const armem::query::data::ProviderSegmentQuery& query,
+ const ProviderSegmentT& providerSegment) const override
{
if (auto q = dynamic_cast<const armem::query::data::provider::All*>(&query))
{
@@ -65,19 +67,19 @@ namespace armarx::armem::server::query_proc::base
}
}
- virtual void process(ResultProviderSegmentT& result,
- const armem::query::data::provider::All& query,
- const ProviderSegmentT& providerSegment) const
+ virtual void
+ process(ResultProviderSegmentT& result,
+ const armem::query::data::provider::All& query,
+ const ProviderSegmentT& providerSegment) const
{
providerSegment.forEachEntity([this, &result, &query](const EntityT& entity)
- {
- this->_processResult(result, entity, query);
- });
+ { this->_processResult(result, entity, query); });
}
- virtual void process(ResultProviderSegmentT& result,
- const armem::query::data::provider::Single& query,
- const ProviderSegmentT& providerSegment) const
+ virtual void
+ process(ResultProviderSegmentT& result,
+ const armem::query::data::provider::Single& query,
+ const ProviderSegmentT& providerSegment) const
{
if (auto entity = providerSegment.findEntity(query.entityName))
{
@@ -85,27 +87,29 @@ namespace armarx::armem::server::query_proc::base
}
}
- virtual void process(ResultProviderSegmentT& result,
- const armem::query::data::provider::Regex& query,
- const ProviderSegmentT& providerSegment) const
+ virtual void
+ process(ResultProviderSegmentT& result,
+ const armem::query::data::provider::Regex& query,
+ const ProviderSegmentT& providerSegment) const
{
const std::regex regex(query.entityNameRegex);
- providerSegment.forEachEntity([this, &result, &query, ®ex](const EntityT& entity)
- {
- if (std::regex_search(entity.name(), regex))
+ providerSegment.forEachEntity(
+ [this, &result, &query, ®ex](const EntityT& entity)
{
- this->_processResult(result, entity, query);
- }
- return true;
- });
+ if (std::regex_search(entity.name(), regex))
+ {
+ this->_processResult(result, entity, query);
+ }
+ return true;
+ });
}
protected:
-
- void _processResult(ResultProviderSegmentT& result,
- const EntityT& entity,
- const armem::query::data::ProviderSegmentQuery& query) const
+ void
+ _processResult(ResultProviderSegmentT& result,
+ const EntityT& entity,
+ const armem::query::data::ProviderSegmentQuery& query) const
{
ResultEntityT* child = result.findEntity(entity.name());
if (child == nullptr)
@@ -117,8 +121,6 @@ namespace armarx::armem::server::query_proc::base
protected:
-
ChildProcessorT childProcessor;
-
};
-}
+} // namespace armarx::armem::server::query_proc::base
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.h
index f1c52ecc827d05aeef96d101cb2b8762ae12f5a4..4a91d1e25bf4a0189afc0af1e4e514de114f89a2 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/CoreSegmentQueryProcessorBase.h
@@ -2,7 +2,6 @@
#include "../../base/CoreSegmentQueryProcessorBase.h"
-
namespace armarx::armem::server::query_proc::ltm::detail
{
@@ -11,11 +10,12 @@ namespace armarx::armem::server::query_proc::ltm::detail
*/
template <class _CoreSegmentT, class _ResultCoreSegmentT, class _ChildProcessorT>
class CoreSegmentQueryProcessorBase :
- public base::CoreSegmentQueryProcessorBase<_CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>
+ public base::
+ CoreSegmentQueryProcessorBase<_CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>
{
protected:
-
- using Base = base::CoreSegmentQueryProcessorBase<_CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>;
+ using Base = base::
+ CoreSegmentQueryProcessorBase<_CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>;
public:
@@ -30,7 +30,5 @@ namespace armarx::armem::server::query_proc::ltm::detail
virtual ~CoreSegmentQueryProcessorBase() = default;
using Base::process;
-
-
};
-}
+} // namespace armarx::armem::server::query_proc::ltm::detail
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.cpp
index e01bbbf5a0d6862f75d245fef36d42e752ad42ef..928e1b8b8408b3b0bb8a9b170f2f2874f93220e9 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.cpp
@@ -2,7 +2,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::armem::server::query_proc::base
{
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h
index 5d3f9b0ed7e2128be282661107886986c6edc1da..545ac55171ad0488ba1c93253c25bcdbd3876376 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/EntityQueryProcessorBase.h
@@ -1,19 +1,16 @@
#pragma once
-#include "../../base/EntityQueryProcessorBase.h"
-
#include <ArmarXCore/core/logging/Logging.h>
+#include "../../base/EntityQueryProcessorBase.h"
namespace armarx::armem::server::query_proc::ltm::detail
{
template <class _EntityT, class _ResultEntityT>
- class EntityQueryProcessorBase :
- public base::EntityQueryProcessorBase<_EntityT, _ResultEntityT>
+ class EntityQueryProcessorBase : public base::EntityQueryProcessorBase<_EntityT, _ResultEntityT>
{
protected:
-
using Base = base::EntityQueryProcessorBase<_EntityT, _ResultEntityT>;
public:
@@ -30,11 +27,12 @@ namespace armarx::armem::server::query_proc::ltm::detail
protected:
// default addResultSnapshot method. Always copies the data
- void addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const override
+ void
+ addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const override
{
ResultSnapshotT s;
snapshot.loadAllReferences(s);
result.addSnapshot(s);
}
};
-}
+} // namespace armarx::armem::server::query_proc::ltm::detail
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/MemoryQueryProcessorBase.h
index 8ae444bc831046d435566987bfcb9664ee951f25..c076044cc152ef8007fda2f642547e1d679d0b12 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/MemoryQueryProcessorBase.h
@@ -2,7 +2,6 @@
#include "../../base/MemoryQueryProcessorBase.h"
-
namespace armarx::armem::server::query_proc::ltm::detail
{
@@ -11,11 +10,9 @@ namespace armarx::armem::server::query_proc::ltm::detail
public base::MemoryQueryProcessorBase<_MemoryT, _ResultMemoryT, _ChildProcessorT>
{
protected:
-
using Base = base::MemoryQueryProcessorBase<_MemoryT, _ResultMemoryT, _ChildProcessorT>;
public:
-
using MemoryT = typename Base::MemoryT;
using CoreSegmentT = typename Base::CoreSegmentT;
using ResultMemoryT = typename Base::ResultMemoryT;
@@ -29,13 +26,11 @@ namespace armarx::armem::server::query_proc::ltm::detail
using Base::process;
protected:
-
- bool _processAllowed(const armem::query::data::MemoryQuery& query) const final
+ bool
+ _processAllowed(const armem::query::data::MemoryQuery& query) const final
{
// only execute if query target is correct
return query.target == armem::query::data::QueryTarget::WM_LTM;
}
-
-
};
-}
+} // namespace armarx::armem::server::query_proc::ltm::detail
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.h
index 0559e2716d74e94fd5b070b8cc25229ab34924a4..89d535955aa7cb78bfa0a4fb325e7d635b5385d0 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/detail/ProviderSegmentQueryProcessorBase.h
@@ -2,20 +2,21 @@
#include "../../base/ProviderSegmentQueryProcessorBase.h"
-
namespace armarx::armem::server::query_proc::ltm::detail
{
template <class _ProviderSegmentT, class _ResultProviderSegmentT, class _ChildProcessorT>
class ProviderSegmentQueryProcessorBase :
- public base::ProviderSegmentQueryProcessorBase<_ProviderSegmentT, _ResultProviderSegmentT, _ChildProcessorT>
+ public base::ProviderSegmentQueryProcessorBase<_ProviderSegmentT,
+ _ResultProviderSegmentT,
+ _ChildProcessorT>
{
protected:
-
- using Base = base::ProviderSegmentQueryProcessorBase<_ProviderSegmentT, _ResultProviderSegmentT, _ChildProcessorT>;
+ using Base = base::ProviderSegmentQueryProcessorBase<_ProviderSegmentT,
+ _ResultProviderSegmentT,
+ _ChildProcessorT>;
public:
-
using ProviderSegmentT = typename Base::ProviderSegmentT;
using EntityT = typename Base::EntityT;
using ResultProviderSegmentT = typename Base::ResultProviderSegmentT;
@@ -27,7 +28,5 @@ namespace armarx::armem::server::query_proc::ltm::detail
virtual ~ProviderSegmentQueryProcessorBase() = default;
using Base::process;
-
-
};
-}
+} // namespace armarx::armem::server::query_proc::ltm::detail
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.cpp b/source/RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.cpp
index cf48f8897c8b04b35a14086ae6cc93aed2f1b5cb..385581b186beb86606e76a98de7a40efeae76915 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.cpp
@@ -1,7 +1,5 @@
#include "ltm.h"
-
namespace armarx::armem::server::query_proc::ltm_server::disk
{
}
-
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.h
index 70a8f32e4554d2743346e2fae6f7dddc303c0c50..58f5c06ad2b1633d078f84267050d17315d36420 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/CoreSegmentQueryProcessorBase.h
@@ -2,7 +2,6 @@
#include "../../base/CoreSegmentQueryProcessorBase.h"
-
namespace armarx::armem::server::query_proc::wm::detail
{
@@ -11,15 +10,15 @@ namespace armarx::armem::server::query_proc::wm::detail
*/
template <class _CoreSegmentT, class _ResultCoreSegmentT, class _ChildProcessorT>
class CoreSegmentQueryProcessorBase :
- public base::CoreSegmentQueryProcessorBase<_CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>
+ public base::
+ CoreSegmentQueryProcessorBase<_CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>
{
protected:
-
- using Base = base::CoreSegmentQueryProcessorBase<_CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>;
+ using Base = base::
+ CoreSegmentQueryProcessorBase<_CoreSegmentT, _ResultCoreSegmentT, _ChildProcessorT>;
public:
-
using CoreSegmentT = typename Base::CoreSegmentT;
using ProviderSegmentT = typename Base::ProviderSegmentT;
using ResultCoreSegmentT = typename Base::ResultCoreSegmentT;
@@ -28,12 +27,9 @@ namespace armarx::armem::server::query_proc::wm::detail
public:
-
using Base::CoreSegmentQueryProcessorBase;
virtual ~CoreSegmentQueryProcessorBase() = default;
using Base::process;
-
-
};
-}
+} // namespace armarx::armem::server::query_proc::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.cpp
index c5923363004f6b036e5d4756731687bbe96f82b9..c3f531481650949bcfc2cedd4170edcc7a6d4b1f 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.cpp
@@ -2,7 +2,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::armem::server::query_proc::wm::detail
{
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.h
index 5fbd2300c13561d64075720d399bc0e4c87633f0..ea4029f1ddbbef963f6b412756eaf3959dc446e1 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/EntityQueryProcessorBase.h
@@ -1,23 +1,20 @@
#pragma once
-#include "../../base/EntityQueryProcessorBase.h"
-
#include <ArmarXCore/core/logging/Logging.h>
+#include "../../base/EntityQueryProcessorBase.h"
+
namespace armarx::armem::server::query_proc::wm::detail
{
template <class _EntityT, class _ResultEntityT>
- class EntityQueryProcessorBase :
- public base::EntityQueryProcessorBase<_EntityT, _ResultEntityT>
+ class EntityQueryProcessorBase : public base::EntityQueryProcessorBase<_EntityT, _ResultEntityT>
{
protected:
-
using Base = base::EntityQueryProcessorBase<_EntityT, _ResultEntityT>;
public:
-
using EntityT = typename Base::EntityT;
using EntitySnapshotT = typename Base::EntitySnapshotT;
using ResultEntityT = typename Base::ResultEntityT;
@@ -31,14 +28,13 @@ namespace armarx::armem::server::query_proc::wm::detail
protected:
- void addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const override
+ void
+ addResultSnapshot(ResultEntityT& result, const EntitySnapshotT& snapshot) const override
{
snapshot.forEachInstance([](const typename EntitySnapshotT::EntityInstanceT& instance)
- {
- instance.metadata().access();
- });
+ { instance.metadata().access(); });
EntitySnapshotT copy = snapshot;
result.addSnapshot(std::move(copy));
}
};
-}
+} // namespace armarx::armem::server::query_proc::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.cpp b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.cpp
index f72b60033120b639668c3b7b38790df91a671ec6..4ec25f15803cb6b6015f9a6547db5011a0de08c9 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.cpp
@@ -1,6 +1,5 @@
#include "MemoryQueryProcessorBase.h"
-
namespace armarx::armem::server::query_proc::wm::detail
{
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.h
index b6ae970639732bd871402edbb8ee96cd0d68eb04..bbd4bc1593df279e9ab91404e59984daa1ed2956 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/MemoryQueryProcessorBase.h
@@ -2,7 +2,6 @@
#include "../../base/MemoryQueryProcessorBase.h"
-
namespace armarx::armem::server::query_proc::wm::detail
{
@@ -11,12 +10,10 @@ namespace armarx::armem::server::query_proc::wm::detail
public base::MemoryQueryProcessorBase<_MemoryT, _ResultMemoryT, _ChildProcessorT>
{
protected:
-
using Base = base::MemoryQueryProcessorBase<_MemoryT, _ResultMemoryT, _ChildProcessorT>;
public:
-
using MemoryT = typename Base::MemoryT;
using CoreSegmentT = typename Base::CoreSegmentT;
using ResultMemoryT = typename Base::ResultMemoryT;
@@ -29,7 +26,5 @@ namespace armarx::armem::server::query_proc::wm::detail
virtual ~MemoryQueryProcessorBase() = default;
using Base::process;
-
-
};
-}
+} // namespace armarx::armem::server::query_proc::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.h
index 75abb12eefab229f61a9ff5f8dd5c75aca218801..fc70a70195e11d01cc078099e66c77eb32b981a7 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/detail/ProviderSegmentQueryProcessorBase.h
@@ -2,21 +2,22 @@
#include "../../base/ProviderSegmentQueryProcessorBase.h"
-
namespace armarx::armem::server::query_proc::wm::detail
{
template <class _ProviderSegmentT, class _ResultProviderSegmentT, class _ChildProcessorT>
class ProviderSegmentQueryProcessorBase :
- public base::ProviderSegmentQueryProcessorBase<_ProviderSegmentT, _ResultProviderSegmentT, _ChildProcessorT>
+ public base::ProviderSegmentQueryProcessorBase<_ProviderSegmentT,
+ _ResultProviderSegmentT,
+ _ChildProcessorT>
{
protected:
-
- using Base = base::ProviderSegmentQueryProcessorBase<_ProviderSegmentT, _ResultProviderSegmentT, _ChildProcessorT>;
+ using Base = base::ProviderSegmentQueryProcessorBase<_ProviderSegmentT,
+ _ResultProviderSegmentT,
+ _ChildProcessorT>;
public:
-
using ProviderSegmentT = typename Base::ProviderSegmentT;
using EntityT = typename Base::EntityT;
using ResultProviderSegmentT = typename Base::ResultProviderSegmentT;
@@ -29,7 +30,5 @@ namespace armarx::armem::server::query_proc::wm::detail
virtual ~ProviderSegmentQueryProcessorBase() = default;
using Base::process;
-
-
};
-}
+} // namespace armarx::armem::server::query_proc::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp
index 138ee95251fcac618829c38dd580ade96e9cc94e..f1497f5675519a3c24ca8ef3e7076246f23ec618 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.cpp
@@ -2,7 +2,6 @@
#include <ArmarXCore/core/time/ice_conversions.h>
-
namespace armarx::armem::server::query_proc::wm::detail
{
@@ -10,32 +9,30 @@ namespace armarx::armem::server::query_proc::wm::detail
{
}
-}
-
+} // namespace armarx::armem::server::query_proc::wm::detail
namespace armarx::armem::server::query_proc::wm
{
ProviderSegmentQueryProcessor::ProviderSegmentQueryProcessor(armem::query::DataMode dataMode) :
- detail::ProviderSegmentQueryProcessorBase<armem::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>(dataMode),
+ detail::ProviderSegmentQueryProcessorBase<armem::wm::ProviderSegment,
+ armem::wm::ProviderSegment,
+ EntityQueryProcessor>(dataMode),
HasDataMode(dataMode)
{
}
-
CoreSegmentQueryProcessor::CoreSegmentQueryProcessor(armem::query::DataMode dataMode) :
CoreSegmentQueryProcessorBase(dataMode), HasDataMode(dataMode)
{
}
-
MemoryQueryProcessor::MemoryQueryProcessor(armem::query::DataMode dataMode) :
MemoryQueryProcessorBase(dataMode), HasDataMode(dataMode)
{
}
-}
-
+} // namespace armarx::armem::server::query_proc::wm
namespace armarx::armem::server::query_proc::wm_server
{
@@ -44,31 +41,24 @@ namespace armarx::armem::server::query_proc::wm_server
{
}
-
CoreSegmentQueryProcessor::CoreSegmentQueryProcessor(armem::query::DataMode dataMode) :
- CoreSegmentQueryProcessorBase(dataMode),
- HasDataMode(dataMode)
+ CoreSegmentQueryProcessorBase(dataMode), HasDataMode(dataMode)
{
}
-
- void CoreSegmentQueryProcessor::process(
- armem::wm::CoreSegment& result,
- const armem::query::data::CoreSegmentQuery& query,
- const CoreSegment& coreSegment) const
+ void
+ CoreSegmentQueryProcessor::process(armem::wm::CoreSegment& result,
+ const armem::query::data::CoreSegmentQuery& query,
+ const CoreSegment& coreSegment) const
{
- coreSegment.doLocked([&]()
- {
- CoreSegmentQueryProcessorBase::process(result, query, coreSegment);
- });
+ coreSegment.doLocked(
+ [&]() { CoreSegmentQueryProcessorBase::process(result, query, coreSegment); });
}
-
MemoryQueryProcessor::MemoryQueryProcessor(armem::query::DataMode dataMode) :
- MemoryQueryProcessorBase(dataMode),
- HasDataMode(dataMode)
+ MemoryQueryProcessorBase(dataMode), HasDataMode(dataMode)
{
}
-}
+} // namespace armarx::armem::server::query_proc::wm_server
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h
index 4ecb033d49c1caff143808c36ac915f65497d78b..99d2fcbee0fc81268db7bbc1492e26a7f104ccd8 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/wm/wm.h
@@ -4,10 +4,10 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-#include "detail/MemoryQueryProcessorBase.h"
#include "detail/CoreSegmentQueryProcessorBase.h"
-#include "detail/ProviderSegmentQueryProcessorBase.h"
#include "detail/EntityQueryProcessorBase.h"
+#include "detail/MemoryQueryProcessorBase.h"
+#include "detail/ProviderSegmentQueryProcessorBase.h"
namespace armarx::armem::server::query_proc::wm::detail
{
@@ -15,40 +15,35 @@ namespace armarx::armem::server::query_proc::wm::detail
class HasDataMode
{
public:
-
HasDataMode(armem::query::DataMode dataMode);
protected:
-
armem::query::DataMode dataMode;
-
};
-
-
template <class SourceEntityT>
class EntityQueryProcessor :
public EntityQueryProcessorBase<SourceEntityT, armem::wm::Entity>,
public HasDataMode
{
protected:
-
using Base = EntityQueryProcessorBase<SourceEntityT, armem::wm::Entity>;
using Entity = armem::wm::Entity;
public:
-
EntityQueryProcessor(armem::query::DataMode dataMode = armem::query::DataMode::WithData) :
HasDataMode(dataMode)
- {}
+ {
+ }
using Base::process;
protected:
-
- void addResultSnapshot(armem::wm::Entity& result, const typename SourceEntityT::EntitySnapshotT& snapshot) const
+ void
+ addResultSnapshot(armem::wm::Entity& result,
+ const typename SourceEntityT::EntitySnapshotT& snapshot) const
{
bool withData = (dataMode == armem::query::DataMode::WithData);
if (withData)
@@ -59,23 +54,16 @@ namespace armarx::armem::server::query_proc::wm::detail
{
// 1. access real data
snapshot.forEachInstance([](const server::wm::EntityInstance& i)
- {
- i.metadata().access();
- });
+ { i.metadata().access(); });
// 2. create copy and remove data from copy
server::wm::EntitySnapshot copy = snapshot;
- copy.forEachInstance([](server::wm::EntityInstance & i)
- {
- i.data() = nullptr;
- });
+ copy.forEachInstance([](server::wm::EntityInstance& i) { i.data() = nullptr; });
result.addSnapshot(std::move(copy));
}
}
-
};
-}
-
+} // namespace armarx::armem::server::query_proc::wm::detail
namespace armarx::armem::server::query_proc::wm
{
@@ -83,130 +71,135 @@ namespace armarx::armem::server::query_proc::wm
using EntityQueryProcessor = detail::EntityQueryProcessor<armem::wm::Entity>;
class ProviderSegmentQueryProcessor :
- public detail::ProviderSegmentQueryProcessorBase<armem::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>,
+ public detail::ProviderSegmentQueryProcessorBase<armem::wm::ProviderSegment,
+ armem::wm::ProviderSegment,
+ EntityQueryProcessor>,
public detail::HasDataMode
{
protected:
-
- using Base = detail::ProviderSegmentQueryProcessorBase<armem::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>;
+ using Base = detail::ProviderSegmentQueryProcessorBase<armem::wm::ProviderSegment,
+ armem::wm::ProviderSegment,
+ EntityQueryProcessor>;
using ProviderSegment = armem::wm::ProviderSegment;
using Entity = armem::wm::Entity;
public:
-
- ProviderSegmentQueryProcessor(armem::query::DataMode dataMode = armem::query::DataMode::WithData);
+ ProviderSegmentQueryProcessor(
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData);
using Base::process;
};
-
class CoreSegmentQueryProcessor :
- public detail::CoreSegmentQueryProcessorBase <armem::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>,
+ public detail::CoreSegmentQueryProcessorBase<armem::wm::CoreSegment,
+ armem::wm::CoreSegment,
+ ProviderSegmentQueryProcessor>,
public detail::HasDataMode
{
protected:
-
- using Base = wm::detail::CoreSegmentQueryProcessorBase<armem::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>;
+ using Base = wm::detail::CoreSegmentQueryProcessorBase<armem::wm::CoreSegment,
+ armem::wm::CoreSegment,
+ ProviderSegmentQueryProcessor>;
using CoreSegment = armem::wm::CoreSegment;
using ProviderSegment = armem::wm::ProviderSegment;
public:
-
- CoreSegmentQueryProcessor(armem::query::DataMode dataMode = armem::query::DataMode::WithData);
+ CoreSegmentQueryProcessor(
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData);
using Base::process;
-
};
-
class MemoryQueryProcessor :
- public detail::MemoryQueryProcessorBase<armem::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>,
+ public detail::MemoryQueryProcessorBase<armem::wm::Memory,
+ armem::wm::Memory,
+ CoreSegmentQueryProcessor>,
public detail::HasDataMode
{
protected:
-
- using Base = detail::MemoryQueryProcessorBase<armem::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>;
+ using Base = detail::MemoryQueryProcessorBase<armem::wm::Memory,
+ armem::wm::Memory,
+ CoreSegmentQueryProcessor>;
using Memory = armem::wm::Memory;
using CoreSegment = armem::wm::CoreSegment;
public:
-
MemoryQueryProcessor(armem::query::DataMode dataMode = armem::query::DataMode::WithData);
using Base::process;
-
};
-}
-
+} // namespace armarx::armem::server::query_proc::wm
namespace armarx::armem::server::query_proc::wm_server
{
using EntityQueryProcessor = wm::detail::EntityQueryProcessor<server::wm::Entity>;
-
class ProviderSegmentQueryProcessor :
- public wm::detail::ProviderSegmentQueryProcessorBase<server::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>,
+ public wm::detail::ProviderSegmentQueryProcessorBase<server::wm::ProviderSegment,
+ armem::wm::ProviderSegment,
+ EntityQueryProcessor>,
public wm::detail::HasDataMode
{
protected:
-
- using Base = wm::detail::ProviderSegmentQueryProcessorBase<server::wm::ProviderSegment, armem::wm::ProviderSegment, EntityQueryProcessor>;
+ using Base = wm::detail::ProviderSegmentQueryProcessorBase<server::wm::ProviderSegment,
+ armem::wm::ProviderSegment,
+ EntityQueryProcessor>;
using ProviderSegment = server::wm::ProviderSegment;
using Entity = server::wm::Entity;
public:
-
- ProviderSegmentQueryProcessor(armem::query::DataMode dataMode = armem::query::DataMode::WithData);
-
+ ProviderSegmentQueryProcessor(
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData);
};
-
class CoreSegmentQueryProcessor :
- public wm::detail::CoreSegmentQueryProcessorBase <server::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>,
+ public wm::detail::CoreSegmentQueryProcessorBase<server::wm::CoreSegment,
+ armem::wm::CoreSegment,
+ ProviderSegmentQueryProcessor>,
public wm::detail::HasDataMode
{
protected:
-
- using Base = wm::detail::CoreSegmentQueryProcessorBase <server::wm::CoreSegment, armem::wm::CoreSegment, ProviderSegmentQueryProcessor>;
+ using Base = wm::detail::CoreSegmentQueryProcessorBase<server::wm::CoreSegment,
+ armem::wm::CoreSegment,
+ ProviderSegmentQueryProcessor>;
using CoreSegment = server::wm::CoreSegment;
using ProviderSegment = server::wm::ProviderSegment;
public:
-
- CoreSegmentQueryProcessor(armem::query::DataMode dataMode = armem::query::DataMode::WithData);
+ CoreSegmentQueryProcessor(
+ armem::query::DataMode dataMode = armem::query::DataMode::WithData);
using Base::process;
/// Locks the core segment, then delegates back to `CoreSegmentQueryProcessorBase`.
- void process(
- armem::wm::CoreSegment& result,
- const armem::query::data::CoreSegmentQuery& query,
- const CoreSegment& coreSegment) const override;
-
+ void process(armem::wm::CoreSegment& result,
+ const armem::query::data::CoreSegmentQuery& query,
+ const CoreSegment& coreSegment) const override;
};
-
class MemoryQueryProcessor :
- public wm::detail::MemoryQueryProcessorBase<server::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>,
+ public wm::detail::MemoryQueryProcessorBase<server::wm::Memory,
+ armem::wm::Memory,
+ CoreSegmentQueryProcessor>,
public wm::detail::HasDataMode
{
protected:
-
- using Base = wm::detail::MemoryQueryProcessorBase<server::wm::Memory, armem::wm::Memory, CoreSegmentQueryProcessor>;
+ using Base = wm::detail::MemoryQueryProcessorBase<server::wm::Memory,
+ armem::wm::Memory,
+ CoreSegmentQueryProcessor>;
using Memory = server::wm::Memory;
using CoreSegment = server::wm::CoreSegment;
public:
-
MemoryQueryProcessor(armem::query::DataMode dataMode = armem::query::DataMode::WithData);
using Base::process;
};
-}
+} // namespace armarx::armem::server::query_proc::wm_server
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.cpp b/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.cpp
index 818c2d38520fa55b31390cff00123720f0fdbefd..be6b354841c73a881436c9687752c95309ddfdab 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.cpp
@@ -3,19 +3,18 @@
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
namespace armarx::armem::server::segment
{
SpecializedCoreSegment::SpecializedCoreSegment(
- armem::server::MemoryToIceAdapter& iceMemory,
- const std::string& defaultCoreSegmentName,
- aron::type::ObjectPtr coreSegmentAronType,
- int defaultMaxHistorySize,
- const std::vector<PredictionEngine>& predictionEngines) :
+ armem::server::MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName,
+ aron::type::ObjectPtr coreSegmentAronType,
+ int defaultMaxHistorySize,
+ const std::vector<PredictionEngine>& predictionEngines) :
Base(iceMemory),
aronType(coreSegmentAronType),
predictionEngines(predictionEngines),
@@ -24,15 +23,13 @@ namespace armarx::armem::server::segment
Logging::setTag("armarx::armem::SpecializedCoreSegment");
}
-
SpecializedCoreSegment::~SpecializedCoreSegment()
{
}
-
- void SpecializedCoreSegment::defineProperties(
- armarx::PropertyDefinitionsPtr defs,
- const std::string& prefix)
+ void
+ SpecializedCoreSegment::defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
ARMARX_CHECK_NOT_NULL(defs);
@@ -42,11 +39,12 @@ namespace armarx::armem::server::segment
defs->optional(properties.maxHistorySize,
prefix + "seg.CoreMaxHistorySize",
- "Maximal size of the " + properties.segmentName + " entity histories (-1 for infinite).");
+ "Maximal size of the " + properties.segmentName +
+ " entity histories (-1 for infinite).");
}
-
- void SpecializedCoreSegment::init()
+ void
+ SpecializedCoreSegment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
Logging::setTag(properties.segmentName + " Core Segment");
@@ -66,19 +64,20 @@ namespace armarx::armem::server::segment
}
}
- void SpecializedCoreSegment::setDefaultCoreSegmentName(const std::string& coreSegmentName)
+ void
+ SpecializedCoreSegment::setDefaultCoreSegmentName(const std::string& coreSegmentName)
{
this->properties.segmentName = coreSegmentName;
}
-
- void SpecializedCoreSegment::setDefaultMaxHistorySize(int64_t maxHistorySize)
+ void
+ SpecializedCoreSegment::setDefaultMaxHistorySize(int64_t maxHistorySize)
{
this->properties.maxHistorySize = maxHistorySize;
}
-
- void SpecializedCoreSegment::setAronType(aron::type::ObjectPtr aronType)
+ void
+ SpecializedCoreSegment::setAronType(aron::type::ObjectPtr aronType)
{
this->aronType = aronType;
}
@@ -90,16 +89,17 @@ namespace armarx::armem::server::segment
this->predictionEngines = predictionEngines;
}
- wm::CoreSegment& SpecializedCoreSegment::getCoreSegment()
+ wm::CoreSegment&
+ SpecializedCoreSegment::getCoreSegment()
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
return *segmentPtr;
}
-
- const wm::CoreSegment& SpecializedCoreSegment::getCoreSegment() const
+ const wm::CoreSegment&
+ SpecializedCoreSegment::getCoreSegment() const
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
return *segmentPtr;
}
-}
+} // namespace armarx::armem::server::segment
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h
index e34a65795c142a4629591e3f285bc58f9a869353..767047f51d1be77148f1767537e6b17930fa77bc 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h
@@ -1,67 +1,64 @@
#pragma once
-#include <RobotAPI/libraries/armem/server/forward_declarations.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
-#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
+#include <string>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <string>
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
#include "detail/SpecializedSegment.h"
namespace armarx::armem::server::segment
{
- /**
+ /**
* @brief A base class for core segments
*/
- class SpecializedCoreSegment : public detail::SegmentBase<server::wm::CoreSegment>
+ class SpecializedCoreSegment : public detail::SegmentBase<server::wm::CoreSegment>
+ {
+ using Base = detail::SegmentBase<server::wm::CoreSegment>;
+
+ public:
+ SpecializedCoreSegment(MemoryToIceAdapter& iceMemory,
+ const std::string& defaultCoreSegmentName = "",
+ aron::type::ObjectPtr coreSegmentAronType = nullptr,
+ int defaultMaxHistorySize = 10,
+ const std::vector<PredictionEngine>& predictionEngines = {});
+
+ virtual ~SpecializedCoreSegment() override;
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
+ virtual void init() override;
+
+ template <class FunctionT>
+ auto
+ doLocked(FunctionT&& function) const
{
- using Base = detail::SegmentBase<server::wm::CoreSegment>;
-
- public:
-
- SpecializedCoreSegment(
- MemoryToIceAdapter& iceMemory,
- const std::string& defaultCoreSegmentName = "",
- aron::type::ObjectPtr coreSegmentAronType = nullptr,
- int defaultMaxHistorySize = 10,
- const std::vector<PredictionEngine>& predictionEngines = {});
-
- virtual ~SpecializedCoreSegment() override;
-
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
- virtual void init() override;
+ return segmentPtr->doLocked(function);
+ }
- template <class FunctionT>
- auto doLocked(FunctionT&& function) const
- {
- return segmentPtr->doLocked(function);
- }
+ void setDefaultCoreSegmentName(const std::string& coreSegmentName);
+ void setDefaultMaxHistorySize(int64_t maxHistorySize);
+ void setAronType(aron::type::ObjectPtr aronType);
+ void setPredictionEngines(const std::vector<PredictionEngine>& predictionEngines);
- void setDefaultCoreSegmentName(const std::string& coreSegmentName);
- void setDefaultMaxHistorySize(int64_t maxHistorySize);
- void setAronType(aron::type::ObjectPtr aronType);
- void
- setPredictionEngines(const std::vector<PredictionEngine>& predictionEngines);
+ wm::CoreSegment& getCoreSegment();
+ const wm::CoreSegment& getCoreSegment() const;
- wm::CoreSegment& getCoreSegment();
- const wm::CoreSegment& getCoreSegment() const;
+ public:
+ aron::type::ObjectPtr aronType;
+ std::vector<PredictionEngine> predictionEngines;
- public:
-
- aron::type::ObjectPtr aronType;
- std::vector<PredictionEngine> predictionEngines;
-
- struct Properties
- {
- std::string segmentName;
- int64_t maxHistorySize;
- };
- Properties properties;
-
+ struct Properties
+ {
+ std::string segmentName;
+ int64_t maxHistorySize;
};
-}
+
+ Properties properties;
+ };
+} // namespace armarx::armem::server::segment
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.cpp b/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.cpp
index f5ab3b86543d601310a02a0de59820a2d459a32e..eade7b88b5d79a061d0cd5671e9c3a4dff2473da 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.cpp
@@ -3,9 +3,8 @@
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
namespace armarx::armem::server::segment
{
@@ -31,13 +30,13 @@ namespace armarx::armem::server::segment
Logging::setTag("armarx::armem::SpecializedProviderSegment");
}
-
SpecializedProviderSegment::~SpecializedProviderSegment()
{
}
-
- void SpecializedProviderSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ SpecializedProviderSegment::defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
ARMARX_CHECK_NOT_NULL(defs);
@@ -49,11 +48,12 @@ namespace armarx::armem::server::segment
defs->optional(properties.maxHistorySize,
prefix + "seg.ProviderMaxHistorySize",
- "Maximal size of the " + properties.segmentName + " entity histories (-1 for infinite).");
+ "Maximal size of the " + properties.segmentName +
+ " entity histories (-1 for infinite).");
}
-
- void SpecializedProviderSegment::init()
+ void
+ SpecializedProviderSegment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
@@ -76,19 +76,21 @@ namespace armarx::armem::server::segment
}
}
- void SpecializedProviderSegment::setDefaultProviderSegmentName(const std::string& providerSegmentName)
+ void
+ SpecializedProviderSegment::setDefaultProviderSegmentName(
+ const std::string& providerSegmentName)
{
this->properties.segmentName = providerSegmentName;
}
-
- void SpecializedProviderSegment::setDefaultMaxHistorySize(int64_t maxHistorySize)
+ void
+ SpecializedProviderSegment::setDefaultMaxHistorySize(int64_t maxHistorySize)
{
this->properties.maxHistorySize = maxHistorySize;
}
-
- void SpecializedProviderSegment::setAronType(aron::type::ObjectPtr aronType)
+ void
+ SpecializedProviderSegment::setAronType(aron::type::ObjectPtr aronType)
{
this->aronType = aronType;
}
@@ -100,17 +102,18 @@ namespace armarx::armem::server::segment
this->predictionEngines = predictionEngines;
}
- wm::ProviderSegment& SpecializedProviderSegment::getProviderSegment()
+ wm::ProviderSegment&
+ SpecializedProviderSegment::getProviderSegment()
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
return *segmentPtr;
}
-
- const wm::ProviderSegment& SpecializedProviderSegment::getProviderSegment() const
+ const wm::ProviderSegment&
+ SpecializedProviderSegment::getProviderSegment() const
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
return *segmentPtr;
}
-}
+} // namespace armarx::armem::server::segment
diff --git a/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h b/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h
index 6cbac5cad70e5590e2ac190768d9aa38c86542a2..ba88056f07e34f1d054ee4689cd4dee66138d7af 100644
--- a/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h
@@ -1,64 +1,62 @@
#pragma once
-#include <RobotAPI/libraries/armem/server/forward_declarations.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
-#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
+#include <string>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <string>
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
#include "SpecializedCoreSegment.h"
namespace armarx::armem::server::segment
{
- /**
+ /**
* @brief A base class for provider segments
*/
- class SpecializedProviderSegment : public detail::SegmentBase<server::wm::ProviderSegment>
+ class SpecializedProviderSegment : public detail::SegmentBase<server::wm::ProviderSegment>
+ {
+ using Base = detail::SegmentBase<server::wm::ProviderSegment>;
+
+ public:
+ SpecializedProviderSegment(
+ MemoryToIceAdapter& iceMemory,
+ const std::string& defaultProviderSegmentName = "",
+ const std::string& defaultCoreSegmentName = "",
+ aron::type::ObjectPtr providerSegmentAronType = nullptr,
+ aron::type::ObjectPtr coreSegmentAronType = nullptr,
+ int defaultMaxHistorySize = 10,
+ const std::vector<PredictionEngine>& providerSegmentPredictionEngines = {},
+ const std::vector<PredictionEngine>& coreSegmentPredictionEngines = {});
+
+ virtual ~SpecializedProviderSegment() override;
+
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
+ virtual void init() override;
+
+ void setDefaultProviderSegmentName(const std::string& providerSegmentName);
+ void setDefaultMaxHistorySize(int64_t maxHistorySize);
+ void setAronType(aron::type::ObjectPtr aronType);
+ void setPredictionEngines(const std::vector<PredictionEngine>& predictionEngines);
+
+ wm::ProviderSegment& getProviderSegment();
+ const wm::ProviderSegment& getProviderSegment() const;
+
+
+ public:
+ aron::type::ObjectPtr aronType;
+ std::vector<PredictionEngine> predictionEngines;
+ SpecializedCoreSegment coreSegment;
+
+ struct Properties
{
- using Base = detail::SegmentBase<server::wm::ProviderSegment>;
-
- public:
-
- SpecializedProviderSegment(
- MemoryToIceAdapter& iceMemory,
- const std::string& defaultProviderSegmentName = "",
- const std::string& defaultCoreSegmentName = "",
- aron::type::ObjectPtr providerSegmentAronType = nullptr,
- aron::type::ObjectPtr coreSegmentAronType = nullptr,
- int defaultMaxHistorySize = 10,
- const std::vector<PredictionEngine>& providerSegmentPredictionEngines = {},
- const std::vector<PredictionEngine>& coreSegmentPredictionEngines = {});
-
- virtual ~SpecializedProviderSegment() override;
-
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
- virtual void init() override;
-
- void setDefaultProviderSegmentName(const std::string& providerSegmentName);
- void setDefaultMaxHistorySize(int64_t maxHistorySize);
- void setAronType(aron::type::ObjectPtr aronType);
- void
- setPredictionEngines(const std::vector<PredictionEngine>& predictionEngines);
-
- wm::ProviderSegment& getProviderSegment();
- const wm::ProviderSegment& getProviderSegment() const;
-
-
- public:
-
- aron::type::ObjectPtr aronType;
- std::vector<PredictionEngine> predictionEngines;
- SpecializedCoreSegment coreSegment;
-
- struct Properties
- {
- std::string segmentName;
- int64_t maxHistorySize;
- };
- Properties properties;
+ std::string segmentName;
+ int64_t maxHistorySize;
};
-}
+
+ Properties properties;
+ };
+} // namespace armarx::armem::server::segment
diff --git a/source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.h b/source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.h
index b82402f556eaec531720809ff61825ab6da09eed..12a6fbbf3a96a8dd5f7d3ded6155863a78e005df 100644
--- a/source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.h
+++ b/source/RobotAPI/libraries/armem/server/segment/detail/SpecializedSegment.h
@@ -1,15 +1,13 @@
#pragma once
-#include <RobotAPI/libraries/armem/server/forward_declarations.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
-#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
+#include <string>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <string>
-
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
namespace armarx::armem::server::segment
{
@@ -24,21 +22,22 @@ namespace armarx::armem::server::segment
class SegmentBase : public armarx::Logging
{
public:
-
SegmentBase() = delete;
- SegmentBase(MemoryToIceAdapter& iceMemory) :
- iceMemory(iceMemory)
+
+ SegmentBase(MemoryToIceAdapter& iceMemory) : iceMemory(iceMemory)
{
Logging::setTag("armarx::armem::Segment");
}
- MemoryID& id()
+ MemoryID&
+ id()
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
return segmentPtr->id();
}
- const MemoryID& id() const
+ const MemoryID&
+ id() const
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
return segmentPtr->id();
@@ -46,18 +45,17 @@ namespace armarx::armem::server::segment
virtual ~SegmentBase() = default;
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") = 0;
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") = 0;
virtual void init() = 0;
public:
-
SegmentType* segmentPtr;
protected:
-
// Memory connection
MemoryToIceAdapter& iceMemory;
};
- }
-}
+ } // namespace detail
+} // namespace armarx::armem::server::segment
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
index d2bbeb2f8d1ec31e4b69beacd1e30a3994d00752..824816db18732eb729fd06e4c586e92e8ea5e8ff 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemLTMBenchmark.cpp
@@ -24,12 +24,13 @@
#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+
#include <filesystem>
#include <iostream>
#include <ArmarXCore/core/time/StopWatch.h>
-#include <RobotAPI/Test.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/ltm/Memory.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp
index db6c68b2d4c6142ef3e7716b1d80245b02b93734..ee2b2a7a1cc833b2c7db084a8191120e1328877a 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemLTMTest.cpp
@@ -25,6 +25,7 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
+
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/ltm/Memory.h>
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp
index eb562e3278db9cb51aaa364e726e6fe5f71655fc..d2548f8a382d6acdc94f1fa9fdb6644eefe28d25 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemMemoryTest.cpp
@@ -24,11 +24,12 @@
#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+
#include <iostream>
#include <SimoxUtility/meta/type_name.h>
-#include <RobotAPI/Test.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
diff --git a/source/RobotAPI/libraries/armem/server/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/server/test/ArMemQueryTest.cpp
index 583dbb5fd23bfd33f3a61b722a00862ea06172b3..bdca8325adfe0f7647f5c69ddc979779b61d13df 100644
--- a/source/RobotAPI/libraries/armem/server/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ArMemQueryTest.cpp
@@ -25,17 +25,18 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/interface/armem/query.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h>
-#include <RobotAPI/libraries/armem/server/query_proc/wm/wm.h>
-#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <iostream>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string.h>
-#include <iostream>
+#include <ArmarXCore/core/exceptions/LocalException.h>
+
+#include <RobotAPI/interface/armem/query.h>
+#include <RobotAPI/libraries/armem/core/base/detail/negative_index_semantics.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/query_proc/wm/wm.h>
namespace armem = armarx::armem;
@@ -43,7 +44,6 @@ namespace aron = armarx::aron;
namespace query = armarx::armem::query::data;
using EntityQueryProcessor = armarx::armem::server::query_proc::wm::EntityQueryProcessor;
-
namespace ArMemQueryTest
{
@@ -59,7 +59,6 @@ namespace ArMemQueryTest
std::vector<armem::wm::Entity> results;
-
Fixture()
{
entity = armem::wm::Entity("entity");
@@ -76,31 +75,32 @@ namespace ArMemQueryTest
snapshot.time() = armem::Time(armem::Duration::MicroSeconds(5000));
entity.addSnapshot(snapshot);
}
+
~Fixture()
{
}
-
template <class QueryT>
- void addResults(QueryT query = {})
+ void
+ addResults(QueryT query = {})
{
// Test whether we get the same result when we pass the concrete type
// (static resolution) and when we pass the base type (dynamic resolution).
results.emplace_back(processor.process(query, entity));
- results.emplace_back(processor.process(static_cast<query::EntityQuery const&>(query), entity));
+ results.emplace_back(
+ processor.process(static_cast<query::EntityQuery const&>(query), entity));
}
};
-}
+} // namespace ArMemQueryTest
BOOST_FIXTURE_TEST_SUITE(ArMemQueryTest, ArMemQueryTest::Fixture)
-
-
BOOST_AUTO_TEST_CASE(test_entity_UnknownQueryType)
{
- BOOST_CHECK_THROW(processor.process(UnknownEntityQuery(), entity), armem::error::UnknownQueryType);
+ BOOST_CHECK_THROW(processor.process(UnknownEntityQuery(), entity),
+ armem::error::UnknownQueryType);
std::string msg;
try
{
@@ -114,8 +114,6 @@ BOOST_AUTO_TEST_CASE(test_entity_UnknownQueryType)
BOOST_CHECK(simox::alg::contains(msg, "UnknownEntityQuery"));
}
-
-
BOOST_AUTO_TEST_CASE(test_entity_Single_latest)
{
addResults(query::entity::Single());
@@ -125,7 +123,8 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_latest)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_CHECK_EQUAL(result.size(), 1);
- const armem::wm::EntitySnapshot* first = result.findFirstSnapshotAfterOrAt(armem::Time(armem::Duration::MicroSeconds(0)));
+ const armem::wm::EntitySnapshot* first =
+ result.findFirstSnapshotAfterOrAt(armem::Time(armem::Duration::MicroSeconds(0)));
BOOST_REQUIRE_NE(first, nullptr);
BOOST_CHECK_EQUAL(first->time(), armem::Time(armem::Duration::MicroSeconds(5000)));
}
@@ -138,6 +137,7 @@ t_usec(long usec)
t.timeSinceEpoch.microSeconds = usec;
return t;
}
+
static armem::dto::Duration
d_usec(long usec)
{
@@ -155,11 +155,11 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
BOOST_REQUIRE_EQUAL(result.size(), 1);
- BOOST_CHECK_EQUAL(result.getFirstSnapshot().time(), armem::Time(armem::Duration::MicroSeconds(3000)));
+ BOOST_CHECK_EQUAL(result.getFirstSnapshot().time(),
+ armem::Time(armem::Duration::MicroSeconds(3000)));
}
}
-
BOOST_AUTO_TEST_CASE(test_entity_Single_non_existing)
{
addResults(query::entity::Single(t_usec(3500)));
@@ -172,7 +172,6 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_non_existing)
}
}
-
BOOST_AUTO_TEST_CASE(test_entity_All)
{
addResults(query::entity::All());
@@ -193,8 +192,6 @@ BOOST_AUTO_TEST_CASE(test_entity_All)
}
}
-
-
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
{
addResults(query::entity::TimeRange(t_usec(1500), t_usec(3500)));
@@ -207,15 +204,12 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
BOOST_CHECK_EQUAL(result.size(), 2);
std::vector<armem::Time> times = result.getTimestamps();
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(2000)), armem::Time(armem::Duration::MicroSeconds(3000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(2000)),
+ armem::Time(armem::Duration::MicroSeconds(3000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
}
-
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
{
addResults(query::entity::TimeRange(t_usec(2000), t_usec(4000)));
@@ -228,12 +222,9 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
BOOST_CHECK_EQUAL(result.size(), 3);
std::vector<armem::Time> times = result.getTimestamps();
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(2000)),
- armem::Time(armem::Duration::MicroSeconds(3000)),
- armem::Time(armem::Duration::MicroSeconds(4000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(2000)),
+ armem::Time(armem::Duration::MicroSeconds(3000)),
+ armem::Time(armem::Duration::MicroSeconds(4000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
}
@@ -255,7 +246,6 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
}
}
-
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
{
addResults(query::entity::TimeRange(t_usec(2400), t_usec(2600)));
@@ -270,7 +260,6 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
}
}
-
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
{
addResults(query::entity::TimeRange(t_usec(-1), t_usec(2500)));
@@ -283,16 +272,12 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
BOOST_CHECK_EQUAL(result.size(), 2);
const std::vector<armem::Time> times = result.getTimestamps();
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(1000)),
- armem::Time(armem::Duration::MicroSeconds(2000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(1000)),
+ armem::Time(armem::Duration::MicroSeconds(2000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
}
-
BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
{
addResults(query::entity::TimeRange(t_usec(2500), t_usec(-1)));
@@ -304,17 +289,13 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
BOOST_CHECK_EQUAL(result.size(), 3);
const std::vector<armem::Time> times = result.getTimestamps();
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(3000)),
- armem::Time(armem::Duration::MicroSeconds(4000)),
- armem::Time(armem::Duration::MicroSeconds(5000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(3000)),
+ armem::Time(armem::Duration::MicroSeconds(4000)),
+ armem::Time(armem::Duration::MicroSeconds(5000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
}
-
/* BeforeTime */
BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_1)
@@ -343,19 +324,13 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeTime_2)
const std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 2);
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(2000)),
- armem::Time(armem::Duration::MicroSeconds(3000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(2000)),
+ armem::Time(armem::Duration::MicroSeconds(3000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
-
}
-
-
/* BeforeOrAtTime */
BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_before)
@@ -384,7 +359,7 @@ BOOST_AUTO_TEST_CASE(test_entity_BeforeOrAtTime_at)
std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
- BOOST_REQUIRE_EQUAL(times.front(), armem::Time(armem::Duration::MicroSeconds(3000)));
+ BOOST_REQUIRE_EQUAL(times.front(), armem::Time(armem::Duration::MicroSeconds(3000)));
}
}
@@ -419,15 +394,11 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_no_limit)
std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 2);
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(3000)),
- armem::Time(armem::Duration::MicroSeconds(4000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(3000)),
+ armem::Time(armem::Duration::MicroSeconds(4000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
-
}
/**
@@ -446,15 +417,11 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_600)
std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 2);
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(3000)),
- armem::Time(armem::Duration::MicroSeconds(4000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(3000)),
+ armem::Time(armem::Duration::MicroSeconds(4000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
-
}
/**
@@ -473,7 +440,6 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_too_small)
std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 0);
}
-
}
/**
@@ -492,10 +458,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_next)
std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(4000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(4000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
@@ -517,10 +480,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_limit_only_previous)
std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(3000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(3000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
@@ -542,10 +502,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_perfect_match)
std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(3000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(3000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
@@ -603,10 +560,7 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
std::vector<armem::Time> times = result.getTimestamps();
BOOST_REQUIRE_EQUAL(times.size(), 1);
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(5'000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(5'000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
@@ -614,11 +568,10 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_future_valid)
BOOST_AUTO_TEST_CASE(test_entity_TimeApprox_lookup_invalid_timestamp)
{
- BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox(t_usec(-1), d_usec(1))), ::armarx::LocalException);
+ BOOST_REQUIRE_THROW(addResults(query::entity::TimeApprox(t_usec(-1), d_usec(1))),
+ ::armarx::LocalException);
}
-
-
BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
{
BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(0, 0), 0);
@@ -650,7 +603,6 @@ BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
BOOST_CHECK_EQUAL(armem::base::detail::negativeIndexSemantics(-20, 10), 0);
}
-
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
{
addResults(query::entity::IndexRange());
@@ -667,16 +619,14 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
const std::vector<armem::Time> expected = entity.getTimestamps();
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
-
}
-
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::IndexRange(1, 3)); // => [1, 2, 3]
- addResults(query::entity::IndexRange(1, -2)); // 5 - 2 = 3
- addResults(query::entity::IndexRange(-4, 3)); // 5 - 4 = 1
+ addResults(query::entity::IndexRange(1, 3)); // => [1, 2, 3]
+ addResults(query::entity::IndexRange(1, -2)); // 5 - 2 = 3
+ addResults(query::entity::IndexRange(-4, 3)); // 5 - 4 = 1
addResults(query::entity::IndexRange(-4, -2));
BOOST_REQUIRE_GT(results.size(), 0);
@@ -687,24 +637,20 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
BOOST_CHECK_EQUAL(result.size(), 3);
std::vector<armem::Time> times = result.getTimestamps();
- std::vector<armem::Time> expected
- {
- armem::Time(armem::Duration::MicroSeconds(2000)),
- armem::Time(armem::Duration::MicroSeconds(3000)),
- armem::Time(armem::Duration::MicroSeconds(4000))
- };
+ std::vector<armem::Time> expected{armem::Time(armem::Duration::MicroSeconds(2000)),
+ armem::Time(armem::Duration::MicroSeconds(3000)),
+ armem::Time(armem::Duration::MicroSeconds(4000))};
BOOST_CHECK_EQUAL_COLLECTIONS(times.begin(), times.end(), expected.begin(), expected.end());
}
}
-
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_range)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
- addResults(query::entity::IndexRange(1, 0));
- addResults(query::entity::IndexRange(2, 1));
- addResults(query::entity::IndexRange(5, 3));
- addResults(query::entity::IndexRange(4, -3)); // 5-3 = 2
+ addResults(query::entity::IndexRange(1, 0));
+ addResults(query::entity::IndexRange(2, 1));
+ addResults(query::entity::IndexRange(5, 3));
+ addResults(query::entity::IndexRange(4, -3)); // 5-3 = 2
addResults(query::entity::IndexRange(3, -3));
addResults(query::entity::IndexRange(1, -5));
@@ -718,15 +664,14 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_range)
}
}
-
BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_entity)
{
entity.clear();
BOOST_REQUIRE_EQUAL(entity.size(), 0);
- addResults(query::entity::IndexRange(0, 0));
+ addResults(query::entity::IndexRange(0, 0));
addResults(query::entity::IndexRange(0, 10));
addResults(query::entity::IndexRange(-10, -1));
- addResults(query::entity::IndexRange(2, 5));
+ addResults(query::entity::IndexRange(2, 5));
addResults(query::entity::IndexRange(3, -3));
addResults(query::entity::IndexRange(-1, 10));
diff --git a/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp
index 9e84d09f91b32491f72f709521c1bbc0c283acfc..751bfe38b100f857cf2c0bc9fde2c672e013d5d4 100644
--- a/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp
+++ b/source/RobotAPI/libraries/armem/server/test/ForgettingExperiments.cpp
@@ -1,18 +1,18 @@
#include "ForgettingExperiments.h"
-#include <iostream>
-#include <fstream>
#include <chrono>
#include <filesystem>
+#include <fstream>
+#include <iostream>
namespace armarx::armem::server::test
{
-void get_information_single_ltm(
- std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>& filter
- )
-{
- /*
+ void
+ get_information_single_ltm(
+ std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>& filter)
+ {
+ /*
nlohmann::json info;
for(const auto& pair: filter.second){
if(pair.second.accepted + pair.second.rejected < 1){
@@ -55,10 +55,12 @@ void get_information_single_ltm(
jsonData[filter.first] = info;
info.clear();
*/
-}
+ }
-void save_statistics(
- std::map<std::string, std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>> stats,
+ void
+ save_statistics(
+ std::map<std::string,
+ std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>> stats,
std::map<std::string, armarx::core::time::DateTime> time_stats,
armarx::core::time::DateTime firstStartedRecording,
armarx::core::time::DateTime firstStoppedRecording,
@@ -67,52 +69,60 @@ void save_statistics(
std::string memoryName,
std::string sim_json_data,
std::string object_memory_json_data)
-{
- ARMARX_INFO << "Saving statistics";
-
- std::filesystem::path d_p;
- try{
- d_p = test::getStatisticsDirectory(exportPath, exportName, memoryName);
- std::filesystem::create_directories(d_p);
- ARMARX_INFO << "Experiments will be saved at: " << d_p.string();
- } catch(...){
- ARMARX_WARNING << "Creating needed directories for saving statistics did not work";
- }
+ {
+ ARMARX_INFO << "Saving statistics";
+
+ std::filesystem::path d_p;
+ try
+ {
+ d_p = test::getStatisticsDirectory(exportPath, exportName, memoryName);
+ std::filesystem::create_directories(d_p);
+ ARMARX_INFO << "Experiments will be saved at: " << d_p.string();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Creating needed directories for saving statistics did not work";
+ }
- //json object for statistics:
- nlohmann::json jsonData;
-
- //get current date:
- auto now = std::chrono::high_resolution_clock::now();
- auto now_ax_datetime = DateTime::Now();
- auto now_time = std::chrono::high_resolution_clock::to_time_t(now);
- auto time = localtime(&now_time);
-
- std::stringstream ss;
- ss << std::put_time(time, "%Y-%m-%d %H:%M:%S");
- std::string timeString = ss.str();
- std::stringstream s;
- s << std::put_time(time, "%Y%m%d%H%M%S");
- std::string timeID = s.str();
- jsonData["Statistics saved at"] = timeString;
-
- bool ended_without_recording = false;
-
- if(firstStoppedRecording.toMilliSecondsSinceEpoch() < firstStartedRecording.toMilliSecondsSinceEpoch()){
- //this can happen if the recording is not propperly stopped but interrupted by stopping
- //the component
- firstStoppedRecording = now_ax_datetime;
- }
+ //json object for statistics:
+ nlohmann::json jsonData;
- for(const auto& filter: stats){
- filter_statistics(filter.second, memoryName,firstStartedRecording, firstStoppedRecording, &jsonData);
- }
+ //get current date:
+ auto now = std::chrono::high_resolution_clock::now();
+ auto now_ax_datetime = DateTime::Now();
+ auto now_time = std::chrono::high_resolution_clock::to_time_t(now);
+ auto time = localtime(&now_time);
- if(ended_without_recording){
- return;
- }
+ std::stringstream ss;
+ ss << std::put_time(time, "%Y-%m-%d %H:%M:%S");
+ std::string timeString = ss.str();
+ std::stringstream s;
+ s << std::put_time(time, "%Y%m%d%H%M%S");
+ std::string timeID = s.str();
+ jsonData["Statistics saved at"] = timeString;
+
+ bool ended_without_recording = false;
+
+ if (firstStoppedRecording.toMilliSecondsSinceEpoch() <
+ firstStartedRecording.toMilliSecondsSinceEpoch())
+ {
+ //this can happen if the recording is not propperly stopped but interrupted by stopping
+ //the component
+ firstStoppedRecording = now_ax_datetime;
+ }
+
+ for (const auto& filter : stats)
+ {
+ filter_statistics(
+ filter.second, memoryName, firstStartedRecording, firstStoppedRecording, &jsonData);
+ }
+
+ if (ended_without_recording)
+ {
+ return;
+ }
- /*
+ /*
if(sim_json_data.empty() || object_memory_json_data.empty()){
jsonData["Scene-Information for Simulation"] = load_scene_information("sim");
jsonData["Scene-Information for ObjectMemory"] = load_scene_information("om");
@@ -123,64 +133,74 @@ void save_statistics(
}
*/
- std::string path = d_p.string();
- path += "/tmp_wm_";
- path += timeID;
- path += ".json";
- try{
- std::ofstream outputFile;
- outputFile.open(path);
- if(outputFile.is_open()){
- outputFile << jsonData.dump(4);
- outputFile.close();
- } else {
- if(outputFile.fail()){
- ARMARX_INFO << outputFile.rdstate();
- outputFile.clear();
+ std::string path = d_p.string();
+ path += "/tmp_wm_";
+ path += timeID;
+ path += ".json";
+ try
+ {
+ std::ofstream outputFile;
+ outputFile.open(path);
+ if (outputFile.is_open())
+ {
+ outputFile << jsonData.dump(4);
+ outputFile.close();
+ }
+ else
+ {
+ if (outputFile.fail())
+ {
+ ARMARX_INFO << outputFile.rdstate();
+ outputFile.clear();
+ }
+ ARMARX_INFO << "File is not open";
}
- ARMARX_INFO << "File is not open";
}
- } catch(...){
- ARMARX_WARNING << "std::ofstream failed";
- }
-
- ARMARX_INFO << "Saving statistics completed";
-}
+ catch (...)
+ {
+ ARMARX_WARNING << "std::ofstream failed";
+ }
-nlohmann::json load_scene_information(std::string om_or_sim)
-{
- std::string home_dir = getenv("HOME");
- if(home_dir.empty()){
- ARMARX_WARNING << "Trying to open home directory but env variable HOME is not set";
+ ARMARX_INFO << "Saving statistics completed";
}
- std::filesystem::path path = home_dir;
- path /= "code";
- path /= "h2t" ;
- path /= "PriorKnowledgeData";
- path /= "data";
- path /= "PriorKnowledgeData";
- path /= "scenes";
- std::string name_for_current_scene = "ARMAR-III-kitchen"; //this can be changed later, maybe to something that indicates better that these are the changed files
- std::string file_name = name_for_current_scene + "_" + om_or_sim + ".json";
- path /= file_name;
- std::ifstream ifs(path);
- nlohmann::json json_data = nlohmann::json::parse(ifs);
-
- return json_data;
+ nlohmann::json
+ load_scene_information(std::string om_or_sim)
+ {
+ std::string home_dir = getenv("HOME");
+ if (home_dir.empty())
+ {
+ ARMARX_WARNING << "Trying to open home directory but env variable HOME is not set";
+ }
+ std::filesystem::path path = home_dir;
+ path /= "code";
+ path /= "h2t";
+ path /= "PriorKnowledgeData";
+ path /= "data";
+ path /= "PriorKnowledgeData";
+ path /= "scenes";
+ std::string name_for_current_scene =
+ "ARMAR-III-kitchen"; //this can be changed later, maybe to something that indicates better that these are the changed files
+ std::string file_name = name_for_current_scene + "_" + om_or_sim + ".json";
+ path /= file_name;
+
+ std::ifstream ifs(path);
+ nlohmann::json json_data = nlohmann::json::parse(ifs);
+
+ return json_data;
+ }
-}
+ nlohmann::json
+ find_difference(nlohmann::json om, nlohmann::json sim)
+ {
+ //the new object is at the last position of sim at the moment
+ auto objects = sim.at("objects");
+ int num_elements = objects.size();
+ auto object = objects[num_elements - 1];
+ return object;
+ }
-nlohmann::json find_difference(nlohmann::json om, nlohmann::json sim)
-{
- //the new object is at the last position of sim at the moment
- auto objects = sim.at("objects");
- int num_elements = objects.size();
- auto object = objects[num_elements - 1];
- return object;
-}
-
-/**
+ /**
* @brief getStatisticsDirectory returns the path to where the statistics for this forgetting process
* should be saved
* @param memoryName name of the memory, e.g. VisionMemory
@@ -188,100 +208,111 @@ nlohmann::json find_difference(nlohmann::json om, nlohmann::json sim)
* @param exportPath path to the export folder with date for this LTM
* @return path
*/
-std::filesystem::path getStatisticsDirectory(
- std::filesystem::path exportPath,
- std::string exportName,
- std::string memoryName)
-{
- //path consists of exportPath/Date/exportName/memoryName with the date being the start date!
- std::filesystem::path statisticsPath = exportPath;
-
- //add parts to exportPath
- statisticsPath /= exportName;
- statisticsPath /= memoryName;
-
- //add Statistics Directory:
- statisticsPath /= "Statistics";
-
- return statisticsPath;
-}
-
-void filter_statistics(
- std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics> statistics_processors,
- std::string memoryName,
- armarx::core::time::DateTime firstStartedRecording,
- armarx::core::time::DateTime firstStoppedRecording,
- nlohmann::json *statistics)
-{
- //TODO: start time of processor should be when enabling recording for filters... not creation
- //TODO: make sure order stays the way it was when added
- nlohmann::json ltm_information;
-
- for(const auto& statistics_processor: statistics_processors){
- nlohmann::json processor_information;
-
- //get start time of the processor:
- auto time = std::chrono::high_resolution_clock::to_time_t(statistics_processor.second.start_time);
- auto t = localtime(&time);
- std::stringstream ss;
- ss << std::put_time(t, "%Y-%m-%d %H:%M:%S");
- std::string timeStringStart = ss.str();
- processor_information["Processor instance created at"] = timeStringStart;
-
- //get end time of the processor:
- auto timeEnd = std::chrono::high_resolution_clock::to_time_t(statistics_processor.second.end_time);
- auto t_end = localtime(&timeEnd);
- std::stringstream s;
- s << std::put_time(t_end, "%Y-%m-%d %H:%M:%S");
- std::string timeStringEnd = s.str();
- processor_information["Processor instance last used at"] = timeStringEnd;
-
- //get time the processor ran for in seconds:
- long seconds = timeEnd - time;
- processor_information["Lifetime of processor in sec"] = seconds;
-
- //actual runtime in first recording:
- processor_information["Processor started at"] = firstStartedRecording.toDateTimeString();
- processor_information["Processor stopped at"] = firstStoppedRecording.toDateTimeString();
- auto runtime = firstStoppedRecording.toMilliSecondsSinceEpoch() - firstStartedRecording.toMilliSecondsSinceEpoch();
- auto seconds_runtime = runtime / 1000.0;
- processor_information["Duration of recording in seconds"] = seconds_runtime;
-
- //add additional information:
- processor_information["Additional information"] = statistics_processor.second.additional_info;
-
- //add number of accepted elements if this is a filter:
- processor_information["Accepted elements"] = statistics_processor.second.accepted;
+ std::filesystem::path
+ getStatisticsDirectory(std::filesystem::path exportPath,
+ std::string exportName,
+ std::string memoryName)
+ {
+ //path consists of exportPath/Date/exportName/memoryName with the date being the start date!
+ std::filesystem::path statisticsPath = exportPath;
+
+ //add parts to exportPath
+ statisticsPath /= exportName;
+ statisticsPath /= memoryName;
+
+ //add Statistics Directory:
+ statisticsPath /= "Statistics";
+
+ return statisticsPath;
+ }
- //add number of rejected elements if this is a filter:
- processor_information["Rejected elements"] = statistics_processor.second.rejected;
+ void
+ filter_statistics(std::map<std::string, ltm::processor::SnapshotFilter::FilterStatistics>
+ statistics_processors,
+ std::string memoryName,
+ armarx::core::time::DateTime firstStartedRecording,
+ armarx::core::time::DateTime firstStoppedRecording,
+ nlohmann::json* statistics)
+ {
+ //TODO: start time of processor should be when enabling recording for filters... not creation
+ //TODO: make sure order stays the way it was when added
+ nlohmann::json ltm_information;
+
+ for (const auto& statistics_processor : statistics_processors)
+ {
+ nlohmann::json processor_information;
+
+ //get start time of the processor:
+ auto time = std::chrono::high_resolution_clock::to_time_t(
+ statistics_processor.second.start_time);
+ auto t = localtime(&time);
+ std::stringstream ss;
+ ss << std::put_time(t, "%Y-%m-%d %H:%M:%S");
+ std::string timeStringStart = ss.str();
+ processor_information["Processor instance created at"] = timeStringStart;
+
+ //get end time of the processor:
+ auto timeEnd =
+ std::chrono::high_resolution_clock::to_time_t(statistics_processor.second.end_time);
+ auto t_end = localtime(&timeEnd);
+ std::stringstream s;
+ s << std::put_time(t_end, "%Y-%m-%d %H:%M:%S");
+ std::string timeStringEnd = s.str();
+ processor_information["Processor instance last used at"] = timeStringEnd;
+
+ //get time the processor ran for in seconds:
+ long seconds = timeEnd - time;
+ processor_information["Lifetime of processor in sec"] = seconds;
+
+ //actual runtime in first recording:
+ processor_information["Processor started at"] =
+ firstStartedRecording.toDateTimeString();
+ processor_information["Processor stopped at"] =
+ firstStoppedRecording.toDateTimeString();
+ auto runtime = firstStoppedRecording.toMilliSecondsSinceEpoch() -
+ firstStartedRecording.toMilliSecondsSinceEpoch();
+ auto seconds_runtime = runtime / 1000.0;
+ processor_information["Duration of recording in seconds"] = seconds_runtime;
+
+ //add additional information:
+ processor_information["Additional information"] =
+ statistics_processor.second.additional_info;
+
+ //add number of accepted elements if this is a filter:
+ processor_information["Accepted elements"] = statistics_processor.second.accepted;
+
+ //add number of rejected elements if this is a filter:
+ processor_information["Rejected elements"] = statistics_processor.second.rejected;
+
+ //add additional time needed:
+ processor_information["Additional time needed in sec"] =
+ statistics_processor.second.additional_time.count();
+
+ //add importance or similarity type if this type of filter:
+ auto importance_type = statistics_processor.second.importance_type;
+ if (!importance_type.empty())
+ {
+ processor_information["Importance type"] = importance_type;
+ }
+ auto similarity_type = statistics_processor.second.similarity_type;
+ if (similarity_type != aron::similarity::NDArraySimilarity::Type::NONE)
+ {
+ processor_information["Similarity type"] = std::to_string(similarity_type);
+ //add number of objects compared each time if similarity filter:
+ processor_information["Number of objects compared each time"] =
+ statistics_processor.second.number_of_compared_objects;
+ }
- //add additional time needed:
- processor_information["Additional time needed in sec"] = statistics_processor.second.additional_time.count();
+ //add memory name as a sanity check:
+ processor_information["Memory name"] = memoryName;
- //add importance or similarity type if this type of filter:
- auto importance_type = statistics_processor.second.importance_type;
- if (!importance_type.empty()){
- processor_information["Importance type"] = importance_type;
- }
- auto similarity_type = statistics_processor.second.similarity_type;
- if(similarity_type != aron::similarity::NDArraySimilarity::Type::NONE){
- processor_information["Similarity type"] = std::to_string(similarity_type);
- //add number of objects compared each time if similarity filter:
- processor_information["Number of objects compared each time"] =
- statistics_processor.second.number_of_compared_objects;
+ // add information about this specific processor to the information about the ltm:
+ ltm_information[statistics_processor.first] = processor_information;
}
- //add memory name as a sanity check:
- processor_information["Memory name"] = memoryName;
- // add information about this specific processor to the information about the ltm:
- ltm_information[statistics_processor.first] = processor_information;
+ // add the ltm information to the statistics json object:
+ (*statistics)["Episodic Memory Information"] = ltm_information;
}
-
- // add the ltm information to the statistics json object:
- (*statistics)["Episodic Memory Information"] = ltm_information;
-}
-
-}
+} // namespace armarx::armem::server::test
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
index bd6096c2fa6470d764b107046418435c8d9b0b38..7d35758667906f15ea9eedb0ab33156bd06fbcf6 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.cpp
@@ -2,18 +2,23 @@
#include <ArmarXCore/core/logging/Logging.h>
-
namespace armarx::armem::server::wm::detail
{
- void MaxHistorySize::setMaxHistorySize(long maxSize, const std::string& additionalInfo)
+ void
+ MaxHistorySize::setMaxHistorySize(long maxSize, const std::string& additionalInfo)
{
- if (maxSize < 0){
- if(additionalInfo == ""){
- ARMARX_DEBUG << "The maxHistorySize for this memory entity is set to < 0. "
- << "This means nothing will ever be forgotten in working memory. "
- << "This may slow down the memory server. \n"
- << "It is better to set the maxHistorySize per Provider- or CoreSegment";
- } else {
+ if (maxSize < 0)
+ {
+ if (additionalInfo == "")
+ {
+ ARMARX_DEBUG
+ << "The maxHistorySize for this memory entity is set to < 0. "
+ << "This means nothing will ever be forgotten in working memory. "
+ << "This may slow down the memory server. \n"
+ << "It is better to set the maxHistorySize per Provider- or CoreSegment";
+ }
+ else
+ {
ARMARX_DEBUG << "The maxHistorySize for this memory entity is set to < 0. "
<< "This means nothing will ever be forgotten in working memory. "
<< "This may slow down the memory server."
@@ -23,8 +28,9 @@ namespace armarx::armem::server::wm::detail
this->_maxHistorySize = maxSize;
}
- long MaxHistorySize::getMaxHistorySize() const
+ long
+ MaxHistorySize::getMaxHistorySize() const
{
return _maxHistorySize;
}
-}
+} // namespace armarx::armem::server::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
index 7d0bbd6691f7b0a57ffe231fe2bb4faf828fd488..fbe0081456f737b57a0d285122c6ee3cc6a50171 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/MaxHistorySize.h
@@ -7,7 +7,6 @@ namespace armarx::armem::server::wm::detail
class MaxHistorySize
{
public:
-
/**
* @brief Set the maximum number of snapshots to be contained in an entity.
* Affected entities are to be update right away.
@@ -18,7 +17,6 @@ namespace armarx::armem::server::wm::detail
protected:
-
/**
* @brief Maximum size of entity histories.
*
@@ -27,30 +25,24 @@ namespace armarx::armem::server::wm::detail
* @see Entity::maxHstorySize
*/
long _maxHistorySize = -1;
-
};
-
-
template <class DerivedT>
class MaxHistorySizeParent : public MaxHistorySize
{
public:
-
/**
* @brief Sets the maximum history size of entities in this container.
* This affects all current entities as well as new ones.
*
* @see MaxHistorySize::setMaxHistorySize()
*/
- void setMaxHistorySize(long maxSize)
+ void
+ setMaxHistorySize(long maxSize)
{
MaxHistorySize::setMaxHistorySize(maxSize);
- static_cast<DerivedT&>(*this).forEachChild([maxSize](auto & child)
- {
- child.setMaxHistorySize(maxSize);
- });
+ static_cast<DerivedT&>(*this).forEachChild([maxSize](auto& child)
+ { child.setMaxHistorySize(maxSize); });
}
-
};
-}
+} // namespace armarx::armem::server::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h b/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
index 3b2eccc022514313ceb0aaea7154e5c77258ea79..e51262c58a7d3049ceb54df14eb50b39ead87655 100644
--- a/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
+++ b/source/RobotAPI/libraries/armem/server/wm/detail/Prediction.h
@@ -30,12 +30,11 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Prediction.h>
+#include <RobotAPI/libraries/armem/core/base/detail/Predictive.h>
#include <RobotAPI/libraries/armem/core/base/detail/derived.h>
#include <RobotAPI/libraries/armem/core/base/detail/lookup_mixins.h>
-#include <RobotAPI/libraries/armem/core/base/detail/Predictive.h>
#include <RobotAPI/libraries/armem/core/container_maps.h>
-
namespace armarx::armem::server::wm::detail
{
@@ -53,7 +52,8 @@ namespace armarx::armem::server::wm::detail
{
using Predictive = armem::base::detail::Predictive<DerivedT>;
- Predictive* _asPredictive()
+ Predictive*
+ _asPredictive()
{
return dynamic_cast<Predictive*>(&base::detail::derived<DerivedT>(this));
}
@@ -150,7 +150,6 @@ namespace armarx::armem::server::wm::detail
std::map<std::string, Predictor> _predictors; // NOLINT
};
-
/**
* Can do predictions itself and has children it could delegate predictions to.
*/
@@ -160,8 +159,8 @@ namespace armarx::armem::server::wm::detail
public:
using Prediction<DerivedT>::Prediction;
- explicit PredictionContainer(const std::map<PredictionEngine, Predictor>& predictors = {})
- : Prediction<DerivedT>(predictors)
+ explicit PredictionContainer(const std::map<PredictionEngine, Predictor>& predictors = {}) :
+ Prediction<DerivedT>(predictors)
{
}
@@ -180,13 +179,15 @@ namespace armarx::armem::server::wm::detail
auto iter =
armem::findMostSpecificEntryContainingIDAnd<std::vector<PredictionEngine>>(
- engines, request.snapshotID,
+ engines,
+ request.snapshotID,
[&request](const MemoryID& /*unused*/,
const std::vector<PredictionEngine>& supported) -> bool
{
for (const PredictionEngine& engine : supported)
{
- if (engine.engineID == request.predictionSettings.predictionEngineID)
+ if (engine.engineID ==
+ request.predictionSettings.predictionEngineID)
{
return true;
}
@@ -213,7 +214,6 @@ namespace armarx::armem::server::wm::detail
return results;
}
-
/**
* Semantics: This container or one of its children (target) is responsible
* for performing the prediction.
@@ -272,8 +272,8 @@ namespace armarx::armem::server::wm::detail
}
private:
-
- std::string _getChildName(const MemoryID& parent, const MemoryID& child)
+ std::string
+ _getChildName(const MemoryID& parent, const MemoryID& child)
{
ARMARX_CHECK(armem::contains(parent, child));
ARMARX_CHECK(parent != child);
@@ -288,8 +288,6 @@ namespace armarx::armem::server::wm::detail
return childItems[index];
}
-
-
};
} // namespace armarx::armem::server::wm::detail
diff --git a/source/RobotAPI/libraries/armem/server/wm/ice_conversions.cpp b/source/RobotAPI/libraries/armem/server/wm/ice_conversions.cpp
index 1ba3352d6e8bb443693ebfa5752a46cad9c24267..19bcd419c30ec2d095fa3ab2683768f054adbd12 100644
--- a/source/RobotAPI/libraries/armem/server/wm/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/ice_conversions.cpp
@@ -2,63 +2,79 @@
#include <RobotAPI/libraries/armem/core/base/ice_conversions.h>
-
namespace armarx::armem::server
{
- void wm::toIce(data::EntityInstance& ice, const EntityInstance& data)
+ void
+ wm::toIce(data::EntityInstance& ice, const EntityInstance& data)
{
base::toIce(ice, data);
}
- void wm::fromIce(const data::EntityInstance& ice, EntityInstance& data)
+
+ void
+ wm::fromIce(const data::EntityInstance& ice, EntityInstance& data)
{
base::fromIce(ice, data);
}
-
- void wm::toIce(data::EntitySnapshot& ice, const EntitySnapshot& snapshot)
+ void
+ wm::toIce(data::EntitySnapshot& ice, const EntitySnapshot& snapshot)
{
base::toIce(ice, snapshot);
}
- void wm::fromIce(const data::EntitySnapshot& ice, EntitySnapshot& snapshot)
+
+ void
+ wm::fromIce(const data::EntitySnapshot& ice, EntitySnapshot& snapshot)
{
base::fromIce(ice, snapshot);
}
- void wm::toIce(data::Entity& ice, const Entity& entity)
+ void
+ wm::toIce(data::Entity& ice, const Entity& entity)
{
base::toIce(ice, entity);
}
- void wm::fromIce(const data::Entity& ice, Entity& entity)
+
+ void
+ wm::fromIce(const data::Entity& ice, Entity& entity)
{
base::fromIce(ice, entity);
}
- void wm::toIce(data::ProviderSegment& ice, const ProviderSegment& providerSegment)
+ void
+ wm::toIce(data::ProviderSegment& ice, const ProviderSegment& providerSegment)
{
base::toIce(ice, providerSegment);
}
- void wm::fromIce(const data::ProviderSegment& ice, ProviderSegment& providerSegment)
+
+ void
+ wm::fromIce(const data::ProviderSegment& ice, ProviderSegment& providerSegment)
{
base::fromIce(ice, providerSegment);
}
- void wm::toIce(data::CoreSegment& ice, const CoreSegment& coreSegment)
+ void
+ wm::toIce(data::CoreSegment& ice, const CoreSegment& coreSegment)
{
base::toIce(ice, coreSegment);
}
- void wm::fromIce(const data::CoreSegment& ice, CoreSegment& coreSegment)
+
+ void
+ wm::fromIce(const data::CoreSegment& ice, CoreSegment& coreSegment)
{
base::fromIce(ice, coreSegment);
}
- void wm::toIce(data::Memory& ice, const Memory& memory)
+ void
+ wm::toIce(data::Memory& ice, const Memory& memory)
{
base::toIce(ice, memory);
}
- void wm::fromIce(const data::Memory& ice, Memory& memory)
+
+ void
+ wm::fromIce(const data::Memory& ice, Memory& memory)
{
base::fromIce(ice, memory);
}
-}
+} // namespace armarx::armem::server
diff --git a/source/RobotAPI/libraries/armem/server/wm/ice_conversions.h b/source/RobotAPI/libraries/armem/server/wm/ice_conversions.h
index c25b16775bc577c61d0c804cf4525b23ecd5e5d1..4b597ce7ebd42dcdc54438aec48bbf955336f6c9 100644
--- a/source/RobotAPI/libraries/armem/server/wm/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/server/wm/ice_conversions.h
@@ -4,7 +4,6 @@
#include "memory_definitions.h"
-
namespace armarx::armem::server::wm
{
@@ -27,4 +26,4 @@ namespace armarx::armem::server::wm
void toIce(data::Memory& ice, const Memory& memory);
void fromIce(const data::Memory& ice, Memory& memory);
-}
+} // namespace armarx::armem::server::wm
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
index b4dfb1cf6de2a847c5285208f8f157e9d57409ad..0f21567abbc3b9a3782f384fed7f73139d2c289e 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.cpp
@@ -1,36 +1,36 @@
#include "memory_definitions.h"
-#include "error.h"
-
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-#include <RobotAPI/libraries/armem/core/container_maps.h>
+#include <map>
+#include <vector>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <map>
-#include <vector>
+#include <RobotAPI/libraries/armem/core/container_maps.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include "error.h"
namespace armarx::armem::server::wm
{
- void Entity::setMaxHistorySize(long maxSize)
+ void
+ Entity::setMaxHistorySize(long maxSize)
{
std::string containerID = (this->id()).str();
MaxHistorySize::setMaxHistorySize(maxSize, containerID);
truncate();
}
-
- auto Entity::update(const EntityUpdate& update) -> UpdateResult
+ auto
+ Entity::update(const EntityUpdate& update) -> UpdateResult
{
UpdateResult result = EntityBase::update(update);
result.removedSnapshots = this->truncate();
return result;
}
-
- std::vector<EntitySnapshot> Entity::truncate()
+ std::vector<EntitySnapshot>
+ Entity::truncate()
{
std::vector<EntitySnapshot> removedElements;
if (_maxHistorySize >= 0)
@@ -45,7 +45,6 @@ namespace armarx::armem::server::wm
return removedElements;
}
-
// TODO: add core segment if param is set
std::vector<Memory::Base::UpdateResult>
Memory::updateLocking(const Commit& commit)
@@ -68,16 +67,17 @@ namespace armarx::armem::server::wm
CoreSegment& coreSegment = it->second;
// Lock the core segment for the whole batch.
- coreSegment.doLocked([&result, &coreSegment, updates = &updates]()
- {
- for (const EntityUpdate* update : *updates)
+ coreSegment.doLocked(
+ [&result, &coreSegment, updates = &updates]()
{
- auto r = coreSegment.update(*update);
- Base::UpdateResult ret { r };
- ret.memoryUpdateType = UpdateType::UpdatedExisting;
- result.push_back(ret);
- }
- });
+ for (const EntityUpdate* update : *updates)
+ {
+ auto r = coreSegment.update(*update);
+ Base::UpdateResult ret{r};
+ ret.memoryUpdateType = UpdateType::UpdatedExisting;
+ result.push_back(ret);
+ }
+ });
}
else
{
@@ -89,12 +89,12 @@ namespace armarx::armem::server::wm
if (not missingCoreSegmentNames.empty())
{
// Just throw an exception for the first entry. We can extend this exception in the future.
- throw armem::error::MissingEntry::create<CoreSegment>(missingCoreSegmentNames.front(), *this);
+ throw armem::error::MissingEntry::create<CoreSegment>(missingCoreSegmentNames.front(),
+ *this);
}
return result;
}
-
// TODO: Add core segment if param is set
Memory::Base::UpdateResult
Memory::updateLocking(const EntityUpdate& update)
@@ -103,11 +103,8 @@ namespace armarx::armem::server::wm
CoreSegment& segment = getCoreSegment(update.entityID.coreSegmentName);
Base::UpdateResult result;
- segment.doLocked([&result, &segment, &update]()
- {
- result = segment.update(update);
- });
+ segment.doLocked([&result, &segment, &update]() { result = segment.update(update); });
result.memoryUpdateType = UpdateType::UpdatedExisting;
return result;
}
-}
+} // namespace armarx::armem::server::wm
diff --git a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
index 1249dcf039eac37545cc3b93cdf978eaa61be57f..a0eb1aaa67396115d490277153e904f14271721e 100644
--- a/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
+++ b/source/RobotAPI/libraries/armem/server/wm/memory_definitions.h
@@ -1,20 +1,18 @@
#pragma once
-#include "detail/MaxHistorySize.h"
-#include "detail/Prediction.h"
+#include <mutex>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
+#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
#include <RobotAPI/libraries/armem/core/base/EntityInstanceBase.h>
#include <RobotAPI/libraries/armem/core/base/EntitySnapshotBase.h>
-#include <RobotAPI/libraries/armem/core/base/EntityBase.h>
-#include <RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h>
-#include <RobotAPI/libraries/armem/core/base/CoreSegmentBase.h>
#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
-
+#include <RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h>
#include <RobotAPI/libraries/armem/core/wm/detail/data_lookup_mixins.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <mutex>
-
+#include "detail/MaxHistorySize.h"
+#include "detail/Prediction.h"
namespace armarx::armem::server::wm
{
@@ -25,15 +23,13 @@ namespace armarx::armem::server::wm
using EntityInstance = armem::wm::EntityInstance;
using EntitySnapshot = armem::wm::EntitySnapshot;
-
/// @see base::EntityBase
class Entity :
- public base::EntityBase<EntitySnapshot, Entity>
- , public detail::MaxHistorySize
- , public armem::wm::detail::FindInstanceDataMixinForEntity<Entity>
+ public base::EntityBase<EntitySnapshot, Entity>,
+ public detail::MaxHistorySize,
+ public armem::wm::detail::FindInstanceDataMixinForEntity<Entity>
{
public:
-
using base::EntityBase<EntitySnapshot, Entity>::EntityBase;
@@ -48,76 +44,70 @@ namespace armarx::armem::server::wm
protected:
-
/// If maximum size is set, ensure `history`'s is not higher.
std::vector<EntitySnapshotT> truncate();
-
};
-
-
/// @see base::ProviderSegmentBase
class ProviderSegment :
- public base::ProviderSegmentBase<Entity, ProviderSegment>
- , public detail::MaxHistorySizeParent<ProviderSegment>
- , public armem::wm::detail::FindInstanceDataMixin<ProviderSegment>
- , public armem::server::wm::detail::Prediction<ProviderSegment>
+ public base::ProviderSegmentBase<Entity, ProviderSegment>,
+ public detail::MaxHistorySizeParent<ProviderSegment>,
+ public armem::wm::detail::FindInstanceDataMixin<ProviderSegment>,
+ public armem::server::wm::detail::Prediction<ProviderSegment>
{
public:
-
using base::ProviderSegmentBase<Entity, ProviderSegment>::ProviderSegmentBase;
using ProviderSegmentBase::addEntity;
- template <class ...Args>
- Entity& addEntity(const std::string& name, Args... args)
+ template <class... Args>
+ Entity&
+ addEntity(const std::string& name, Args... args)
{
Entity& added = ProviderSegmentBase::addEntity(name, args...);
added.setMaxHistorySize(this->getMaxHistorySize());
return added;
}
-
};
-
-
/// @brief base::CoreSegmentBase
class CoreSegment :
- public base::CoreSegmentBase<ProviderSegment, CoreSegment>
- , public detail::MaxHistorySizeParent<CoreSegment>
- , public armem::wm::detail::FindInstanceDataMixin<CoreSegment>
- , public armem::server::wm::detail::PredictionContainer<CoreSegment>
+ public base::CoreSegmentBase<ProviderSegment, CoreSegment>,
+ public detail::MaxHistorySizeParent<CoreSegment>,
+ public armem::wm::detail::FindInstanceDataMixin<CoreSegment>,
+ public armem::server::wm::detail::PredictionContainer<CoreSegment>
{
using Base = base::CoreSegmentBase<ProviderSegment, CoreSegment>;
public:
-
using Base::CoreSegmentBase;
/// @see base::CoreSegmentBase::addProviderSegment()
using CoreSegmentBase::addProviderSegment;
- template <class ...Args>
- ProviderSegment& addProviderSegment(const std::string& name, Args... args)
+
+ template <class... Args>
+ ProviderSegment&
+ addProviderSegment(const std::string& name, Args... args)
{
ProviderSegmentT& added = CoreSegmentBase::addProviderSegment(name, args...);
int maxHistorySize = this->getMaxHistorySize();
- if(maxHistorySize < 0){
+ if (maxHistorySize < 0)
+ {
ARMARX_INFO << "The maxHistorySize for this core segment is set to < 0. "
<< "This means nothing will ever be forgotten in working memory. "
<< "This may slow down the memory server. \n"
- << "Core Segment Name: "
- << (this->id()).str();
+ << "Core Segment Name: " << (this->id()).str();
}
added.setMaxHistorySize(maxHistorySize);
return added;
}
-
// Locking interface
template <class FunctionT>
- auto doLocked(FunctionT&& function) const
+ auto
+ doLocked(FunctionT&& function) const
{
std::scoped_lock lock(_mutex);
return function();
@@ -125,23 +115,18 @@ namespace armarx::armem::server::wm
private:
-
mutable std::mutex _mutex;
-
};
-
-
/// @see base::MemoryBase
class Memory :
- public base::MemoryBase<CoreSegment, Memory>
- , public armem::wm::detail::FindInstanceDataMixin<Memory>
- , public armem::server::wm::detail::PredictionContainer<Memory>
+ public base::MemoryBase<CoreSegment, Memory>,
+ public armem::wm::detail::FindInstanceDataMixin<Memory>,
+ public armem::server::wm::detail::PredictionContainer<Memory>
{
using Base = base::MemoryBase<CoreSegment, Memory>;
public:
-
using Base::MemoryBase;
@@ -157,8 +142,6 @@ namespace armarx::armem::server::wm
* @brief Update the memory, locking the updated core segment.
*/
Base::UpdateResult updateLocking(const EntityUpdate& update);
-
};
-}
-
+} // namespace armarx::armem::server::wm
diff --git a/source/RobotAPI/libraries/armem/test/ArMemEntityInstanceDataTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemEntityInstanceDataTest.cpp
index 24193227415c833e91251a03208e830738ca1b31..67e6c797d2648ec20de9b4cf92a6d72f760ce955 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemEntityInstanceDataTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemEntityInstanceDataTest.cpp
@@ -24,10 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+
#include <iostream>
#include <set>
-#include <RobotAPI/Test.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/common/aron/Color.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron/time.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
index 53771403181a1afcba608d94979faa54db61404b..44384b3fee396ffcdc0ba53d55a4e95d0d7df406 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemForEachTest.cpp
@@ -24,10 +24,11 @@
#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+
#include <iostream>
#include <set>
-#include <RobotAPI/Test.h>
#include <RobotAPI/libraries/armem/core/Commit.h>
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
diff --git a/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
index 4c133b5ac32e58eaf573e3b26f334b148c6a17cb..8587db94c69a5242413f2aae64b5b13b52038982 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemGetFindTest.cpp
@@ -24,9 +24,10 @@
#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+
#include <iostream>
-#include <RobotAPI/Test.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
diff --git a/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
index 071b0bc4b38882bbd7e87307d946312ba50d6f10..9342630e1850a348ab995e766571e2fafcd4ad51 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemIceConversionsTest.cpp
@@ -25,17 +25,19 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <iostream>
+
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+
namespace armem = armarx::armem;
namespace aron = armarx::aron;
-
BOOST_AUTO_TEST_CASE(test_entity)
{
armem::wm::Entity entity("entity");
@@ -67,6 +69,6 @@ BOOST_AUTO_TEST_CASE(test_entity)
BOOST_CHECK_EQUAL(entityOut.size(), entity.size());
std::vector<armem::Time> timestamps = entityOut.getTimestamps();
- BOOST_CHECK_EQUAL_COLLECTIONS(timestamps.begin(), timestamps.end(), expectedTimestamps.begin(), expectedTimestamps.end());
+ BOOST_CHECK_EQUAL_COLLECTIONS(
+ timestamps.begin(), timestamps.end(), expectedTimestamps.begin(), expectedTimestamps.end());
}
-
diff --git a/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
index 987cf20c980e0478056dd529d4ad877de2499ba4..875542b5c98d12ba85a1b09baead7695902ad855 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
@@ -25,14 +25,14 @@
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem/core/error.h>
#include <iostream>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/error.h>
-namespace armem = armarx::armem;
+namespace armem = armarx::armem;
BOOST_AUTO_TEST_CASE(test_MemoryID_contains)
{
@@ -91,11 +91,9 @@ BOOST_AUTO_TEST_CASE(test_MemoryID_contains)
}
}
-
-
BOOST_AUTO_TEST_CASE(test_MemoryID_from_to_string)
{
- armem::MemoryID idIn {"Memory/Core/Prov/entity/2810381/2"};
+ armem::MemoryID idIn{"Memory/Core/Prov/entity/2810381/2"};
BOOST_CHECK_EQUAL(idIn.memoryName, "Memory");
BOOST_CHECK_EQUAL(idIn.coreSegmentName, "Core");
@@ -111,7 +109,7 @@ BOOST_AUTO_TEST_CASE(test_MemoryID_from_to_string)
BOOST_CHECK_EQUAL(idOut, idIn);
}
- idIn.entityName = "KIT/Amicelli/0"; // Like an ObjectID
+ idIn.entityName = "KIT/Amicelli/0"; // Like an ObjectID
BOOST_CHECK_EQUAL(idIn.entityName, "KIT/Amicelli/0");
BOOST_TEST_CONTEXT(VAROUT(idIn.str()))
@@ -121,7 +119,7 @@ BOOST_AUTO_TEST_CASE(test_MemoryID_from_to_string)
BOOST_CHECK_EQUAL(idOut, idIn);
}
- idIn = armem::MemoryID {"InThe\\/Mid/AtTheEnd\\//\\/AtTheStart/YCB\\/sugar\\/-1//2"};
+ idIn = armem::MemoryID{"InThe\\/Mid/AtTheEnd\\//\\/AtTheStart/YCB\\/sugar\\/-1//2"};
BOOST_CHECK_EQUAL(idIn.memoryName, "InThe/Mid");
BOOST_CHECK_EQUAL(idIn.coreSegmentName, "AtTheEnd/");
BOOST_CHECK_EQUAL(idIn.providerSegmentName, "/AtTheStart");
@@ -136,10 +134,9 @@ BOOST_AUTO_TEST_CASE(test_MemoryID_from_to_string)
}
}
-
BOOST_AUTO_TEST_CASE(test_MemoryID_copy_move_ctors_ops)
{
- const armem::MemoryID id("A/B/C/123/1"), moved("////1"); // int is not moved
+ const armem::MemoryID id("A/B/C/123/1"), moved("////1"); // int is not moved
{
const armem::MemoryID out(id);
BOOST_CHECK_EQUAL(out, id);
diff --git a/source/RobotAPI/libraries/armem/test/ArMemPrefixesTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemPrefixesTest.cpp
index 4efc404325b61dbea0f68c7ec1bda5ecb051b8c6..e327d2cc9033d3ed0f24d160c8c066db5b21b604 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemPrefixesTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemPrefixesTest.cpp
@@ -24,11 +24,12 @@
#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+
#include <iostream>
-#include <RobotAPI/Test.h>
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/container_maps.h>
+#include <RobotAPI/libraries/armem/core/error.h>
namespace armem = armarx::armem;
@@ -74,8 +75,8 @@ BOOST_AUTO_TEST_CASE(test_MemoryID_prefixes)
BOOST_CHECK((armem::accumulateEntriesContainingID(idMap, empty) == std::vector<int>{0}));
BOOST_CHECK((armem::accumulateEntriesContainingID(idMap, complete.getCoreSegmentID()) ==
std::vector<int>{2, 1, 0}));
- BOOST_CHECK(
- (armem::accumulateEntriesContainingID(idMap, complete) == std::vector<int>{3, 2, 1, 0}));
+ BOOST_CHECK((armem::accumulateEntriesContainingID(idMap, complete) ==
+ std::vector<int>{3, 2, 1, 0}));
}
idListMap.emplace("mem", std::vector<int>{1, 2});
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
index ebf288ab7f2a9e4f831b1433b3573e94dee47fac..1738bd7d3fa84a8e98036f18379b8a307cef73f8 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryBuilderTest.cpp
@@ -26,19 +26,18 @@
#include <RobotAPI/Test.h>
-#include <ArmarXCore/core/time/ice_conversions.h>
+#include <iostream>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <iostream>
-
namespace armem = armarx::armem;
namespace query = armarx::armem::query::data;
-
namespace ArMemQueryBuilderTest
{
struct Fixture
@@ -46,25 +45,20 @@ namespace ArMemQueryBuilderTest
Fixture()
{
}
+
~Fixture()
{
}
};
-}
+} // namespace ArMemQueryBuilderTest
BOOST_FIXTURE_TEST_SUITE(ArMemQueryBuilderTest, ArMemQueryBuilderTest::Fixture)
-
-
BOOST_AUTO_TEST_CASE(test_all_all)
{
armem::client::query::Builder qb;
- qb
- .coreSegments().all()
- .providerSegments().all()
- .entities().all()
- .snapshots().all();
+ qb.coreSegments().all().providerSegments().all().entities().all().snapshots().all();
query::MemoryQuerySeq memoryQueries = qb.buildMemoryQueries();
@@ -85,7 +79,6 @@ BOOST_AUTO_TEST_CASE(test_all_all)
BOOST_CHECK(query::entity::AllPtr::dynamicCast(entityQuery));
}
-
BOOST_AUTO_TEST_CASE(test_mixed)
{
using namespace armem::client::query_fns;
@@ -93,17 +86,14 @@ BOOST_AUTO_TEST_CASE(test_mixed)
std::vector<std::string> entityNames = {"one", "two"};
armem::client::query::Builder qb(armem::query::DataMode::WithData);
- qb
- .coreSegments(withName("core"))
- .providerSegments(withName("provider"))
- .entities(withName(entityNames.at(0)),
- withName(entityNames.at(1)))
- .snapshots(latest(),
- atTime(armem::Time(armem::Duration::MilliSeconds(3000))),
- indexRange(5, -10),
- timeRange(armem::Time(armem::Duration::MilliSeconds(1000)),
- armem::Time(armem::Duration::MilliSeconds(2000))))
- ;
+ qb.coreSegments(withName("core"))
+ .providerSegments(withName("provider"))
+ .entities(withName(entityNames.at(0)), withName(entityNames.at(1)))
+ .snapshots(latest(),
+ atTime(armem::Time(armem::Duration::MilliSeconds(3000))),
+ indexRange(5, -10),
+ timeRange(armem::Time(armem::Duration::MilliSeconds(1000)),
+ armem::Time(armem::Duration::MilliSeconds(2000))));
armem::client::QueryInput input = qb.buildQueryInput();
@@ -129,9 +119,11 @@ BOOST_AUTO_TEST_CASE(test_mixed)
BOOST_REQUIRE_EQUAL(coreSegQuery->providerSegmentQueries.size(), entityNames.size());
for (size_t i = 0; i < coreSegQuery->providerSegmentQueries.size(); ++i)
{
- const query::ProviderSegmentQueryPtr& provSegQuery = coreSegQuery->providerSegmentQueries[i];
+ const query::ProviderSegmentQueryPtr& provSegQuery =
+ coreSegQuery->providerSegmentQueries[i];
{
- query::provider::SinglePtr single = query::provider::SinglePtr::dynamicCast(provSegQuery);
+ query::provider::SinglePtr single =
+ query::provider::SinglePtr::dynamicCast(provSegQuery);
BOOST_REQUIRE(single);
BOOST_CHECK_EQUAL(single->entityName, entityNames.at(i));
}
@@ -141,13 +133,16 @@ BOOST_AUTO_TEST_CASE(test_mixed)
{
auto latest = query::entity::SinglePtr::dynamicCast(*it);
BOOST_REQUIRE(latest);
- BOOST_CHECK_EQUAL(latest->timestamp.timeSinceEpoch.microSeconds, armem::Time::Invalid().toMicroSecondsSinceEpoch());
+ BOOST_CHECK_EQUAL(latest->timestamp.timeSinceEpoch.microSeconds,
+ armem::Time::Invalid().toMicroSecondsSinceEpoch());
++it;
}
{
auto atTime = query::entity::SinglePtr::dynamicCast(*it);
BOOST_REQUIRE(atTime);
- BOOST_CHECK_EQUAL(atTime->timestamp, armarx::toIce<armem::dto::Time>(armem::Time(armem::Duration::MilliSeconds(3000))));
+ BOOST_CHECK_EQUAL(
+ atTime->timestamp,
+ armarx::toIce<armem::dto::Time>(armem::Time(armem::Duration::MilliSeconds(3000))));
++it;
}
{
@@ -160,38 +155,36 @@ BOOST_AUTO_TEST_CASE(test_mixed)
{
auto timeRange = query::entity::TimeRangePtr::dynamicCast(*it);
BOOST_REQUIRE(timeRange);
- BOOST_CHECK_EQUAL(timeRange->minTimestamp, armarx::toIce<armem::dto::Time>(armem::Time(armem::Duration::MilliSeconds(1000))));
- BOOST_CHECK_EQUAL(timeRange->maxTimestamp, armarx::toIce<armem::dto::Time>(armem::Time(armem::Duration::MilliSeconds(2000))));
+ BOOST_CHECK_EQUAL(
+ timeRange->minTimestamp,
+ armarx::toIce<armem::dto::Time>(armem::Time(armem::Duration::MilliSeconds(1000))));
+ BOOST_CHECK_EQUAL(
+ timeRange->maxTimestamp,
+ armarx::toIce<armem::dto::Time>(armem::Time(armem::Duration::MilliSeconds(2000))));
++it;
}
}
}
-
-
BOOST_AUTO_TEST_CASE(test_branched_hierarchy)
{
armem::client::query::Builder qb;
// Common root tree
- armem::client::query::ProviderSegmentSelector& provider = qb
- .coreSegments().withName("core")
- .providerSegments().withName("provider");
+ armem::client::query::ProviderSegmentSelector& provider =
+ qb.coreSegments().withName("core").providerSegments().withName("provider");
// 1st entity branch
- provider
- .entities().withName("first")
- .snapshots().all();
+ provider.entities().withName("first").snapshots().all();
// 2nd entity branch
- provider
- .entities().withNamesMatching(".*_regex")
- .snapshots().latest();
+ provider.entities().withNamesMatching(".*_regex").snapshots().latest();
BOOST_TEST_MESSAGE(std::numeric_limits<long>::min());
armem::client::QueryInput input = qb.buildQueryInput();
- const query::ProviderSegmentQuerySeq& provQueries = input.memoryQueries.front()->coreSegmentQueries.front()->providerSegmentQueries;
+ const query::ProviderSegmentQuerySeq& provQueries =
+ input.memoryQueries.front()->coreSegmentQueries.front()->providerSegmentQueries;
BOOST_CHECK_EQUAL(provQueries.size(), 2);
auto it = provQueries.begin();
@@ -214,7 +207,8 @@ BOOST_AUTO_TEST_CASE(test_branched_hierarchy)
BOOST_CHECK_EQUAL(regex->entityQueries.size(), 1);
auto snapshotLatest = query::entity::SinglePtr::dynamicCast(regex->entityQueries.at(0));
BOOST_CHECK(snapshotLatest);
- BOOST_CHECK_EQUAL(snapshotLatest->timestamp.timeSinceEpoch.microSeconds, armem::Time::Invalid().toMicroSecondsSinceEpoch());
+ BOOST_CHECK_EQUAL(snapshotLatest->timestamp.timeSinceEpoch.microSeconds,
+ armem::Time::Invalid().toMicroSecondsSinceEpoch());
++it;
}
diff --git a/source/RobotAPI/libraries/armem/util/prediction_helpers.h b/source/RobotAPI/libraries/armem/util/prediction_helpers.h
index 1734dca8f20e488e789e0c39af380b24376a5e9f..076411f4d96ccf80a38ce78beeb958828a05a544 100644
--- a/source/RobotAPI/libraries/armem/util/prediction_helpers.h
+++ b/source/RobotAPI/libraries/armem/util/prediction_helpers.h
@@ -70,11 +70,11 @@ namespace armarx::armem
template <typename SegmentType, typename DataType, typename LatestType>
SnapshotRangeInfo<DataType, LatestType>
getSnapshotsInRange(const SegmentType* segment,
- const MemoryID& entityID,
- const DateTime& startTime,
- const DateTime& endTime,
- std::function<DataType(const aron::data::DictPtr&)> dictToData,
- std::function<LatestType(const aron::data::DictPtr&)> dictToLatest)
+ const MemoryID& entityID,
+ const DateTime& startTime,
+ const DateTime& endTime,
+ std::function<DataType(const aron::data::DictPtr&)> dictToData,
+ std::function<LatestType(const aron::data::DictPtr&)> dictToLatest)
{
SnapshotRangeInfo<DataType, LatestType> result;
result.success = false;
@@ -128,7 +128,7 @@ namespace armarx::armem
result.errorMessage = sstream.str();
}
}
-
+
return result;
}
diff --git a/source/RobotAPI/libraries/armem/util/util.cpp b/source/RobotAPI/libraries/armem/util/util.cpp
index fcaab64e05b6eb35e9c516a9637f77a97b210952..5e4cca5943e68411471ca81e274b4d3b4b9991fa 100644
--- a/source/RobotAPI/libraries/armem/util/util.cpp
+++ b/source/RobotAPI/libraries/armem/util/util.cpp
@@ -30,7 +30,6 @@ namespace armarx::armem
return {};
}
-
std::optional<std::pair<armarx::aron::data::DictPtr, armarx::aron::type::ObjectPtr>>
extractDataAndType(const armarx::armem::wm::Memory& memory,
const armarx::armem::MemoryID& memoryID)
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index 050df3f880d715e44f2e07f37e06bf9b6708d760..b494024a3259f6ee6074956935d14e454dac0024 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -21,8 +21,8 @@
#pragma once
-#include <vector>
#include <optional>
+#include <vector>
#include <ArmarXCore/core/logging/Logging.h>
@@ -30,7 +30,6 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>
-
namespace armarx::armem
{
@@ -42,7 +41,8 @@ namespace armarx::armem
* @return std::optional<AronClass>
*/
template <typename AronClass>
- std::optional<AronClass> tryCast(const wm::EntityInstance& item)
+ std::optional<AronClass>
+ tryCast(const wm::EntityInstance& item)
{
static_assert(std::is_base_of<armarx::aron::cpp::AronGeneratedClass, AronClass>::value);
@@ -86,17 +86,18 @@ namespace armarx::armem
std::vector<AronClass> outV;
// loop over all entities and their snapshots
- for (const auto &[s, entity] : entities)
+ for (const auto& [s, entity] : entities)
{
- entity.forEachInstance([&outV](const wm::EntityInstance& entityInstance)
- {
- const auto o = tryCast<AronClass>(entityInstance);
-
- if (o)
+ entity.forEachInstance(
+ [&outV](const wm::EntityInstance& entityInstance)
{
- outV.push_back(*o);
- }
- });
+ const auto o = tryCast<AronClass>(entityInstance);
+
+ if (o)
+ {
+ outV.push_back(*o);
+ }
+ });
}
return outV;
@@ -121,8 +122,9 @@ namespace armarx::armem
* @return vector of "pred"-transformed elements that can be cast to AronClass
*/
template <typename AronClass>
- auto transformAllOfType(const std::map<std::string, wm::Entity>& entities,
- auto pred) -> std::vector<decltype(pred(AronClass()))>
+ auto
+ transformAllOfType(const std::map<std::string, wm::Entity>& entities, auto pred)
+ -> std::vector<decltype(pred(AronClass()))>
{
static_assert(std::is_base_of<armarx::aron::cpp::AronGeneratedClass, AronClass>::value);
@@ -134,17 +136,18 @@ namespace armarx::armem
}
// loop over all entities and their snapshots
- for (const auto &[s, entity] : entities)
+ for (const auto& [s, entity] : entities)
{
- entity.forEachInstance([&outV, &pred](const wm::EntityInstance& entityInstance)
- {
- const auto o = tryCast<AronClass>(entityInstance);
-
- if (o)
+ entity.forEachInstance(
+ [&outV, &pred](const wm::EntityInstance& entityInstance)
{
- outV.push_back(pred(*o));
- }
- });
+ const auto o = tryCast<AronClass>(entityInstance);
+
+ if (o)
+ {
+ outV.push_back(pred(*o));
+ }
+ });
}
return outV;
@@ -162,9 +165,8 @@ namespace armarx::armem
* @param memoryID the MemoryID to query for
* @return memory containing the object referenced by the MemoryID if available
*/
- std::optional<armarx::armem::wm::Memory>
- resolveID(armarx::armem::client::MemoryNameSystem& mns,
- const armarx::armem::MemoryID& memoryID);
+ std::optional<armarx::armem::wm::Memory> resolveID(armarx::armem::client::MemoryNameSystem& mns,
+ const armarx::armem::MemoryID& memoryID);
/**
diff --git a/source/RobotAPI/libraries/armem_grasping/armem_grasping.cpp b/source/RobotAPI/libraries/armem_grasping/armem_grasping.cpp
index e6ebd327ebabc155cf493e47f1b7dec224010eb1..ae8cae37202a3facf310e1811a534ed081cc98d3 100644
--- a/source/RobotAPI/libraries/armem_grasping/armem_grasping.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/armem_grasping.cpp
@@ -22,11 +22,10 @@
#include "armem_grasping.h"
-#include "aron_conversions.h"
#include "GraspCandidateReader.h"
#include "GraspCandidateWriter.h"
#include "aron/GraspCandidate.aron.generated.h"
-
+#include "aron_conversions.h"
namespace armarx
{
diff --git a/source/RobotAPI/libraries/armem_grasping/armem_grasping.h b/source/RobotAPI/libraries/armem_grasping/armem_grasping.h
index 4e83d82529eba347bf87baa0c96f02da4603f3a4..51c2ffcbd3958687e95d097550503765fdfdceed 100644
--- a/source/RobotAPI/libraries/armem_grasping/armem_grasping.h
+++ b/source/RobotAPI/libraries/armem_grasping/armem_grasping.h
@@ -22,7 +22,6 @@
#pragma once
-
namespace armarx
{
/**
@@ -41,7 +40,6 @@ namespace armarx
class armem_grasping
{
public:
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
index 2ae44a2916d7dceedeb06a6699b3aaa582bc3cef..9a6dd803dbec75667cb83136de43330256ff3db6 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.cpp
@@ -121,9 +121,9 @@ namespace armarx::armem::grasping::segment
// check if grasp has a prepose by checking if grasp name ends with "_Prepose"
{
const std::string prePoseName = retGrasp.name + PREPOSE_SUFFIX;
-
+
ARMARX_DEBUG << "Checking for prepose '" << prePoseName << "' ...";
-
+
if (preGraspSet->hasGrasp(prePoseName))
{
retGrasp.prepose =
@@ -133,15 +133,15 @@ namespace armarx::armem::grasping::segment
preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
- ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '" << retGrasp.name << "' in set '"
- << retGraspSet.name << "' for obj '" << objectClassName
- << "' with pose \n"
- << retGrasp.prepose.value();
+ ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '"
+ << retGrasp.name << "' in set '" << retGraspSet.name
+ << "' for obj '" << objectClassName << "' with pose \n"
+ << retGrasp.prepose.value();
}
}
// check if grasp has a prepose for a grasp with a specific name, e.g., "XY_Grasp" (GRASP_OPTIONAL_SUFFIX)
- if (// not retGrasp.prepose.has_value() and
+ if ( // not retGrasp.prepose.has_value() and
simox::alg::ends_with(retGrasp.name, GRASP_OPTIONAL_SUFFIX))
{
const std::string prePoseName =
@@ -159,17 +159,17 @@ namespace armarx::armem::grasping::segment
// remove the prepose from the set as it found its match
preGraspSet->removeGrasp(preGraspSet->getGrasp(prePoseName));
- ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '" << retGrasp.name << "' in set '"
- << retGraspSet.name << "' for obj '" << objectClassName
- << "' with pose \n"
- << retGrasp.prepose.value();
+ ARMARX_DEBUG << "Found prepose `" + prePoseName + "` for grasp '"
+ << retGrasp.name << "' in set '" << retGraspSet.name
+ << "' for obj '" << objectClassName << "' with pose \n"
+ << retGrasp.prepose.value();
}
}
ARMARX_DEBUG << "Found grasp '" << retGrasp.name << "' in set '"
- << retGraspSet.name << "' for obj '" << objectClassName
- << "' with pose \n"
- << retGrasp.pose;
+ << retGraspSet.name << "' for obj '" << objectClassName
+ << "' with pose \n"
+ << retGrasp.pose;
retGraspSet.grasps.push_back(retGrasp);
}
@@ -181,7 +181,7 @@ namespace armarx::armem::grasping::segment
<< " preposes in the grasp set '" << retGraspSet.name
<< "' for obj '" << objectClassName
<< "' that do not have a corresponding grasp!";
- for(const auto& preGrasp : preGraspSet->getGrasps())
+ for (const auto& preGrasp : preGraspSet->getGrasps())
{
ARMARX_WARNING << "Prepose '" << preGrasp->getName();
}
diff --git a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
index 75aadd96aff419a636b802fcc73548b080e4d8d8..36504338a877c42932c646521bb0e9c556ab7cc4 100644
--- a/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
+++ b/source/RobotAPI/libraries/armem_grasping/server/KnownGraspProviderSegment.h
@@ -1,8 +1,7 @@
-# pragma once
-
-#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#pragma once
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
namespace armarx::armem::grasping::segment
@@ -28,4 +27,4 @@ namespace armarx::armem::grasping::segment
static const constexpr char* PREPOSE_SUFFIX = "_Prepose";
static const constexpr char* GRASP_OPTIONAL_SUFFIX = "_Grasp";
};
-}
+} // namespace armarx::armem::grasping::segment
diff --git a/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.cpp b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.cpp
index b8bf2121617d084537b456fb741fad34fcbf8e26..186b5e94a80c4ef7b47f70d9d5aa8f72b459ad03 100644
--- a/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.cpp
@@ -2,7 +2,6 @@
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-
namespace armarx::armem::gui
{
ActionsMenuBuilder::ActionsMenuBuilder(
diff --git a/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h
index 4d1261f65cecf4cfd4afd77867ad4e2b9af7ebac..8a9d7f2b4c2abe4df81512cb9c1cd5ec810b2343 100644
--- a/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h
@@ -3,8 +3,8 @@
#include <QMenu>
#include <RobotAPI/interface/armem/actions.h>
-#include <RobotAPI/libraries/armem/core/actions.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/actions.h>
namespace armarx::armem::gui
{
@@ -18,8 +18,7 @@ namespace armarx::armem::gui
QMenu* buildActionsMenu(actions::data::GetActionsOutput& actionsOutput);
private:
- void
- addMenuEntry(QMenu* menu, actions::ActionPath path, const actions::MenuEntry& entry);
+ void addMenuEntry(QMenu* menu, actions::ActionPath path, const actions::MenuEntry& entry);
MemoryID memoryID;
QWidget* parent;
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index fc6c74049d6768053daa7eb8cec2a8553bb0cd9f..6036a13a6e5221b7eb231bbfbf928a4834ce4b35 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -458,7 +458,7 @@ namespace armarx::armem::gui
// Please note: Here we assume that a memory server with the same name does not exist.
// I think this assumption is ok, since nobody should use filepaths as memory name.
// Nonetheless, we did not restrict the user to do so...
- std::string virtualMemoryName = name;// + " (at " + path.string() + ")";
+ std::string virtualMemoryName = name; // + " (at " + path.string() + ")";
memory.id().memoryName = virtualMemoryName;
memoryData[virtualMemoryName] = std::move(memory);
}
diff --git a/source/RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h b/source/RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h
index f3ca45cf17b05c72936a5afb8958e4eb5686aaab..91e6bd8fc789874a3aa4f24fcc91d8a1b66fb788 100644
--- a/source/RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h
@@ -8,7 +8,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx
{
@@ -29,37 +28,55 @@ namespace armarx
TreeWidgetBuilder() = default;
+
/// Constructor to automatically derive the template argument.
TreeWidgetBuilder(const ElementT&)
- {}
+ {
+ }
- TreeWidgetBuilder(CompareFn compareFn, MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate) :
+ TreeWidgetBuilder(CompareFn compareFn,
+ MakeItemFn makeItemFn,
+ UpdateItemFn updateItemFn = NoUpdate) :
compareFn(compareFn), makeItemFn(makeItemFn), updateItemFn(updateItemFn)
- {}
- TreeWidgetBuilder(NameFn nameFn, MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate,
- int nameColumn = 0) :
- compareFn(MakeCompareNameFn(nameFn, nameColumn)), makeItemFn(makeItemFn), updateItemFn(updateItemFn)
- {}
+ {
+ }
+ TreeWidgetBuilder(NameFn nameFn,
+ MakeItemFn makeItemFn,
+ UpdateItemFn updateItemFn = NoUpdate,
+ int nameColumn = 0) :
+ compareFn(MakeCompareNameFn(nameFn, nameColumn)),
+ makeItemFn(makeItemFn),
+ updateItemFn(updateItemFn)
+ {
+ }
- void setCompareFn(CompareFn compareFn)
+ void
+ setCompareFn(CompareFn compareFn)
{
this->compareFn = compareFn;
}
- void setNameFn(NameFn nameFn, int nameColumn = 0)
+
+ void
+ setNameFn(NameFn nameFn, int nameColumn = 0)
{
compareFn = MakeCompareNameFn(nameFn, nameColumn);
}
- void setMakeItemFn(MakeItemFn makeItemFn)
+
+ void
+ setMakeItemFn(MakeItemFn makeItemFn)
{
this->makeItemFn = makeItemFn;
}
- void setUpdateItemFn(UpdateItemFn updateItemFn)
+
+ void
+ setUpdateItemFn(UpdateItemFn updateItemFn)
{
this->updateItemFn = updateItemFn;
}
- void setExpand(bool expand)
+ void
+ setExpand(bool expand)
{
this->expand = expand;
}
@@ -91,11 +108,11 @@ namespace armarx
template <class ParentT, class ContainerT>
void updateTreeWithContainer(ParentT* parent, const ContainerT& elements);
-
/// No update function (default).
- static bool NoUpdate(const ElementT& element, QTreeWidgetItem* item)
+ static bool
+ NoUpdate(const ElementT& element, QTreeWidgetItem* item)
{
- (void) element, (void) item;
+ (void)element, (void)item;
return true;
}
@@ -104,18 +121,14 @@ namespace armarx
private:
-
CompareFn compareFn;
MakeItemFn makeItemFn;
UpdateItemFn updateItemFn;
/// Whether to expand items on first creation.
bool expand = false;
-
};
-
-
/**
* A class to efficiently build and maintain sorted items of `QTreeWidget`
* or `QTreeWidgetItem` based on a map matching the intended structure.
@@ -131,78 +144,93 @@ namespace armarx
using NameFn = std::function<std::string(const KeyT& key, const ValueT& value)>;
using MakeItemFn = std::function<QTreeWidgetItem*(const KeyT& key, const ValueT& value)>;
- using UpdateItemFn = std::function<bool(const KeyT& key, const ValueT& value, QTreeWidgetItem* item)>;
-
+ using UpdateItemFn =
+ std::function<bool(const KeyT& key, const ValueT& value, QTreeWidgetItem* item)>;
MapTreeWidgetBuilder()
{
setNameFn(KeyAsName);
}
+
/// Allows declaring instance from container without explicit template arguments.
MapTreeWidgetBuilder(const MapT&) : MapTreeWidgetBuilder()
- {}
+ {
+ }
- MapTreeWidgetBuilder(MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate) : MapTreeWidgetBuilder()
+ MapTreeWidgetBuilder(MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate) :
+ MapTreeWidgetBuilder()
{
setMakeItemFn(makeItemFn);
setUpdateItemFn(updateItemFn);
}
- MapTreeWidgetBuilder(NameFn nameFn, MakeItemFn makeItemFn, UpdateItemFn updateItemFn = NoUpdate)
+
+ MapTreeWidgetBuilder(NameFn nameFn,
+ MakeItemFn makeItemFn,
+ UpdateItemFn updateItemFn = NoUpdate)
{
setNameFn(nameFn);
setMakeItemFn(makeItemFn);
setUpdateItemFn(updateItemFn);
}
- void setNameFn(NameFn nameFn)
+ void
+ setNameFn(NameFn nameFn)
{
- builder.setNameFn([nameFn](const ElementT & element)
- {
- const auto& [key, value] = element;
- return nameFn(key, value);
- });
+ builder.setNameFn(
+ [nameFn](const ElementT& element)
+ {
+ const auto& [key, value] = element;
+ return nameFn(key, value);
+ });
}
- void setCompareFn(CompareFn compareFn)
+ void
+ setCompareFn(CompareFn compareFn)
{
builder.setCompareFn(compareFn);
}
- void setMakeItemFn(MakeItemFn makeItemFn)
+ void
+ setMakeItemFn(MakeItemFn makeItemFn)
{
- builder.setMakeItemFn([makeItemFn](const ElementT & element)
- {
- const auto& [key, value] = element;
- return makeItemFn(key, value);
- });
+ builder.setMakeItemFn(
+ [makeItemFn](const ElementT& element)
+ {
+ const auto& [key, value] = element;
+ return makeItemFn(key, value);
+ });
}
- void setUpdateItemFn(UpdateItemFn updateItemFn)
+
+ void
+ setUpdateItemFn(UpdateItemFn updateItemFn)
{
- builder.setUpdateItemFn([updateItemFn](const ElementT & element, QTreeWidgetItem * item)
- {
- const auto& [key, value] = element;
- return updateItemFn(key, value, item);
- });
+ builder.setUpdateItemFn(
+ [updateItemFn](const ElementT& element, QTreeWidgetItem* item)
+ {
+ const auto& [key, value] = element;
+ return updateItemFn(key, value, item);
+ });
}
- void setExpand(bool expand)
+ void
+ setExpand(bool expand)
{
builder.setExpand(expand);
}
-
template <class ParentT>
- void updateTree(ParentT* tree, const MapT& elements)
+ void
+ updateTree(ParentT* tree, const MapT& elements)
{
builder.updateTreeWithContainer(tree, elements);
}
-
/// A name function using the key as name.
- static std::string KeyAsName(const KeyT& key, const ValueT& value)
+ static std::string
+ KeyAsName(const KeyT& key, const ValueT& value)
{
- (void) value;
- if constexpr(std::is_same<KeyT, std::string>())
+ (void)value;
+ if constexpr (std::is_same<KeyT, std::string>())
{
return key;
}
@@ -215,67 +243,82 @@ namespace armarx
}
/// No update function (default).
- static bool NoUpdate(const KeyT& key, const ValueT& value, QTreeWidgetItem* item)
+ static bool
+ NoUpdate(const KeyT& key, const ValueT& value, QTreeWidgetItem* item)
{
- (void) key, (void) value, (void) item;
+ (void)key, (void)value, (void)item;
return true;
}
private:
-
TreeWidgetBuilder<typename MapT::value_type> builder;
-
};
-
-
namespace detail
{
- template <class ParentT> struct ParentAPI;
+ template <class ParentT>
+ struct ParentAPI;
- template <> struct ParentAPI<QTreeWidget>
+ template <>
+ struct ParentAPI<QTreeWidget>
{
- static int getItemCount(QTreeWidget* tree)
+ static int
+ getItemCount(QTreeWidget* tree)
{
return tree->topLevelItemCount();
}
- static QTreeWidgetItem* getItem(QTreeWidget* tree, int index)
+
+ static QTreeWidgetItem*
+ getItem(QTreeWidget* tree, int index)
{
return tree->topLevelItem(index);
}
- static void insertItem(QTreeWidget* tree, int index, QTreeWidgetItem* item)
+
+ static void
+ insertItem(QTreeWidget* tree, int index, QTreeWidgetItem* item)
{
tree->insertTopLevelItem(index, item);
}
- static QTreeWidgetItem* takeItem(QTreeWidget* tree, int index)
+
+ static QTreeWidgetItem*
+ takeItem(QTreeWidget* tree, int index)
{
return tree->takeTopLevelItem(index);
}
};
- template <> struct ParentAPI<QTreeWidgetItem>
+ template <>
+ struct ParentAPI<QTreeWidgetItem>
{
- static int getItemCount(QTreeWidgetItem* parent)
+ static int
+ getItemCount(QTreeWidgetItem* parent)
{
return parent->childCount();
}
- static QTreeWidgetItem* getItem(QTreeWidgetItem* parent, int index)
+
+ static QTreeWidgetItem*
+ getItem(QTreeWidgetItem* parent, int index)
{
return parent->child(index);
}
- static QTreeWidgetItem* takeItem(QTreeWidgetItem* parent, int index)
+
+ static QTreeWidgetItem*
+ takeItem(QTreeWidgetItem* parent, int index)
{
return parent->takeChild(index);
}
- static void insertItem(QTreeWidgetItem* parent, int index, QTreeWidgetItem* item)
+
+ static void
+ insertItem(QTreeWidgetItem* parent, int index, QTreeWidgetItem* item)
{
parent->insertChild(index, item);
}
};
template <typename T>
- int compare(const T& lhs, const T& rhs)
+ int
+ compare(const T& lhs, const T& rhs)
{
if (lhs < rhs)
{
@@ -290,43 +333,45 @@ namespace armarx
return 1;
}
}
- inline int compare(const std::string& lhs, const std::string& rhs)
+
+ inline int
+ compare(const std::string& lhs, const std::string& rhs)
{
return lhs.compare(rhs);
}
- }
-
+ } // namespace detail
template <class ElementT>
- auto TreeWidgetBuilder<ElementT>::MakeCompareNameFn(NameFn nameFn, int nameColumn) -> CompareFn
+ auto
+ TreeWidgetBuilder<ElementT>::MakeCompareNameFn(NameFn nameFn, int nameColumn) -> CompareFn
{
- return [nameFn, nameColumn](const ElementT & element, QTreeWidgetItem * item)
- {
- return detail::compare(nameFn(element), item->text(nameColumn).toStdString());
- };
+ return [nameFn, nameColumn](const ElementT& element, QTreeWidgetItem* item)
+ { return detail::compare(nameFn(element), item->text(nameColumn).toStdString()); };
}
-
template <class ElementT>
template <class ParentT, class ContainerT>
- void TreeWidgetBuilder<ElementT>::updateTreeWithContainer(ParentT* parent, const ContainerT& elements)
+ void
+ TreeWidgetBuilder<ElementT>::updateTreeWithContainer(ParentT* parent,
+ const ContainerT& elements)
{
- this->updateTreeWithIterator(parent, [&elements](auto&& elementFn)
- {
- for (const auto& element : elements)
- {
- if (not elementFn(element))
- {
- break;
- }
- }
- });
+ this->updateTreeWithIterator(parent,
+ [&elements](auto&& elementFn)
+ {
+ for (const auto& element : elements)
+ {
+ if (not elementFn(element))
+ {
+ break;
+ }
+ }
+ });
}
-
template <class ElementT>
template <class ParentT, class IteratorFn>
- void TreeWidgetBuilder<ElementT>::updateTreeWithIterator(ParentT* parent, IteratorFn&& iteratorFn)
+ void
+ TreeWidgetBuilder<ElementT>::updateTreeWithIterator(ParentT* parent, IteratorFn&& iteratorFn)
{
using api = detail::ParentAPI<ParentT>;
@@ -335,50 +380,51 @@ namespace armarx
ARMARX_CHECK_NOT_NULL(compareFn) << "compareFn must be set";
int currentIndex = 0;
- iteratorFn([this, &parent, ¤tIndex](const auto & element)
- {
- bool inserted = false;
- QTreeWidgetItem* item = nullptr;
- if (currentIndex >= api::getItemCount(parent))
+ iteratorFn(
+ [this, &parent, ¤tIndex](const auto& element)
{
- // Add elements to the end of the list.
- ARMARX_CHECK_NOT_NULL(makeItemFn);
- item = makeItemFn(element);
- api::insertItem(parent, api::getItemCount(parent), item);
- ++currentIndex;
- inserted = true;
- }
- else
- {
- QTreeWidgetItem* currentItem = api::getItem(parent, currentIndex);
- while (currentItem != nullptr && compareFn(element, currentItem) > 0)
+ bool inserted = false;
+ QTreeWidgetItem* item = nullptr;
+ if (currentIndex >= api::getItemCount(parent))
{
- delete api::takeItem(parent, currentIndex);
- currentItem = api::getItem(parent, currentIndex);
- }
- if (currentItem == nullptr || compareFn(element, currentItem) < 0)
- {
- // Insert new item before child.
+ // Add elements to the end of the list.
+ ARMARX_CHECK_NOT_NULL(makeItemFn);
item = makeItemFn(element);
- api::insertItem(parent, currentIndex, item);
+ api::insertItem(parent, api::getItemCount(parent), item);
++currentIndex;
inserted = true;
}
- else if (currentItem != nullptr && compareFn(element, currentItem) == 0)
+ else
{
- // Already existing.
- item = currentItem;
- ++currentIndex;
+ QTreeWidgetItem* currentItem = api::getItem(parent, currentIndex);
+ while (currentItem != nullptr && compareFn(element, currentItem) > 0)
+ {
+ delete api::takeItem(parent, currentIndex);
+ currentItem = api::getItem(parent, currentIndex);
+ }
+ if (currentItem == nullptr || compareFn(element, currentItem) < 0)
+ {
+ // Insert new item before child.
+ item = makeItemFn(element);
+ api::insertItem(parent, currentIndex, item);
+ ++currentIndex;
+ inserted = true;
+ }
+ else if (currentItem != nullptr && compareFn(element, currentItem) == 0)
+ {
+ // Already existing.
+ item = currentItem;
+ ++currentIndex;
+ }
}
- }
- ARMARX_CHECK_NOT_NULL(item);
- bool cont = updateItemFn(element, item);
- if (inserted && expand)
- {
- item->setExpanded(true);
- }
- return cont;
- });
+ ARMARX_CHECK_NOT_NULL(item);
+ bool cont = updateItemFn(element, item);
+ if (inserted && expand)
+ {
+ item->setExpanded(true);
+ }
+ return cont;
+ });
// Remove superfluous items. (currentIndex must point behind the last item)
while (api::getItemCount(parent) > currentIndex)
{
@@ -387,4 +433,4 @@ namespace armarx
ARMARX_CHECK_EQUAL(currentIndex, api::getItemCount(parent));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_gui/commit_widget/CommitWidget.cpp b/source/RobotAPI/libraries/armem_gui/commit_widget/CommitWidget.cpp
index e5c18ffe25ac193e535c7018482784194a4eb210..307cde12c7c5174c9343de8ffc14cb0e5f59849e 100644
--- a/source/RobotAPI/libraries/armem_gui/commit_widget/CommitWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/commit_widget/CommitWidget.cpp
@@ -1,17 +1,16 @@
#include "CommitWidget.h"
-#include <QWidget>
-#include <QLineEdit>
-#include <QTabWidget>
-#include <QPushButton>
#include <QCheckBox>
#include <QHBoxLayout>
-#include <QVBoxLayout>
+#include <QLineEdit>
+#include <QPushButton>
+#include <QTabWidget>
#include <QTextEdit>
+#include <QVBoxLayout>
+#include <QWidget>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::armem::gui
{
@@ -53,13 +52,15 @@ namespace armarx::armem::gui
setLayout(vlayout);
}
- std::string CommitWidget::getAronJSON() const
+ std::string
+ CommitWidget::getAronJSON() const
{
return _aronJSONInput->toPlainText().toStdString();
}
- std::string CommitWidget::getMemoryID() const
+ std::string
+ CommitWidget::getMemoryID() const
{
return _memoryID->text().toStdString();
}
-}
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/commit_widget/CommitWidget.h b/source/RobotAPI/libraries/armem_gui/commit_widget/CommitWidget.h
index 1795d4877e990bfc30a11be5d978859e115d1aa7..bb5c2a15d58688b23235ce5faf3d6d3c0a15db9a 100644
--- a/source/RobotAPI/libraries/armem_gui/commit_widget/CommitWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/commit_widget/CommitWidget.h
@@ -1,10 +1,10 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/query/DataMode.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-
#include <QWidget>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/query/DataMode.h>
+
class QLineEdit;
class QPushButton;
class QTextEdit;
@@ -18,7 +18,6 @@ namespace armarx::armem::gui
using This = CommitWidget;
public:
-
CommitWidget();
std::string getAronJSON() const;
@@ -35,12 +34,10 @@ namespace armarx::armem::gui
signals:
-
private:
-
QPushButton* _sendCommit;
QLineEdit* _memoryID;
QTextEdit* _aronJSONInput;
};
-}
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
index 0b7b737f03b2fd65f11f498f41c5e47c2e7dc5e9..45fe6096bb333f67a83ade705caaaab7dede27ba 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.cpp
@@ -180,7 +180,8 @@ namespace armarx::armem::gui::disk
{
if (std::filesystem::is_directory(p))
{
- armem::server::ltm::Memory ltm(p.parent_path().parent_path(), "MemoryExport", p.filename());
+ armem::server::ltm::Memory ltm(
+ p.parent_path().parent_path(), "MemoryExport", p.filename());
armem::wm::Memory memory =
ltm.loadAllAndResolve(); // load list of references and load data
memoryData[p] = std::move(memory);
diff --git a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
index 5eb35b96057f0639a30d94df4dada8ecd38afd9a..484ecb8be01df401c96435e45ac505eedfd2cfa7 100644
--- a/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/disk/ControlWidget.h
@@ -1,15 +1,17 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-#include <RobotAPI/libraries/armem/client/Query.h>
-
-#include <QWidget>
#include <QString>
+#include <QWidget>
+
+// Qt headers must come before <filesystem>
+// https://stackoverflow.com/questions/69407237/qt-moc-errorusr-include-c-10-bits-fs-fwd-39-parse-error-at-std
#include <filesystem>
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-class QPushButton;
+class QPushButton;
namespace armarx::armem::gui::disk
{
@@ -20,21 +22,17 @@ namespace armarx::armem::gui::disk
using This = ControlWidget;
public:
-
ControlWidget();
- void
- storeOnDisk(
- QString directory,
- const std::vector<wm::Memory> memoryData,
- std::string* outStatus = nullptr);
+ void storeOnDisk(QString directory,
+ const std::vector<wm::Memory> memoryData,
+ std::string* outStatus = nullptr);
std::map<std::filesystem::path, wm::Memory>
- loadFromDisk(
- QString directory,
- const armem::client::QueryInput& queryInput,
- std::string* outStatus = nullptr);
+ loadFromDisk(QString directory,
+ const armem::client::QueryInput& queryInput,
+ std::string* outStatus = nullptr);
signals:
@@ -52,12 +50,10 @@ namespace armarx::armem::gui::disk
private:
-
QPushButton* _loadFromDiskButton;
QPushButton* _storeOnDiskButton;
QString _latestDirectory;
-
};
-}
+} // namespace armarx::armem::gui::disk
diff --git a/source/RobotAPI/libraries/armem_gui/gui_utils.cpp b/source/RobotAPI/libraries/armem_gui/gui_utils.cpp
index 337ebc87d7506f9b4688e3ae3eabed5f4c63d389..1b7d13182caaf7796e30d3e5a95eb3de2574bde6 100644
--- a/source/RobotAPI/libraries/armem_gui/gui_utils.cpp
+++ b/source/RobotAPI/libraries/armem_gui/gui_utils.cpp
@@ -3,13 +3,13 @@
#include <QLayout>
#include <QLayoutItem>
#include <QSplitter>
-#include <QWidget>
#include <QTreeWidgetItem>
+#include <QWidget>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-void armarx::gui::clearLayout(QLayout* layout)
+void
+armarx::gui::clearLayout(QLayout* layout)
{
// Source: https://stackoverflow.com/a/4857631
@@ -31,7 +31,8 @@ void armarx::gui::clearLayout(QLayout* layout)
}
}
-void armarx::gui::clearItem(QTreeWidgetItem* item)
+void
+armarx::gui::clearItem(QTreeWidgetItem* item)
{
while (item->childCount() > 0)
{
@@ -39,8 +40,8 @@ void armarx::gui::clearItem(QTreeWidgetItem* item)
}
}
-
-QSplitter* armarx::gui::useSplitter(QLayout* layout)
+QSplitter*
+armarx::gui::useSplitter(QLayout* layout)
{
ARMARX_CHECK(layout);
@@ -48,7 +49,7 @@ QSplitter* armarx::gui::useSplitter(QLayout* layout)
for (int i = 0; i < layout->count(); ++i)
{
ARMARX_CHECK_NOT_NULL(layout->itemAt(i)->widget())
- << "QSplitter only supports widgets, but layout item #" << i << " is not a widget.";
+ << "QSplitter only supports widgets, but layout item #" << i << " is not a widget.";
}
QSplitter* splitter;
diff --git a/source/RobotAPI/libraries/armem_gui/gui_utils.h b/source/RobotAPI/libraries/armem_gui/gui_utils.h
index 1b7b810ad31455a5a1957dce3fb0a56eba21bfe5..0bdc2d60ef45373e48ee99d35ccaf2c7a70bc14e 100644
--- a/source/RobotAPI/libraries/armem_gui/gui_utils.h
+++ b/source/RobotAPI/libraries/armem_gui/gui_utils.h
@@ -9,7 +9,6 @@ class QLayout;
class QSplitter;
class QTreeWidgetItem;
-
namespace armarx::gui
{
/**
@@ -30,10 +29,9 @@ namespace armarx::gui
*/
void clearItem(QTreeWidgetItem* item);
-
-
template <class WidgetT>
- void replaceWidget(WidgetT*& old, QWidget* neu, QLayout* parentLayout)
+ void
+ replaceWidget(WidgetT*& old, QWidget* neu, QLayout* parentLayout)
{
QLayoutItem* oldItem = parentLayout->replaceWidget(old, neu);
if (oldItem)
@@ -71,4 +69,4 @@ namespace armarx::gui
int numDigits;
int base;
};
-}
+} // namespace armarx::gui
diff --git a/source/RobotAPI/libraries/armem_gui/instance/AronDataView.cpp b/source/RobotAPI/libraries/armem_gui/instance/AronDataView.cpp
index 47f270f724b16b52acbe5eb69613b1ae58756929..c660144f5a490beab1d662a3275b7275be9e1662 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/AronDataView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/AronDataView.cpp
@@ -1,4 +1,5 @@
#include "AronDataView.h"
+
namespace armarx::armem::gui::instance
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/DataView.h b/source/RobotAPI/libraries/armem_gui/instance/DataView.h
index 2b5fcd38166b13937fe3db5765840aa7f81d486e..5f8f2c8e2e3ef6abc240f7fcb28c0b6d3346b9c2 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/DataView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/DataView.h
@@ -23,14 +23,12 @@ class QSplitter;
class QTreeWidget;
class QTreeWidgetItem;
-
namespace armarx::armem::gui::instance
{
class ImageView;
class MemoryIDTreeWidgetItem;
class WidgetsWithToolbar;
-
class DataView : public QWidget, public armarx::Logging
{
Q_OBJECT
@@ -53,8 +51,10 @@ namespace armarx::armem::gui::instance
void updated();
void useTypeInfoChanged(bool enable);
void memoryIdResolutionRequested(const MemoryID& id);
- void actionsMenuRequested(const MemoryID& memoryID, QWidget* parent,
- const QPoint& pos, QMenu* menu);
+ void actionsMenuRequested(const MemoryID& memoryID,
+ QWidget* parent,
+ const QPoint& pos,
+ QMenu* menu);
protected slots:
@@ -101,7 +101,6 @@ namespace armarx::armem::gui::instance
QTreeWidget* tree;
QTreeWidgetItem* treeItemData;
-
class ImageView : public QGroupBox
{
public:
@@ -114,7 +113,6 @@ namespace armarx::armem::gui::instance
WidgetsWithToolbar* toolbar;
-
struct Limits
{
float min = std::numeric_limits<float>::max();
@@ -128,6 +126,7 @@ namespace armarx::armem::gui::instance
/// In this context, n.
const size_t limitsHistoryMaxSize;
};
+
ImageView* imageView = nullptr;
QLabel* statusLabel = nullptr;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp b/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
index 6d233680524bef1b515236fb03ec6e7e8e965ec0..4f66c77f461a9cb9bc49b3851df692a96e35af59 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/GroupBox.cpp
@@ -4,7 +4,6 @@
#include <QHBoxLayout>
#include <QVBoxLayout>
-
namespace armarx::armem::gui::instance
{
@@ -34,9 +33,10 @@ namespace armarx::armem::gui::instance
connect(useTypeInfoCheckBox, &QCheckBox::toggled, view, &InstanceView::setUseTypeInfo);
}
- void GroupBox::setStatusLabel(QLabel* statusLabel)
+ void
+ GroupBox::setStatusLabel(QLabel* statusLabel)
{
view->setStatusLabel(statusLabel);
}
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/GroupBox.h b/source/RobotAPI/libraries/armem_gui/instance/GroupBox.h
index af3c4d4405cadf82bbef1fe007e802a2f9d9ab9a..2cd62f836bc82ab9c11a678309b1835274947e99 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/GroupBox.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/GroupBox.h
@@ -8,7 +8,6 @@
class QCheckBox;
class QLabel;
-
namespace armarx::armem::gui::instance
{
@@ -18,7 +17,6 @@ namespace armarx::armem::gui::instance
using This = GroupBox;
public:
-
GroupBox();
void setStatusLabel(QLabel* statusLabel);
@@ -37,13 +35,11 @@ namespace armarx::armem::gui::instance
public:
-
InstanceView* view;
QCheckBox* useTypeInfoCheckBox;
-
};
-}
+} // namespace armarx::armem::gui::instance
namespace armarx::armem::gui
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/ImageView.cpp b/source/RobotAPI/libraries/armem_gui/instance/ImageView.cpp
index 8c75fbd87d715add23701374643ff22d37f281ec..2611205f9c5bc3061db1e5cdbee04d8ec58365a3 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/ImageView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/ImageView.cpp
@@ -26,7 +26,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::armem::gui::instance
{
@@ -37,26 +36,29 @@ namespace armarx::armem::gui::instance
connect(this, &This::sourceImageChanged, this, &This::updateImage);
}
- void ImageView::setImage(const QImage& image)
+ void
+ ImageView::setImage(const QImage& image)
{
sourceImage = image.copy();
emit sourceImageChanged();
}
- void ImageView::paintEvent(QPaintEvent* event)
+ void
+ ImageView::paintEvent(QPaintEvent* event)
{
- (void) event;
+ (void)event;
QPainter painter(this);
//scaledImage = image.scaled (width(), height(), Qt::KeepAspectRatio, Qt::FastTransformation);
- scaledImage = sourceImage.scaled(width(), height(), Qt::KeepAspectRatio, Qt::SmoothTransformation);
+ scaledImage =
+ sourceImage.scaled(width(), height(), Qt::KeepAspectRatio, Qt::SmoothTransformation);
painter.drawImage(0, 0, scaledImage);
}
- void ImageView::updateImage()
+ void
+ ImageView::updateImage()
{
update(0, 0, width(), height());
}
-}
-
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/ImageView.h b/source/RobotAPI/libraries/armem_gui/instance/ImageView.h
index ad3c6a0934e8cf7a9a09b17d1e2bd71b697008c3..8fead734e722fa3302f5630f05cb1cc35a456793 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/ImageView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/ImageView.h
@@ -21,9 +21,8 @@
#pragma once
-#include <QWidget>
#include <QImage>
-
+#include <QWidget>
namespace armarx::armem::gui::instance
{
@@ -55,15 +54,11 @@ namespace armarx::armem::gui::instance
protected:
-
void paintEvent(QPaintEvent* pPaintEvent) override;
private:
-
QImage sourceImage;
QImage scaledImage;
};
-}
-
-
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 1c7a40faed6b57b15bcd0a4ddd8677b103e8670d..f9c894800d2cd872f1119c858767245626d14dbb 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -1,4 +1,5 @@
#include "InstanceView.h"
+
#include <new>
#include <QAction>
@@ -18,23 +19,20 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
#include <RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h>
#include <RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h>
#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include "MemoryIDTreeWidgetItem.h"
#include "WidgetsWithToolbar.h"
-
namespace armarx::armem::gui::instance
{
@@ -59,7 +57,8 @@ namespace armarx::armem::gui::instance
treeItemMetadata->setExpanded(false);
}
- void InstanceView::update(const MemoryID& id, const wm::Memory& memory)
+ void
+ InstanceView::update(const MemoryID& id, const wm::Memory& memory)
{
aron::type::ObjectPtr aronType = nullptr;
const armem::wm::EntityInstance* instance = nullptr;
@@ -81,14 +80,16 @@ namespace armarx::armem::gui::instance
}
}
- void InstanceView::update(const wm::EntityInstance& instance, aron::type::ObjectPtr aronType)
+ void
+ InstanceView::update(const wm::EntityInstance& instance, aron::type::ObjectPtr aronType)
{
currentInstance = instance;
currentAronType = aronType;
update();
}
- void InstanceView::update()
+ void
+ InstanceView::update()
{
if (currentInstance)
{
@@ -101,30 +102,33 @@ namespace armarx::armem::gui::instance
}
}
- void InstanceView::updateInstanceID(const MemoryID& id)
+ void
+ InstanceView::updateInstanceID(const MemoryID& id)
{
treeItemInstanceID->setInstanceID(id, int(Columns::VALUE));
}
- void InstanceView::updateMetaData(const wm::EntityInstanceMetadata& metadata)
+ void
+ InstanceView::updateMetaData(const wm::EntityInstanceMetadata& metadata)
{
- std::vector<std::string> items =
- {
- std::to_string(metadata.confidence),
- armem::toDateTimeMilliSeconds(metadata.referencedTime),
- armem::toDateTimeMilliSeconds(metadata.sentTime),
- armem::toDateTimeMilliSeconds(metadata.arrivedTime),
- armem::toDateTimeMilliSeconds(metadata.lastAccessedTime) + " (" + std::to_string(metadata.numAccessed) + " times total)"
- };
+ std::vector<std::string> items = {std::to_string(metadata.confidence),
+ armem::toDateTimeMilliSeconds(metadata.referencedTime),
+ armem::toDateTimeMilliSeconds(metadata.sentTime),
+ armem::toDateTimeMilliSeconds(metadata.arrivedTime),
+ armem::toDateTimeMilliSeconds(metadata.lastAccessedTime) +
+ " (" + std::to_string(metadata.numAccessed) +
+ " times total)"};
ARMARX_CHECK_EQUAL(static_cast<size_t>(treeItemMetadata->childCount()), items.size());
int i = 0;
for (const std::string& item : items)
{
- treeItemMetadata->child(i++)->setText(int(Columns::VALUE), QString::fromStdString(item));
+ treeItemMetadata->child(i++)->setText(int(Columns::VALUE),
+ QString::fromStdString(item));
}
}
- QMenu* InstanceView::buildActionsMenu(const QPoint& pos)
+ QMenu*
+ InstanceView::buildActionsMenu(const QPoint& pos)
{
auto* parentMenu = DataView::buildActionsMenu(pos);
@@ -140,18 +144,22 @@ namespace armarx::armem::gui::instance
return parentMenu;
}
- void InstanceView::prepareTreeContextMenu(const QPoint& pos)
+ void
+ InstanceView::prepareTreeContextMenu(const QPoint& pos)
{
if (currentInstance.has_value())
{
auto* menu = buildActionsMenu(pos);
- emit actionsMenuRequested(currentInstance->id(), this, tree->mapToGlobal(pos),
+ emit actionsMenuRequested(currentInstance->id(),
+ this,
+ tree->mapToGlobal(pos),
menu->actions().isEmpty() ? nullptr : menu);
}
}
- aron::data::DictPtr InstanceView::getData()
+ aron::data::DictPtr
+ InstanceView::getData()
{
if (currentInstance)
{
@@ -161,4 +169,3 @@ namespace armarx::armem::gui::instance
}
} // namespace armarx::armem::gui::instance
-
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index 34e2a0fa991d2f2e3fb3f417ca6acb3074c61eeb..745825c6fdcdf55459556d0937de924c7c58b9da 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -9,7 +9,6 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_gui/instance/DataView.h>
-
namespace armarx::armem::gui::instance
{
class MemoryIDTreeWidgetItem;
@@ -21,7 +20,6 @@ namespace armarx::armem::gui::instance
public:
-
InstanceView();
void update(const MemoryID& id, const wm::Memory& memory);
@@ -45,15 +43,13 @@ namespace armarx::armem::gui::instance
void updateMetaData(const wm::EntityInstanceMetadata& metadata);
private:
-
std::optional<wm::EntityInstance> currentInstance;
MemoryIDTreeWidgetItem* treeItemInstanceID;
QTreeWidgetItem* treeItemMetadata;
-
};
-}
+} // namespace armarx::armem::gui::instance
namespace armarx::armem::gui
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.cpp
index 0b0c0cec792666b1bee40d72ceb00951bb4a3bf4..da3c9bc29a294307aac12093ae48ca2677944814 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.cpp
@@ -15,9 +15,8 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h>
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
-
+#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h>
namespace armarx::armem::gui::instance
{
@@ -26,16 +25,18 @@ namespace armarx::armem::gui::instance
{
}
- void InstanceViewList::setStatusLabel(QLabel* statusLabel)
+ void
+ InstanceViewList::setStatusLabel(QLabel* statusLabel)
{
this->statusLabel = statusLabel;
}
- void InstanceViewList::setUseTypeInfo(bool enable)
+ void
+ InstanceViewList::setUseTypeInfo(bool enable)
{
this->useTypeInfo = enable;
update();
}
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
index 8dc24e0dcd9759f3ab89e21e33ff600ad253f082..1258961378eb10f99d85f298138450b15bbcc7c7 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceViewList.h
@@ -4,9 +4,8 @@
#include <QWidget>
-#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h>
-
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/aroncore/navigators/typenavigator/AronObjectTypeNavigator.h>
class QGroupBox;
@@ -15,7 +14,6 @@ class QSplitter;
class QTreeWidget;
class QTreeWidgetItem;
-
namespace armarx::armem::gui::instance
{
namespace instance
@@ -23,7 +21,6 @@ namespace armarx::armem::gui::instance
class ImageView;
}
-
class InstanceViewList : public QWidget
{
Q_OBJECT
@@ -31,14 +28,12 @@ namespace armarx::armem::gui::instance
public:
-
InstanceViewList();
void setStatusLabel(QLabel* statusLabel);
void setUseTypeInfo(bool enable);
-
signals:
void updated();
@@ -48,20 +43,14 @@ namespace armarx::armem::gui::instance
private:
-
-
-
private:
-
-
QSplitter* splitter;
QLabel* statusLabel = nullptr;
bool useTypeInfo = true;
-
};
-}
+} // namespace armarx::armem::gui::instance
namespace armarx::armem::gui
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
index 5cde16500c5e1dbd05f9f376ccf84f3bd2504f39..3a925549f1802cab08d05a1f03b2b2e9d0af4745 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.cpp
@@ -4,11 +4,11 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
namespace armarx::armem::gui::instance
{
- void MemoryIDTreeWidgetItem::addKeyChildren()
+ void
+ MemoryIDTreeWidgetItem::addKeyChildren()
{
addChild(new QTreeWidgetItem({"Memory Name"}));
addChild(new QTreeWidgetItem({"Core Segment Name"}));
@@ -18,8 +18,8 @@ namespace armarx::armem::gui::instance
addChild(new QTreeWidgetItem({"Instance Index"}));
}
-
- void MemoryIDTreeWidgetItem::setInstanceID(const MemoryID& id, int valueColumn)
+ void
+ MemoryIDTreeWidgetItem::setInstanceID(const MemoryID& id, int valueColumn)
{
setText(valueColumn, QString::fromStdString(id.str()));
@@ -35,11 +35,10 @@ namespace armarx::armem::gui::instance
{
static const char* mu = "\u03BC";
std::stringstream ss;
- ss << toDateTimeMilliSeconds(id.timestamp)
- << " (" << id.timestamp.toMicroSecondsSinceEpoch() << " " << mu << "s)";
+ ss << toDateTimeMilliSeconds(id.timestamp) << " ("
+ << id.timestamp.toMicroSecondsSinceEpoch() << " " << mu << "s)";
child(4)->setText(valueColumn, QString::fromStdString(ss.str()));
}
}
-}
-
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h
index a011a59aff48b8725e42aac1c7eab71ac5ca8676..f0ad203cdd7cd451b7f30f8f20e7548e29a39530 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/MemoryIDTreeWidgetItem.h
@@ -2,7 +2,6 @@
#include <QTreeWidgetItem>
-
namespace armarx::armem
{
class MemoryID;
@@ -14,14 +13,10 @@ namespace armarx::armem::gui::instance
class MemoryIDTreeWidgetItem : public QTreeWidgetItem
{
public:
-
using QTreeWidgetItem::QTreeWidgetItem;
void addKeyChildren();
void setInstanceID(const MemoryID& id, int valueColumn = 1);
-
-
};
-}
-
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.cpp b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.cpp
index b41ce7649aa21303658ed3a400f691e5efca1d0e..d2edc53ff43221e5bdc08991e6be3fe08df804e9 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.cpp
@@ -32,7 +32,6 @@
#include "InstanceView.h"
-
namespace armarx::armem::gui::instance
{
@@ -51,23 +50,22 @@ namespace armarx::armem::gui::instance
toolbar->setOrientation(Qt::Orientation::Vertical);
toolbar->setContentsMargins(margin, margin, margin, margin);
- QAction* action = toolbar->addAction(QIcon(":/icons/dialog-close.ico"), "Close", [this]()
- {
- this->close();
- });
+ QAction* action = toolbar->addAction(
+ QIcon(":/icons/dialog-close.ico"), "Close", [this]() { this->close(); });
action->setToolTip("Remove this instance view");
_layout->addWidget(toolbar);
}
-
- void WidgetsWithToolbar::addWidget(QWidget* widget)
+ void
+ WidgetsWithToolbar::addWidget(QWidget* widget)
{
ARMARX_CHECK_GREATER_EQUAL(_layout->count(), 1);
_layout->insertWidget(_layout->count() - 1, widget);
}
- void WidgetsWithToolbar::close()
+ void
+ WidgetsWithToolbar::close()
{
// ARMARX_IMPORTANT << "Closing instance view ...";
emit closing();
@@ -76,5 +74,4 @@ namespace armarx::armem::gui::instance
this->deleteLater();
}
-}
-
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.h b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.h
index 8d2f2a00aa0d2ee74d47f95ce88e49b85b3392e4..974c895e0cbd18701a8509856cf700cafe37fb37 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/WidgetsWithToolbar.h
@@ -28,7 +28,6 @@ class QHBoxLayout;
class QVBoxLayout;
class QToolBar;
-
namespace armarx::armem::gui::instance
{
class WidgetsWithToolbar : public QWidget
@@ -37,7 +36,6 @@ namespace armarx::armem::gui::instance
using This = WidgetsWithToolbar;
public:
-
WidgetsWithToolbar(QWidget* parent = nullptr);
void addWidget(QWidget* widget);
@@ -59,18 +57,11 @@ namespace armarx::armem::gui::instance
protected:
-
-
public:
-
QToolBar* toolbar;
private:
-
QHBoxLayout* _layout;
-
};
-}
-
-
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
index bc941f8aac4ccc2d8440eec70c1014b3f713c7bc..e9a3106007bca235d23c6aaa890f3870815450fb 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.cpp
@@ -7,13 +7,14 @@
#include <RobotAPI/libraries/armem/aron/MemoryID.aron.generated.h>
-const std::string armarx::armem::gui::instance::rawMemoryIDTypeName = armarx::armem::arondto::MemoryID::ToAronType()->getFullName();
+const std::string armarx::armem::gui::instance::rawMemoryIDTypeName =
+ armarx::armem::arondto::MemoryID::ToAronType()->getFullName();
const std::string armarx::armem::gui::instance::sanitizedMemoryIDTypeName = "MemoryID";
-
namespace
{
- std::string remove_wrap(const std::string& string, const std::string& prefix, const std::string& suffix)
+ std::string
+ remove_wrap(const std::string& string, const std::string& prefix, const std::string& suffix)
{
if (simox::alg::starts_with(string, prefix) && simox::alg::ends_with(string, suffix))
{
@@ -24,9 +25,10 @@ namespace
return string;
}
}
-}
+} // namespace
-std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& typeName)
+std::string
+armarx::armem::gui::instance::sanitizeTypeName(const std::string& typeName)
{
if (typeName == rawMemoryIDTypeName)
{
@@ -45,7 +47,7 @@ std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& ty
size_t find = n.rfind(del);
if (find != n.npos)
{
- find += del.size(); // include del
+ find += del.size(); // include del
std::stringstream ss;
ss << n.substr(find) << " (" << n.substr(0, find - del.size()) << ")";
n = ss.str();
@@ -58,10 +60,9 @@ std::string armarx::armem::gui::instance::sanitizeTypeName(const std::string& ty
std::string container = n.substr(0, n.find("<"));
std::string subtype = remove_wrap(n, (container + "<"), ">");
subtype = sanitizeTypeName(subtype);
- n = n.substr(0, n.find("<")+1) + subtype + ">";
+ n = n.substr(0, n.find("<") + 1) + subtype + ">";
}
}
return n;
-
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
index 80bc8fe789e5d2f6a442632f3c88b93020ddbb02..114639f448bb85a75c53a3cc7d61ceaa71c6ed02 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/sanitize_typename.h
@@ -2,7 +2,6 @@
#include <string>
-
namespace armarx::armem::gui::instance
{
@@ -11,5 +10,4 @@ namespace armarx::armem::gui::instance
std::string sanitizeTypeName(const std::string& typeName);
-}
-
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp
index 3141eb165543ba9c8df2e8d3754f14954aa15181..99c586eb8e69f289251cc071c078e2f7436e1aef 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.cpp
@@ -1,16 +1,16 @@
#include "serialize_path.h"
-#include <RobotAPI/libraries/aron/core/Path.h>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <QString>
+#include <QStringList>
#include <SimoxUtility/algorithm/string.h>
-#include <QString>
-#include <QStringList>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/aron/core/Path.h>
-QStringList armarx::armem::gui::instance::serializePath(const aron::Path& path)
+QStringList
+armarx::armem::gui::instance::serializePath(const aron::Path& path)
{
QStringList qpath;
qpath.append(QString::fromStdString(path.getRootIdentifier()));
@@ -22,7 +22,8 @@ QStringList armarx::armem::gui::instance::serializePath(const aron::Path& path)
return qpath;
}
-armarx::aron::Path armarx::armem::gui::instance::deserializePath(const QStringList& qpath)
+armarx::aron::Path
+armarx::armem::gui::instance::deserializePath(const QStringList& qpath)
{
ARMARX_CHECK_GREATER_EQUAL(qpath.size(), 2);
std::vector<std::string> pathItems;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h
index 26517c6d11e3ee25071bb6f5e01da3996c6553cf..fe025bf22829974dda042617add613cb3d9001f4 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/serialize_path.h
@@ -1,18 +1,15 @@
#pragma once
-
namespace armarx::aron
{
class Path;
}
class QStringList;
-
namespace armarx::armem::gui::instance
{
QStringList serializePath(const aron::Path& path);
aron::Path deserializePath(const QStringList& qpath);
-}
-
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
index 27cb691cf870d1663305cb25a60f2541dc29e7c6..fbe068205162805cf91797d6d0386c489a46db3b 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.cpp
@@ -4,7 +4,6 @@
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
-
namespace armarx::armem::gui::instance
{
@@ -12,34 +11,37 @@ namespace armarx::armem::gui::instance
{
}
- void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::data::DictPtr& data)
+ void
+ DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::data::DictPtr& data)
{
DictBuilder builder = getDictBuilder();
- builder.setUpdateItemFn([this, &data](const std::string & key, QTreeWidgetItem * item)
- {
- auto child = data->getElement(key);
- this->update(item, key, child, data->getPath());
- return true;
- });
+ builder.setUpdateItemFn(
+ [this, &data](const std::string& key, QTreeWidgetItem* item)
+ {
+ auto child = data->getElement(key);
+ this->update(item, key, child, data->getPath());
+ return true;
+ });
builder.updateTreeWithContainer(parent, data->getAllKeys());
}
- void DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::data::ListPtr& data)
+ void
+ DataTreeBuilder::updateTree(QTreeWidgetItem* parent, const aron::data::ListPtr& data)
{
ListBuilder builder = getListBuilder();
- builder.setUpdateItemFn([this, &data](size_t key, QTreeWidgetItem * item)
- {
- auto child = data->getElement(key);
- this->update(item, std::to_string(key), child, data->getPath());
- return true;
- });
+ builder.setUpdateItemFn(
+ [this, &data](size_t key, QTreeWidgetItem* item)
+ {
+ auto child = data->getElement(key);
+ this->update(item, std::to_string(key), child, data->getPath());
+ return true;
+ });
builder.updateTreeWithContainer(parent, getIndex(data->childrenSize()));
}
-
void
DataTreeBuilder::update(QTreeWidgetItem* item,
const std::string& key,
@@ -66,6 +68,5 @@ namespace armarx::armem::gui::instance
auto empty = std::make_shared<aron::data::Dict>(parentPath.withElement(key));
updateTree(item, empty);
}
-
}
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
index 80773c86aa7074b88b962fa2ae44c7b4f54e65f1..d3ba26ffb28eee7c39a1a93abf6ae3d1ec79ed9a 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilder.h
@@ -7,14 +7,12 @@
#include "DataTreeBuilderBase.h"
-
namespace armarx::armem::gui::instance
{
class DataTreeBuilder : public DataTreeBuilderBase
{
public:
-
DataTreeBuilder();
void updateTree(QTreeWidgetItem* parent, const aron::data::DictPtr& data);
@@ -27,4 +25,4 @@ namespace armarx::armem::gui::instance
const aron::data::VariantPtr& data,
const aron::Path& parentPath);
};
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
index 1f0d140918dc615251e67629df69e3c2b5d53ff0..1812e355e5322e0d1b51957f19ced74370f872f2 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.cpp
@@ -6,53 +6,59 @@
// #include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
-#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/armem_gui/instance/display_visitors/DataDisplayVisitor.h>
-
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
namespace armarx::armem::gui::instance
{
const int keyIndexRole = Qt::UserRole + 10;
-
DataTreeBuilderBase::DataTreeBuilderBase()
{
}
DataTreeBuilderBase::~DataTreeBuilderBase()
- {}
+ {
+ }
- void DataTreeBuilderBase::setColumns(int key, int value, int type)
+ void
+ DataTreeBuilderBase::setColumns(int key, int value, int type)
{
this->columnKey = key;
this->columnType = type;
this->columnValue = value;
}
-
- QTreeWidgetItem* DataTreeBuilderBase::makeItem(const std::string& key) const
+ QTreeWidgetItem*
+ DataTreeBuilderBase::makeItem(const std::string& key) const
{
return new QTreeWidgetItem(QStringList{QString::fromStdString(key)});
}
- QTreeWidgetItem* DataTreeBuilderBase::makeItem(size_t key) const
+ QTreeWidgetItem*
+ DataTreeBuilderBase::makeItem(size_t key) const
{
QTreeWidgetItem* item = new QTreeWidgetItem();
item->setData(0, keyIndexRole, static_cast<int>(key));
return item;
}
- void DataTreeBuilderBase::setRowTexts(QTreeWidgetItem* item, const std::string& key, const std::string& value, const std::string& typeName) const
+ void
+ DataTreeBuilderBase::setRowTexts(QTreeWidgetItem* item,
+ const std::string& key,
+ const std::string& value,
+ const std::string& typeName) const
{
item->setText(columnKey, QString::fromStdString(key));
item->setText(columnValue, QString::fromStdString(value));
item->setText(columnType, QString::fromStdString(typeName));
}
-
- void DataTreeBuilderBase::setRowTexts(
- QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data)
+ void
+ DataTreeBuilderBase::setRowTexts(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::data::VariantPtr& data)
{
if (!data)
{
@@ -62,41 +68,35 @@ namespace armarx::armem::gui::instance
setRowTexts(item, key, value, sanitizeTypeName(data->getFullName()));
}
- DataTreeBuilderBase::DictBuilder DataTreeBuilderBase::getDictBuilder() const
+ DataTreeBuilderBase::DictBuilder
+ DataTreeBuilderBase::getDictBuilder() const
{
DictBuilder builder;
- builder.setCompareFn([](const std::string & key, QTreeWidgetItem * item)
- {
- return armarx::detail::compare(key, item->text(0).toStdString());
- });
- builder.setMakeItemFn([this](const std::string & key)
- {
- return this->makeItem(key);
- });
+ builder.setCompareFn([](const std::string& key, QTreeWidgetItem* item)
+ { return armarx::detail::compare(key, item->text(0).toStdString()); });
+ builder.setMakeItemFn([this](const std::string& key) { return this->makeItem(key); });
return builder;
}
-
- DataTreeBuilderBase::ListBuilder DataTreeBuilderBase::getListBuilder() const
+ DataTreeBuilderBase::ListBuilder
+ DataTreeBuilderBase::getListBuilder() const
{
ListBuilder builder;
- builder.setCompareFn([](size_t key, QTreeWidgetItem * item)
- {
- QVariant itemKey = item->data(0, keyIndexRole);
- ARMARX_CHECK_EQUAL(itemKey.type(), QVariant::Type::Int);
- // << VAROUT(key) << " | " << VAROUT(item->text(0).toStdString()) << itemKey.typeName();
-
- return armarx::detail::compare(static_cast<int>(key), itemKey.toInt());
- });
- builder.setMakeItemFn([this](size_t key)
- {
- return this->makeItem(key);
- });
+ builder.setCompareFn(
+ [](size_t key, QTreeWidgetItem* item)
+ {
+ QVariant itemKey = item->data(0, keyIndexRole);
+ ARMARX_CHECK_EQUAL(itemKey.type(), QVariant::Type::Int);
+ // << VAROUT(key) << " | " << VAROUT(item->text(0).toStdString()) << itemKey.typeName();
+
+ return armarx::detail::compare(static_cast<int>(key), itemKey.toInt());
+ });
+ builder.setMakeItemFn([this](size_t key) { return this->makeItem(key); });
return builder;
}
-
- std::vector<size_t> DataTreeBuilderBase::getIndex(size_t size) const
+ std::vector<size_t>
+ DataTreeBuilderBase::getIndex(size_t size) const
{
std::vector<size_t> index;
index.reserve(size);
@@ -107,4 +107,4 @@ namespace armarx::armem::gui::instance
return index;
}
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
index 2988b5e9cf55965381eb1e4642728a201cd52e7d..bae2894a7dd5d2dd57952df0719d71662d6b346d 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/DataTreeBuilderBase.h
@@ -4,22 +4,20 @@
#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
-
namespace armarx
{
- template <class ContainerT> struct TreeWidgetBuilder;
+ template <class ContainerT>
+ struct TreeWidgetBuilder;
}
class QTreeWidgetItem;
-
namespace armarx::armem::gui::instance
{
class DataTreeBuilderBase
{
public:
-
DataTreeBuilderBase();
virtual ~DataTreeBuilderBase();
@@ -27,7 +25,6 @@ namespace armarx::armem::gui::instance
protected:
-
using DictBuilder = armarx::TreeWidgetBuilder<std::string>;
using ListBuilder = armarx::TreeWidgetBuilder<size_t>;
@@ -39,16 +36,19 @@ namespace armarx::armem::gui::instance
QTreeWidgetItem* makeItem(const std::string& key) const;
QTreeWidgetItem* makeItem(size_t key) const;
- void setRowTexts(QTreeWidgetItem* item, const std::string& key, const std::string& value, const std::string& typeName = "") const;
- void setRowTexts(QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data);
+ void setRowTexts(QTreeWidgetItem* item,
+ const std::string& key,
+ const std::string& value,
+ const std::string& typeName = "") const;
+ void setRowTexts(QTreeWidgetItem* item,
+ const std::string& key,
+ const aron::data::VariantPtr& data);
public:
-
int columnKey = 0;
int columnValue = 1;
int columnType = 2;
-
};
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
index 750eae39bc7020962e2a0cb275e38bc29faae2eb..06719303621729bf48a2844e0e9534036ae48c32 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_builders/TypedDataTreeBuilder.h
@@ -5,7 +5,6 @@
#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/List.h>
-
#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/List.h>
@@ -18,14 +17,12 @@
class QStringList;
-
namespace armarx::armem::gui::instance
{
class TypedDataTreeBuilder : public DataTreeBuilderBase
{
public:
-
TypedDataTreeBuilder();
@@ -51,25 +48,22 @@ namespace armarx::armem::gui::instance
protected:
-
void updateDispatch(QTreeWidgetItem* item,
- const std::string& key,
- const aron::type::VariantPtr& type,
- const aron::data::VariantPtr& data);
+ const std::string& key,
+ const aron::type::VariantPtr& type,
+ const aron::data::VariantPtr& data);
void update(QTreeWidgetItem* item,
const std::string& key,
const aron::type::VariantPtr& type,
const aron::data::VariantPtr& data);
- void update(QTreeWidgetItem* item,
- const std::string& key,
- const aron::data::VariantPtr& data);
+ void
+ update(QTreeWidgetItem* item, const std::string& key, const aron::data::VariantPtr& data);
template <class DataT, class TypeT>
void _updateTree(QTreeWidgetItem* item, TypeT& type, DataT& data);
-
};
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
index 8bc5889d29298e41dbb27cfa6114f9841c7d3f98..5afa2d2c512af424a24336cbace032275b1e0138 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.cpp
@@ -2,7 +2,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::armem::gui::instance
{
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
index eb8d4004ab2d4e5bb430995b5b9ba27f52da8e6e..4b66ce7ddd87dbd2e09b17286876ef69853d265a 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitor.h
@@ -3,12 +3,11 @@
#include <sstream>
#include <stack>
-#include <QTreeWidget>
#include <QLabel>
+#include <QTreeWidget>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
-
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
namespace armarx::armem::gui::instance
{
@@ -18,76 +17,90 @@ namespace armarx::armem::gui::instance
public TreeDataVisitorBase
{
public:
-
using TreeDataVisitorBase::TreeDataVisitorBase;
-
- void visitDictOnEnter(const aron::data::VariantPtr& n) override
+ void
+ visitDictOnEnter(const aron::data::VariantPtr& n) override
{
const std::string key = n->getPath().getLastElement();
_visitEnter(key, "Dict", n->childrenSize());
}
- void visitDictOnExit(const aron::data::VariantPtr&) override
+
+ void
+ visitDictOnExit(const aron::data::VariantPtr&) override
{
_visitExit();
}
- void visitListOnEnter(const aron::data::VariantPtr& n) override
+ void
+ visitListOnEnter(const aron::data::VariantPtr& n) override
{
const std::string key = n->getPath().getLastElement();
_visitEnter(key, "List", n->childrenSize());
}
- void visitListOnExit(const aron::data::VariantPtr&) override
+
+ void
+ visitListOnExit(const aron::data::VariantPtr&) override
{
_visitExit();
}
-
- void visitBool(const aron::data::VariantPtr& b) override
+ void
+ visitBool(const aron::data::VariantPtr& b) override
{
const auto x = *aron::data::Bool::DynamicCastAndCheck(b);
const std::string key = b->getPath().getLastElement();
this->addValueRow(key, x, "Bool");
}
- void visitDouble(const aron::data::VariantPtr& d) override
+
+ void
+ visitDouble(const aron::data::VariantPtr& d) override
{
const auto x = *aron::data::Double::DynamicCastAndCheck(d);
const std::string key = d->getPath().getLastElement();
this->addValueRow(key, x, "Double");
}
- void visitFloat(const aron::data::VariantPtr& f) override
+
+ void
+ visitFloat(const aron::data::VariantPtr& f) override
{
const auto x = *aron::data::Float::DynamicCastAndCheck(f);
const std::string key = f->getPath().getLastElement();
this->addValueRow(key, x, "Float");
}
- void visitInt(const aron::data::VariantPtr& i) override
+
+ void
+ visitInt(const aron::data::VariantPtr& i) override
{
const auto x = *aron::data::Int::DynamicCastAndCheck(i);
const std::string key = i->getPath().getLastElement();
this->addValueRow(key, x, "Int");
}
- void visitLong(const aron::data::VariantPtr& l) override
+
+ void
+ visitLong(const aron::data::VariantPtr& l) override
{
const auto x = *aron::data::Long::DynamicCastAndCheck(l);
const std::string key = l->getPath().getLastElement();
this->addValueRow(key, x, "Long");
}
- void visitString(const aron::data::VariantPtr& string) override
+
+ void
+ visitString(const aron::data::VariantPtr& string) override
{
const auto x = *aron::data::String::DynamicCastAndCheck(string);
const std::string key = string->getPath().getLastElement();
this->addValueRow(key, x, "String");
}
- void visitNDArray(const aron::data::VariantPtr& array) override
+ void
+ visitNDArray(const aron::data::VariantPtr& array) override
{
const auto x = *aron::data::NDArray::DynamicCastAndCheck(array);
const std::string key = array->getPath().getLastElement();
this->addValueRow(key, x, "ND Array");
}
-
};
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
index 775e95d2bb867ab88f58767f8cda8a68d15b4633..aa78db483afc8e99b88a15ecd8c888b4a2e36760 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.cpp
@@ -1,8 +1,8 @@
#include "TreeDataVisitorBase.h"
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
namespace armarx::armem::gui::instance
{
@@ -17,16 +17,21 @@ namespace armarx::armem::gui::instance
}
TreeDataVisitorBase::~TreeDataVisitorBase()
- {}
+ {
+ }
- void TreeDataVisitorBase::setColumns(int key, int value, int type)
+ void
+ TreeDataVisitorBase::setColumns(int key, int value, int type)
{
this->columnKey = key;
this->columnType = type;
this->columnValue = value;
}
- bool TreeDataVisitorBase::_visitEnter(const std::string& key, const std::string& type, size_t numChildren)
+ bool
+ TreeDataVisitorBase::_visitEnter(const std::string& key,
+ const std::string& type,
+ size_t numChildren)
{
QTreeWidgetItem* item = nullptr;
if (rootItems.size() > 0)
@@ -46,7 +51,8 @@ namespace armarx::armem::gui::instance
return true;
}
- bool TreeDataVisitorBase::_visitExit()
+ bool
+ TreeDataVisitorBase::_visitExit()
{
ARMARX_CHECK_POSITIVE(items.size());
QTreeWidgetItem* item = items.top();
@@ -63,7 +69,8 @@ namespace armarx::armem::gui::instance
return true;
}
- void TreeDataVisitorBase::streamValueText(const aron::data::Bool& n, std::stringstream& ss) const
+ void
+ TreeDataVisitorBase::streamValueText(const aron::data::Bool& n, std::stringstream& ss) const
{
if (n.getValue())
{
@@ -75,14 +82,16 @@ namespace armarx::armem::gui::instance
}
}
- void TreeDataVisitorBase::streamValueText(const aron::data::String& n, std::stringstream& ss) const
+ void
+ TreeDataVisitorBase::streamValueText(const aron::data::String& n, std::stringstream& ss) const
{
ss << "'" << n.getValue() << "'";
}
- void TreeDataVisitorBase::streamValueText(const aron::data::NDArray& n, std::stringstream& ss) const
+ void
+ TreeDataVisitorBase::streamValueText(const aron::data::NDArray& n, std::stringstream& ss) const
{
ss << "shape " << aron::data::NDArray::DimensionsToString(n.getShape());
}
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
index 7250ce376380ec0967989d369a0031ecf210e537..ce8443e13302bf04c20f3301015b04fbcc4e2f1f 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h
@@ -3,19 +3,17 @@
#include <sstream>
#include <stack>
-#include <QTreeWidget>
#include <QLabel>
+#include <QTreeWidget>
#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
-
namespace armarx::armem::gui::instance
{
class TreeDataVisitorBase
{
public:
-
TreeDataVisitorBase();
TreeDataVisitorBase(QTreeWidgetItem* root);
virtual ~TreeDataVisitorBase();
@@ -24,23 +22,26 @@ namespace armarx::armem::gui::instance
protected:
-
// Same for Dict and List
bool _visitEnter(const std::string& key, const std::string& type, size_t numChildren);
bool _visitExit();
-
template <class Navigator>
- bool addValueRow(const std::string& key, Navigator& n, const std::string& typeName)
+ bool
+ addValueRow(const std::string& key, Navigator& n, const std::string& typeName)
{
if (items.size() > 0)
{
- items.top()->addChild(new QTreeWidgetItem(this->makeValueRowStrings(key, n, typeName)));
+ items.top()->addChild(
+ new QTreeWidgetItem(this->makeValueRowStrings(key, n, typeName)));
}
return true;
}
- QStringList makeValueRowStrings(const std::string& key, const std::string& value, const std::string& typeName) const
+ QStringList
+ makeValueRowStrings(const std::string& key,
+ const std::string& value,
+ const std::string& typeName) const
{
QStringList cols;
cols.insert(columnKey, QString::fromStdString(key));
@@ -50,7 +51,8 @@ namespace armarx::armem::gui::instance
}
template <class Navigator>
- QStringList makeValueRowStrings(const std::string& key, Navigator& n, const std::string& typeName) const
+ QStringList
+ makeValueRowStrings(const std::string& key, Navigator& n, const std::string& typeName) const
{
std::stringstream value;
streamValueText(n, value);
@@ -58,24 +60,24 @@ namespace armarx::armem::gui::instance
}
template <class Navigator>
- void streamValueText(Navigator& n, std::stringstream& ss) const
+ void
+ streamValueText(Navigator& n, std::stringstream& ss) const
{
ss << n.getValue();
}
+
void streamValueText(const aron::data::Bool& n, std::stringstream& ss) const;
void streamValueText(const aron::data::String& n, std::stringstream& ss) const;
void streamValueText(const aron::data::NDArray& n, std::stringstream& ss) const;
public:
-
std::stack<QTreeWidgetItem*> rootItems;
std::stack<QTreeWidgetItem*> items;
int columnKey = 0;
int columnValue = 1;
int columnType = 2;
-
};
-}
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
index 8ee94fd6c233047313161fb093ad7784ca7f603d..deed1020edb0c85b68fe5f9defba442d1032bfc6 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedDataVisitor.h
@@ -3,19 +3,16 @@
#include <sstream>
#include <stack>
-#include <QTreeWidget>
#include <QLabel>
+#include <QTreeWidget>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/armem_gui/instance/serialize_path.h>
-
#include <RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeDataVisitorBase.h>
-
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
namespace armarx::armem::gui::instance
{
@@ -25,61 +22,77 @@ namespace armarx::armem::gui::instance
public TreeDataVisitorBase
{
public:
-
using TreeDataVisitorBase::TreeDataVisitorBase;
-
- void visitDictOnEnter(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+ void
+ visitDictOnEnter(const aron::data::VariantPtr& data,
+ const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
const std::string key = data->getPath().getLastElement();
_visitEnter(key, sanitizeTypeName(type->getFullName()), data->childrenSize());
}
- void visitDictOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
+
+ void
+ visitDictOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
{
_visitExit();
}
- void visitObjectOnEnter(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+ void
+ visitObjectOnEnter(const aron::data::VariantPtr& data,
+ const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
const std::string key = data->getPath().getLastElement();
_visitEnter(key, sanitizeTypeName(type->getFullName()), data->childrenSize());
}
- void visitObjectOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
+
+ void
+ visitObjectOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
{
_visitExit();
}
- void visitListOnEnter(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+ void
+ visitListOnEnter(const aron::data::VariantPtr& data,
+ const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
const std::string key = data->getPath().getLastElement();
_visitEnter(key, sanitizeTypeName(type->getFullName()), data->childrenSize());
}
- void visitListOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
+
+ void
+ visitListOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
{
_visitExit();
}
- void visitTupleOnEnter(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+ void
+ visitTupleOnEnter(const aron::data::VariantPtr& data,
+ const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
const std::string key = data->getPath().getLastElement();
_visitEnter(key, sanitizeTypeName(type->getFullName()), data->childrenSize());
}
- void visitTupleOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
+
+ void
+ visitTupleOnExit(const aron::data::VariantPtr&, const aron::type::VariantPtr&) override
{
_visitExit();
}
+
// What about Pair??
- void visitBool(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+ void
+ visitBool(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
@@ -89,7 +102,9 @@ namespace armarx::armem::gui::instance
auto t = *aron::type::Bool::DynamicCastAndCheck(type);
this->addValueRow(key, d, t);
}
- void visitDouble(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+
+ void
+ visitDouble(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
@@ -99,7 +114,9 @@ namespace armarx::armem::gui::instance
auto t = *aron::type::Double::DynamicCastAndCheck(type);
this->addValueRow(key, d, t);
}
- void visitFloat(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+
+ void
+ visitFloat(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
@@ -109,7 +126,9 @@ namespace armarx::armem::gui::instance
auto t = *aron::type::Float::DynamicCastAndCheck(type);
this->addValueRow(key, d, t);
}
- void visitInt(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+
+ void
+ visitInt(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
@@ -119,7 +138,9 @@ namespace armarx::armem::gui::instance
auto t = *aron::type::Int::DynamicCastAndCheck(type);
this->addValueRow(key, d, t);
}
- void visitLong(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+
+ void
+ visitLong(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
@@ -129,7 +150,9 @@ namespace armarx::armem::gui::instance
auto t = *aron::type::Long::DynamicCastAndCheck(type);
this->addValueRow(key, d, t);
}
- void visitString(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+
+ void
+ visitString(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
@@ -139,6 +162,7 @@ namespace armarx::armem::gui::instance
auto t = *aron::type::String::DynamicCastAndCheck(type);
this->addValueRow(key, d, t);
}
+
/*void visitDateTime(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
@@ -151,7 +175,8 @@ namespace armarx::armem::gui::instance
}*/
- void visitMatrix(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+ void
+ visitMatrix(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
@@ -161,7 +186,10 @@ namespace armarx::armem::gui::instance
auto t = *aron::type::Matrix::DynamicCastAndCheck(type);
this->addValueRow(key, d, t);
}
- void visitQuaternion(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+
+ void
+ visitQuaternion(const aron::data::VariantPtr& data,
+ const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
@@ -171,7 +199,10 @@ namespace armarx::armem::gui::instance
auto t = *aron::type::Quaternion::DynamicCastAndCheck(type);
this->addValueRow(key, d, t);
}
- void visitPointCloud(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+
+ void
+ visitPointCloud(const aron::data::VariantPtr& data,
+ const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
@@ -181,7 +212,9 @@ namespace armarx::armem::gui::instance
auto t = *aron::type::PointCloud::DynamicCastAndCheck(type);
this->addValueRow(key, d, t);
}
- void visitImage(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
+
+ void
+ visitImage(const aron::data::VariantPtr& data, const aron::type::VariantPtr& type) override
{
ARMARX_CHECK_NOT_NULL(data);
ARMARX_CHECK_NOT_NULL(type);
@@ -191,13 +224,14 @@ namespace armarx::armem::gui::instance
auto t = *aron::type::Image::DynamicCastAndCheck(type);
this->addValueRow(key, d, t);
}
+
// What aboud NDArray
protected:
-
template <class DataNavigatorT, class TypeNavigatorT>
- bool addValueRow(const std::string& key, const DataNavigatorT& data, const TypeNavigatorT& type)
+ bool
+ addValueRow(const std::string& key, const DataNavigatorT& data, const TypeNavigatorT& type)
{
if (items.size() > 0)
{
@@ -221,7 +255,10 @@ namespace armarx::armem::gui::instance
}
template <class DataNavigatorT, class TypeNavigatorT>
- QTreeWidgetItem* makeItem(const std::string& key, const DataNavigatorT& data, const TypeNavigatorT& type) const
+ QTreeWidgetItem*
+ makeItem(const std::string& key,
+ const DataNavigatorT& data,
+ const TypeNavigatorT& type) const
{
std::stringstream ss;
try
@@ -236,29 +273,39 @@ namespace armarx::armem::gui::instance
es << e.what();
ss << simox::alg::replace_all(es.str(), "\n", " | ");
}
- return new QTreeWidgetItem(this->makeValueRowStrings(key, ss.str(), sanitizeTypeName(type.getFullName())));
+ return new QTreeWidgetItem(
+ this->makeValueRowStrings(key, ss.str(), sanitizeTypeName(type.getFullName())));
}
-
template <class DataNavigatorT, class TypeNavigatorT>
- void streamValueText(const DataNavigatorT& data, const TypeNavigatorT& type, std::stringstream& ss) const
+ void
+ streamValueText(const DataNavigatorT& data,
+ const TypeNavigatorT& type,
+ std::stringstream& ss) const
{
// Fallback to type-agnostic (but data-aware).
- (void) type;
+ (void)type;
TreeDataVisitorBase::streamValueText(data, ss);
}
using TreeDataVisitorBase::streamValueText;
//void streamValueText(const aron::data::Long& data, const aron::type::DateTime& type, std::stringstream& ss) const;
- void streamValueText(const aron::data::NDArray& data, const aron::type::Matrix& type, std::stringstream& ss) const;
+ void streamValueText(const aron::data::NDArray& data,
+ const aron::type::Matrix& type,
+ std::stringstream& ss) const;
- void streamPoseText(const aron::data::NDArray& data, const aron::type::Matrix& type, std::stringstream& ss) const;
- void streamPositionText(const aron::data::NDArray& data, const aron::type::Matrix& type, std::stringstream& ss) const;
- void streamOrientationText(const aron::data::NDArray& data, const aron::type::Matrix& type, std::stringstream& ss) const;
+ void streamPoseText(const aron::data::NDArray& data,
+ const aron::type::Matrix& type,
+ std::stringstream& ss) const;
+ void streamPositionText(const aron::data::NDArray& data,
+ const aron::type::Matrix& type,
+ std::stringstream& ss) const;
+ void streamOrientationText(const aron::data::NDArray& data,
+ const aron::type::Matrix& type,
+ std::stringstream& ss) const;
std::string coeffSep = " ";
};
-}
-
+} // namespace armarx::armem::gui::instance
diff --git a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
index 58e15d056ae74b3d79b48ef917822d20c9de206c..252f1794bd58160fbc5e18eb45bc9757355d7c87 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/tree_visitors/TreeTypedJSONConverter.cpp
@@ -130,7 +130,7 @@ namespace armarx::armem::gui::instance
{
path = elementData->getPath();
}
- else if(elementType)
+ else if (elementType)
{
path = elementType->getPath();
}
@@ -191,7 +191,7 @@ namespace armarx::armem::gui::instance
insertIntoJSON(key, obj);
}
- else if(elementType)
+ else if (elementType)
{
std::string key = elementType->getPath().getLastElement();
insertNULLIntoJSON(key);
@@ -212,7 +212,7 @@ namespace armarx::armem::gui::instance
auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
insertIntoJSON(key, handleGenericNDArray(nd));
}
- else if(elementType)
+ else if (elementType)
{
std::string key = elementType->getPath().getLastElement();
insertNULLIntoJSON(key);
@@ -232,7 +232,8 @@ namespace armarx::armem::gui::instance
const std::string key = elementData->getPath().getLastElement();
auto nd = *aron::data::NDArray::DynamicCastAndCheck(elementData);
nlohmann::json obj;
- Eigen::to_json(obj, aron::data::converter::AronEigenConverter::ConvertToQuaternionf(nd));
+ Eigen::to_json(obj,
+ aron::data::converter::AronEigenConverter::ConvertToQuaternionf(nd));
insertIntoJSON(key, obj);
}
else if (elementType)
@@ -245,7 +246,6 @@ namespace armarx::armem::gui::instance
// should not happen
throw aron::error::AronException(__PRETTY_FUNCTION__, "Both, data and type are NULL.");
}
-
}
void
diff --git a/source/RobotAPI/libraries/armem_gui/lifecycle.cpp b/source/RobotAPI/libraries/armem_gui/lifecycle.cpp
index ce054ff30d516dd5bb64c120b197fd2f50887ae1..174989d7b8ff9d0ab9b53ba44808ba0cbbb9380b 100644
--- a/source/RobotAPI/libraries/armem_gui/lifecycle.cpp
+++ b/source/RobotAPI/libraries/armem_gui/lifecycle.cpp
@@ -24,14 +24,14 @@
#include <ArmarXCore/core/ManagedIceObject.h>
-
namespace armarx::gui
{
LifecycleClient::~LifecycleClient()
{
}
- void LifecycleClient::onInit(ManagedIceObject* component)
+ void
+ LifecycleClient::onInit(ManagedIceObject* component)
{
if (component)
{
@@ -39,7 +39,8 @@ namespace armarx::gui
}
}
- void LifecycleClient::onConnect(ManagedIceObject* component)
+ void
+ LifecycleClient::onConnect(ManagedIceObject* component)
{
if (component)
{
@@ -47,7 +48,8 @@ namespace armarx::gui
}
}
- void LifecycleClient::onDisconnect(ManagedIceObject* component)
+ void
+ LifecycleClient::onDisconnect(ManagedIceObject* component)
{
if (component)
{
@@ -55,53 +57,57 @@ namespace armarx::gui
}
}
- void LifecycleClient::onInit(ManagedIceObject& component)
+ void
+ LifecycleClient::onInit(ManagedIceObject& component)
{
- (void) component;
+ (void)component;
onInit();
}
- void LifecycleClient::onConnect(ManagedIceObject& component)
+ void
+ LifecycleClient::onConnect(ManagedIceObject& component)
{
- (void) component;
+ (void)component;
onConnect();
}
- void LifecycleClient::onDisconnect(ManagedIceObject& component)
+ void
+ LifecycleClient::onDisconnect(ManagedIceObject& component)
{
- (void) component;
+ (void)component;
onDisconnect();
}
- void LifecycleClient::onInit()
+ void
+ LifecycleClient::onInit()
{
}
- void LifecycleClient::onConnect()
+ void
+ LifecycleClient::onConnect()
{
}
- void LifecycleClient::onDisconnect()
+ void
+ LifecycleClient::onDisconnect()
{
}
-}
+} // namespace armarx::gui
-void armarx::gui::connectLifecycle(LifecycleServer* server, LifecycleClient* client)
+void
+armarx::gui::connectLifecycle(LifecycleServer* server, LifecycleClient* client)
{
if (server && client)
{
- server->connect(server, &LifecycleServer::initialized, [client](ManagedIceObject * o)
- {
- client->onInit(o);
- });
- server->connect(server, &LifecycleServer::connected, [client](ManagedIceObject * o)
- {
- client->onConnect(o);
- });
- server->connect(server, &LifecycleServer::disconnected, [client](ManagedIceObject * o)
- {
- client->onDisconnect(o);
- });
+ server->connect(server,
+ &LifecycleServer::initialized,
+ [client](ManagedIceObject* o) { client->onInit(o); });
+ server->connect(server,
+ &LifecycleServer::connected,
+ [client](ManagedIceObject* o) { client->onConnect(o); });
+ server->connect(server,
+ &LifecycleServer::disconnected,
+ [client](ManagedIceObject* o) { client->onDisconnect(o); });
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/lifecycle.h b/source/RobotAPI/libraries/armem_gui/lifecycle.h
index cb5cb3ddfc4d2f2fc3f1b96709db0a3bc49c245e..6cad9b7cd151775bb52883f369421fe51dc76291 100644
--- a/source/RobotAPI/libraries/armem_gui/lifecycle.h
+++ b/source/RobotAPI/libraries/armem_gui/lifecycle.h
@@ -24,11 +24,11 @@
#include <QObject>
-
namespace armarx
{
class ManagedIceObject;
}
+
namespace armarx::gui
{
@@ -36,7 +36,6 @@ namespace armarx::gui
{
Q_OBJECT
public:
-
using QObject::QObject;
@@ -47,19 +46,15 @@ namespace armarx::gui
void initialized(ManagedIceObject* component);
void connected(ManagedIceObject* component);
void disconnected(ManagedIceObject* component);
-
};
-
class LifecycleClient
{
public:
-
virtual ~LifecycleClient();
public:
-
// Used by QtSignals.
virtual void onInit(ManagedIceObject* component);
virtual void onConnect(ManagedIceObject* component);
@@ -74,10 +69,8 @@ namespace armarx::gui
virtual void onInit();
virtual void onConnect();
virtual void onDisconnect();
-
};
-
void connectLifecycle(LifecycleServer* server, LifecycleClient* client);
-}
+} // namespace armarx::gui
diff --git a/source/RobotAPI/libraries/armem_gui/memory/GroupBox.cpp b/source/RobotAPI/libraries/armem_gui/memory/GroupBox.cpp
index 8680fe165cb257b8384f7d32ac2f005007479fff..15bfc79a991094443aca38c19bf1555307796a4a 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/GroupBox.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/GroupBox.cpp
@@ -7,7 +7,6 @@
#include <QSplitter>
#include <QVBoxLayout>
-
namespace armarx::armem::gui::memory
{
@@ -33,29 +32,34 @@ namespace armarx::armem::gui::memory
margin = 0;
_memoryTabGroup->setContentsMargins(margin, margin, margin, margin);
- _memoryTabGroup->setSizePolicy(QSizePolicy::Policy::Expanding, QSizePolicy::Policy::Maximum);
+ _memoryTabGroup->setSizePolicy(QSizePolicy::Policy::Expanding,
+ QSizePolicy::Policy::Maximum);
{
_queryWidget = new armem::gui::QueryWidget();
- _queryWidget->setSizePolicy(QSizePolicy::Policy::Expanding, QSizePolicy::Policy::Maximum);
+ _queryWidget->setSizePolicy(QSizePolicy::Policy::Expanding,
+ QSizePolicy::Policy::Maximum);
_memoryTabWidget->addTab(_queryWidget, QString("Query Settings"));
}
{
_snapshotSelectorWidget = new armem::gui::SnapshotSelectorWidget();
- _snapshotSelectorWidget->setSizePolicy(QSizePolicy::Policy::Expanding, QSizePolicy::Policy::Maximum);
+ _snapshotSelectorWidget->setSizePolicy(QSizePolicy::Policy::Expanding,
+ QSizePolicy::Policy::Maximum);
_memoryTabWidget->addTab(_snapshotSelectorWidget, QString("Snapshot Selection"));
}
{
_predictionWidget = new armem::gui::PredictionWidget(std::move(entityInfoRetriever));
- _predictionWidget->setSizePolicy(QSizePolicy::Policy::Expanding, QSizePolicy::Policy::Maximum);
+ _predictionWidget->setSizePolicy(QSizePolicy::Policy::Expanding,
+ QSizePolicy::Policy::Maximum);
_memoryTabWidget->addTab(_predictionWidget, QString("Prediction"));
}
{
_commitWidget = new armem::gui::CommitWidget();
- _commitWidget->setSizePolicy(QSizePolicy::Policy::Expanding, QSizePolicy::Policy::Maximum);
+ _commitWidget->setSizePolicy(QSizePolicy::Policy::Expanding,
+ QSizePolicy::Policy::Maximum);
_memoryTabWidget->addTab(_commitWidget, QString("Commit"));
}
@@ -69,46 +73,50 @@ namespace armarx::armem::gui::memory
splitter->setSizes({largeHeight, 1});
}
- QueryWidget* GroupBox::queryWidget() const
+ QueryWidget*
+ GroupBox::queryWidget() const
{
return _queryWidget;
}
- QGroupBox* GroupBox::queryGroup() const
+ QGroupBox*
+ GroupBox::queryGroup() const
{
return _memoryTabGroup;
}
- SnapshotSelectorWidget* GroupBox::snapshotSelectorWidget() const
+ SnapshotSelectorWidget*
+ GroupBox::snapshotSelectorWidget() const
{
return _snapshotSelectorWidget;
}
- PredictionWidget* GroupBox::predictionWidget() const
+ PredictionWidget*
+ GroupBox::predictionWidget() const
{
return _predictionWidget;
}
- CommitWidget* GroupBox::commitWidget() const
+ CommitWidget*
+ GroupBox::commitWidget() const
{
return _commitWidget;
}
- MemoryTreeWidget* GroupBox::tree() const
+ MemoryTreeWidget*
+ GroupBox::tree() const
{
return _tree;
}
- armem::client::QueryInput GroupBox::queryInput() const
+ armem::client::QueryInput
+ GroupBox::queryInput() const
{
armem::client::query::Builder queryBuilder(_queryWidget->dataMode());
- queryBuilder
- .coreSegments().all()
- .providerSegments().all()
- .entities().all()
- .snapshots(_snapshotSelectorWidget->selector());
+ queryBuilder.coreSegments().all().providerSegments().all().entities().all().snapshots(
+ _snapshotSelectorWidget->selector());
return queryBuilder.buildQueryInput();
}
-} // namespace armarx::armem::gui::memory
+} // namespace armarx::armem::gui::memory
diff --git a/source/RobotAPI/libraries/armem_gui/memory/GroupBox.h b/source/RobotAPI/libraries/armem_gui/memory/GroupBox.h
index cc9f376ee0a0ad335c7c644fae0ddd90b62fac18..836c7f4c211a5afd19e984be88c009ef9acbb935 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/GroupBox.h
+++ b/source/RobotAPI/libraries/armem_gui/memory/GroupBox.h
@@ -2,8 +2,8 @@
#include <QGroupBox>
-#include <RobotAPI/libraries/armem_gui/memory/TreeWidget.h>
#include <RobotAPI/libraries/armem_gui/commit_widget/CommitWidget.h>
+#include <RobotAPI/libraries/armem_gui/memory/TreeWidget.h>
#include <RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.h>
#include <RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.h>
@@ -11,7 +11,6 @@
class QCheckBox;
class QLabel;
-
namespace armarx::armem::gui::memory
{
@@ -43,7 +42,6 @@ namespace armarx::armem::gui::memory
private:
-
TreeWidget* _tree;
QTabWidget* _memoryTabWidget;
@@ -52,10 +50,9 @@ namespace armarx::armem::gui::memory
SnapshotSelectorWidget* _snapshotSelectorWidget;
PredictionWidget* _predictionWidget;
CommitWidget* _commitWidget;
-
};
-}
+} // namespace armarx::armem::gui::memory
namespace armarx::armem::gui
{
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index f5ec0c9139e8db63acdb2ad006b15ec4ab1e32cb..6eb043ad09befdf11effeffe502c24978c0d3dc7 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -1,15 +1,13 @@
#include "TreeWidget.h"
-#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
-
-#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
-#include <RobotAPI/interface/armem/actions.h>
-
-#include <SimoxUtility/algorithm/string.h>
-
#include <QHeaderView>
#include <QMenu>
+#include <SimoxUtility/algorithm/string.h>
+
+#include <RobotAPI/interface/armem/actions.h>
+#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
namespace armarx::armem::gui::memory
{
@@ -20,7 +18,8 @@ namespace armarx::armem::gui::memory
initWidget();
}
- void TreeWidget::initWidget()
+ void
+ TreeWidget::initWidget()
{
clear();
QStringList columns;
@@ -49,108 +48,110 @@ namespace armarx::armem::gui::memory
connect(this, &This::instanceSelected, this, &This::itemSelected);
setContextMenuPolicy(Qt::CustomContextMenu);
- connect(this, &QTreeWidget::customContextMenuRequested, this, &This::prepareTreeContextMenu);
+ connect(
+ this, &QTreeWidget::customContextMenuRequested, this, &This::prepareTreeContextMenu);
}
- void TreeWidget::initBuilders()
+ void
+ TreeWidget::initBuilders()
{
memoryBuilder.setExpand(true);
- memoryBuilder.setMakeItemFn([this](const std::string & name, const wm::Memory * memory)
- {
- return makeItem(name, *memory);
- });
- memoryBuilder.setUpdateItemFn([this](const std::string&, const wm::Memory * memory, QTreeWidgetItem * memoryItem)
- {
- updateContainerItem(*memory, memoryItem);
- if (memoryItem)
+ memoryBuilder.setMakeItemFn([this](const std::string& name, const wm::Memory* memory)
+ { return makeItem(name, *memory); });
+ memoryBuilder.setUpdateItemFn(
+ [this](const std::string&, const wm::Memory* memory, QTreeWidgetItem* memoryItem)
{
- updateChildren(*memory, memoryItem);
- }
- return true;
- });
+ updateContainerItem(*memory, memoryItem);
+ if (memoryItem)
+ {
+ updateChildren(*memory, memoryItem);
+ }
+ return true;
+ });
- auto nameFn = [](const auto & element)
- {
- return element.name();
- };
+ auto nameFn = [](const auto& element) { return element.name(); };
coreSegmentBuilder.setExpand(true);
coreSegmentBuilder.setNameFn(nameFn, int(Columns::KEY));
- coreSegmentBuilder.setMakeItemFn([this](const wm::CoreSegment & coreSeg)
- {
- return makeItem(coreSeg.name(), coreSeg);
- });
- coreSegmentBuilder.setUpdateItemFn([this](const wm::CoreSegment & coreSeg, QTreeWidgetItem * coreSegItem)
- {
- updateContainerItem(coreSeg, coreSegItem);
- updateChildren(coreSeg, coreSegItem);
- return true;
- });
+ coreSegmentBuilder.setMakeItemFn([this](const wm::CoreSegment& coreSeg)
+ { return makeItem(coreSeg.name(), coreSeg); });
+ coreSegmentBuilder.setUpdateItemFn(
+ [this](const wm::CoreSegment& coreSeg, QTreeWidgetItem* coreSegItem)
+ {
+ updateContainerItem(coreSeg, coreSegItem);
+ updateChildren(coreSeg, coreSegItem);
+ return true;
+ });
provSegmentBuilder.setExpand(true);
provSegmentBuilder.setNameFn(nameFn, int(Columns::KEY));
- provSegmentBuilder.setMakeItemFn([this](const wm::ProviderSegment & provSeg)
- {
- return makeItem(provSeg.name(), provSeg);
- });
- provSegmentBuilder.setUpdateItemFn([this](const wm::ProviderSegment & provSeg, QTreeWidgetItem * provSegItem)
- {
- updateContainerItem(provSeg, provSegItem);
- updateChildren(provSeg, provSegItem);
- return true;
- });
+ provSegmentBuilder.setMakeItemFn([this](const wm::ProviderSegment& provSeg)
+ { return makeItem(provSeg.name(), provSeg); });
+ provSegmentBuilder.setUpdateItemFn(
+ [this](const wm::ProviderSegment& provSeg, QTreeWidgetItem* provSegItem)
+ {
+ updateContainerItem(provSeg, provSegItem);
+ updateChildren(provSeg, provSegItem);
+ return true;
+ });
// entityBuilder.setExpand(true);
entityBuilder.setNameFn(nameFn, int(Columns::KEY));
- entityBuilder.setMakeItemFn([this](const wm::Entity & entity)
- {
- return makeItem(entity.name(), entity);
- });
- entityBuilder.setUpdateItemFn([this](const wm::Entity & entity, QTreeWidgetItem * entityItem)
- {
- updateContainerItem(entity, entityItem);
- updateChildren(entity, entityItem);
- return true;
- });
-
- snapshotBuilder.setMakeItemFn([this](const wm::EntitySnapshot & snapshot)
- {
- QTreeWidgetItem* item = makeItem(toDateTimeMilliSeconds(snapshot.time()), snapshot);
- item->setData(int(Columns::KEY), Qt::ItemDataRole::UserRole, QVariant(static_cast<qlonglong>(snapshot.time().toMicroSecondsSinceEpoch())));
- return item;
- });
- snapshotBuilder.setCompareFn([](const wm::EntitySnapshot & snapshot, QTreeWidgetItem * item)
- {
- return armarx::detail::compare(static_cast<qlonglong>(snapshot.time().toMicroSecondsSinceEpoch()),
- item->data(int(Columns::KEY), Qt::ItemDataRole::UserRole).toLongLong());
- });
- snapshotBuilder.setUpdateItemFn([this](const wm::EntitySnapshot & snapshot, QTreeWidgetItem * snapshotItem)
- {
- updateContainerItem(snapshot, snapshotItem);
- updateChildren(snapshot, snapshotItem);
- return true;
- });
+ entityBuilder.setMakeItemFn([this](const wm::Entity& entity)
+ { return makeItem(entity.name(), entity); });
+ entityBuilder.setUpdateItemFn(
+ [this](const wm::Entity& entity, QTreeWidgetItem* entityItem)
+ {
+ updateContainerItem(entity, entityItem);
+ updateChildren(entity, entityItem);
+ return true;
+ });
- instanceBuilder.setMakeItemFn([this](const wm::EntityInstance & instance)
- {
- QTreeWidgetItem* item = makeItem("", instance);
- return item;
- });
- instanceBuilder.setCompareFn([](const wm::EntityInstance & lhs, QTreeWidgetItem * rhsItem)
- {
- return armarx::detail::compare(lhs.index(), rhsItem->text(0).toInt());
- });
- instanceBuilder.setUpdateItemFn([this](const wm::EntityInstance & instance, QTreeWidgetItem * instanceItem)
- {
- updateItemItem(instance, instanceItem);
- updateChildren(instance, instanceItem);
- return true;
- });
+ snapshotBuilder.setMakeItemFn(
+ [this](const wm::EntitySnapshot& snapshot)
+ {
+ QTreeWidgetItem* item = makeItem(toDateTimeMilliSeconds(snapshot.time()), snapshot);
+ item->setData(
+ int(Columns::KEY),
+ Qt::ItemDataRole::UserRole,
+ QVariant(static_cast<qlonglong>(snapshot.time().toMicroSecondsSinceEpoch())));
+ return item;
+ });
+ snapshotBuilder.setCompareFn(
+ [](const wm::EntitySnapshot& snapshot, QTreeWidgetItem* item)
+ {
+ return armarx::detail::compare(
+ static_cast<qlonglong>(snapshot.time().toMicroSecondsSinceEpoch()),
+ item->data(int(Columns::KEY), Qt::ItemDataRole::UserRole).toLongLong());
+ });
+ snapshotBuilder.setUpdateItemFn(
+ [this](const wm::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem)
+ {
+ updateContainerItem(snapshot, snapshotItem);
+ updateChildren(snapshot, snapshotItem);
+ return true;
+ });
+ instanceBuilder.setMakeItemFn(
+ [this](const wm::EntityInstance& instance)
+ {
+ QTreeWidgetItem* item = makeItem("", instance);
+ return item;
+ });
+ instanceBuilder.setCompareFn(
+ [](const wm::EntityInstance& lhs, QTreeWidgetItem* rhsItem)
+ { return armarx::detail::compare(lhs.index(), rhsItem->text(0).toInt()); });
+ instanceBuilder.setUpdateItemFn(
+ [this](const wm::EntityInstance& instance, QTreeWidgetItem* instanceItem)
+ {
+ updateItemItem(instance, instanceItem);
+ updateChildren(instance, instanceItem);
+ return true;
+ });
}
-
- void TreeWidget::update(const armem::wm::Memory& memory)
+ void
+ TreeWidget::update(const armem::wm::Memory& memory)
{
// Removing elements during the update can create unwanted signals triggering selection handling.
handleSelections = false;
@@ -159,7 +160,8 @@ namespace armarx::armem::gui::memory
emit updated();
}
- void TreeWidget::update(const std::map<std::string, const armem::wm::Memory*>& memories)
+ void
+ TreeWidget::update(const std::map<std::string, const armem::wm::Memory*>& memories)
{
handleSelections = false;
updateChildren(memories, this);
@@ -167,12 +169,14 @@ namespace armarx::armem::gui::memory
emit updated();
}
- std::optional<MemoryID> TreeWidget::selectedID() const
+ std::optional<MemoryID>
+ TreeWidget::selectedID() const
{
return _selectedID;
}
- void TreeWidget::handleSelection()
+ void
+ TreeWidget::handleSelection()
{
if (!handleSelections)
{
@@ -191,7 +195,8 @@ namespace armarx::armem::gui::memory
{
_selectedID = id;
- const std::string levelName = item->data(int(Columns::LEVEL), Qt::UserRole).toString().toStdString();
+ const std::string levelName =
+ item->data(int(Columns::LEVEL), Qt::UserRole).toString().toStdString();
if (levelName == wm::Memory::getLevelName())
{
emit memorySelected(*_selectedID);
@@ -223,58 +228,66 @@ namespace armarx::armem::gui::memory
}
template <class ContainerT>
- static
- auto makeIteratorFn(const ContainerT& container)
+ static auto
+ makeIteratorFn(const ContainerT& container)
{
- return [&container](auto&& elementFn)
- {
- container.forEachChild(elementFn);
- };
+ return [&container](auto&& elementFn) { container.forEachChild(elementFn); };
}
-
- void TreeWidget::updateChildren(const armem::wm::Memory& memory, QTreeWidget* tree)
+ void
+ TreeWidget::updateChildren(const armem::wm::Memory& memory, QTreeWidget* tree)
{
- updateChildren(std::map<std::string, const armem::wm::Memory*> {{memory.name(), &memory}}, tree);
+ updateChildren(std::map<std::string, const armem::wm::Memory*>{{memory.name(), &memory}},
+ tree);
}
- void TreeWidget::updateChildren(const std::map<std::string, const armem::wm::Memory*>& memories, QTreeWidget* tree)
+ void
+ TreeWidget::updateChildren(const std::map<std::string, const armem::wm::Memory*>& memories,
+ QTreeWidget* tree)
{
memoryBuilder.updateTree(tree, memories);
}
-
- void TreeWidget::updateChildren(const armem::wm::Memory& memory, QTreeWidgetItem* memoryItem)
+ void
+ TreeWidget::updateChildren(const armem::wm::Memory& memory, QTreeWidgetItem* memoryItem)
{
coreSegmentBuilder.updateTreeWithIterator(memoryItem, makeIteratorFn(memory));
}
- void TreeWidget::updateChildren(const armem::wm::CoreSegment& coreSeg, QTreeWidgetItem* coreSegItem)
+ void
+ TreeWidget::updateChildren(const armem::wm::CoreSegment& coreSeg, QTreeWidgetItem* coreSegItem)
{
provSegmentBuilder.updateTreeWithIterator(coreSegItem, makeIteratorFn(coreSeg));
}
- void TreeWidget::updateChildren(const armem::wm::ProviderSegment& provSeg, QTreeWidgetItem* provSegItem)
+ void
+ TreeWidget::updateChildren(const armem::wm::ProviderSegment& provSeg,
+ QTreeWidgetItem* provSegItem)
{
entityBuilder.updateTreeWithIterator(provSegItem, makeIteratorFn(provSeg));
}
- void TreeWidget::updateChildren(const armem::wm::Entity& entity, QTreeWidgetItem* entityItem)
+ void
+ TreeWidget::updateChildren(const armem::wm::Entity& entity, QTreeWidgetItem* entityItem)
{
snapshotBuilder.updateTreeWithIterator(entityItem, makeIteratorFn(entity));
}
- void TreeWidget::updateChildren(const armem::wm::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem)
+ void
+ TreeWidget::updateChildren(const armem::wm::EntitySnapshot& snapshot,
+ QTreeWidgetItem* snapshotItem)
{
instanceBuilder.updateTreeWithIterator(snapshotItem, makeIteratorFn(snapshot));
}
- void TreeWidget::updateChildren(const armem::wm::EntityInstance& data, QTreeWidgetItem* dataItem)
+ void
+ TreeWidget::updateChildren(const armem::wm::EntityInstance& data, QTreeWidgetItem* dataItem)
{
- (void) data, (void) dataItem;
+ (void)data, (void)dataItem;
}
- void TreeWidget::prepareTreeContextMenu(const QPoint& pos)
+ void
+ TreeWidget::prepareTreeContextMenu(const QPoint& pos)
{
const QTreeWidgetItem* item = this->itemAt(pos);
if (item == nullptr)
@@ -287,19 +300,22 @@ namespace armarx::armem::gui::memory
}
template <class MemoryItemT>
- QTreeWidgetItem* TreeWidget::makeItem(const std::string& key, const MemoryItemT& memoryItem)
+ QTreeWidgetItem*
+ TreeWidget::makeItem(const std::string& key, const MemoryItemT& memoryItem)
{
- (void) key;
+ (void)key;
return makeItem(memoryItem.getKeyString(), MemoryItemT::getLevelName(), memoryItem.id());
}
- QTreeWidgetItem* TreeWidget::makeItem(const std::string& key, const std::string& levelName, const MemoryID& id)
+ QTreeWidgetItem*
+ TreeWidget::makeItem(const std::string& key, const std::string& levelName, const MemoryID& id)
{
QStringList columns;
columns.insert(int(Columns::KEY), QString::fromStdString(key));
columns.insert(int(Columns::SIZE), "");
columns.insert(int(Columns::TYPE), "");
- columns.insert(int(Columns::LEVEL), QString::fromStdString(simox::alg::capitalize_words(levelName)));
+ columns.insert(int(Columns::LEVEL),
+ QString::fromStdString(simox::alg::capitalize_words(levelName)));
columns.insert(int(Columns::ID), QString::fromStdString(id.str()));
QTreeWidgetItem* item = new QTreeWidgetItem(columns);
@@ -309,21 +325,24 @@ namespace armarx::armem::gui::memory
return item;
}
- void TreeWidget::updateItemItem(const armem::base::detail::MemoryItem& level, QTreeWidgetItem* item)
+ void
+ TreeWidget::updateItemItem(const armem::base::detail::MemoryItem& level, QTreeWidgetItem* item)
{
- (void) level, (void) item;
+ (void)level, (void)item;
}
template <class ContainerT>
- void TreeWidget::updateContainerItem(const ContainerT& container, QTreeWidgetItem* item)
+ void
+ TreeWidget::updateContainerItem(const ContainerT& container, QTreeWidgetItem* item)
{
updateItemItem(container, item);
item->setText(int(Columns::SIZE), QString::number(container.size()));
// Does not work
- if constexpr(std::is_base_of_v<base::detail::AronTyped, ContainerT>)
+ if constexpr (std::is_base_of_v<base::detail::AronTyped, ContainerT>)
{
- const base::detail::AronTyped& cast = static_cast<const base::detail::AronTyped&>(container);
+ const base::detail::AronTyped& cast =
+ static_cast<const base::detail::AronTyped&>(container);
std::string typeName;
if (cast.aronType())
{
@@ -338,4 +357,4 @@ namespace armarx::armem::gui::memory
}
}
-}
+} // namespace armarx::armem::gui::memory
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
index 1ad975e5f29e741df801c55906d0ff364e607996..dad86d93cc9360abf6498d39a12990c5a77fad9c 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
@@ -5,10 +5,8 @@
#include <QTreeWidget>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-
#include <RobotAPI/libraries/armem_gui/TreeWidgetBuilder.h>
-
namespace armarx::armem::gui::memory
{
@@ -18,7 +16,6 @@ namespace armarx::armem::gui::memory
using This = TreeWidget;
public:
-
TreeWidget();
void update(const armem::wm::Memory& memory);
@@ -44,8 +41,10 @@ namespace armarx::armem::gui::memory
void snapshotSelected(const MemoryID& id);
void instanceSelected(const MemoryID& id);
- void actionsMenuRequested(const MemoryID& memoryID, QWidget* parent,
- const QPoint& pos, QMenu* menu);
+ void actionsMenuRequested(const MemoryID& memoryID,
+ QWidget* parent,
+ const QPoint& pos,
+ QMenu* menu);
private slots:
@@ -54,25 +53,28 @@ namespace armarx::armem::gui::memory
private:
-
void initWidget();
void initBuilders();
void updateChildren(const armem::wm::Memory& memory, QTreeWidget* tree);
- void updateChildren(const std::map<std::string, const armem::wm::Memory*>& memories, QTreeWidget* tree);
+ void updateChildren(const std::map<std::string, const armem::wm::Memory*>& memories,
+ QTreeWidget* tree);
void updateChildren(const armem::wm::Memory& memory, QTreeWidgetItem* memoryItem);
void updateChildren(const armem::wm::CoreSegment& coreSeg, QTreeWidgetItem* coreSegItem);
- void updateChildren(const armem::wm::ProviderSegment& provSeg, QTreeWidgetItem* provSegItem);
+ void updateChildren(const armem::wm::ProviderSegment& provSeg,
+ QTreeWidgetItem* provSegItem);
void updateChildren(const armem::wm::Entity& entity, QTreeWidgetItem* entityItem);
- void updateChildren(const armem::wm::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem);
+ void updateChildren(const armem::wm::EntitySnapshot& snapshot,
+ QTreeWidgetItem* snapshotItem);
void updateChildren(const armem::wm::EntityInstance& data, QTreeWidgetItem* parent);
void prepareTreeContextMenu(const QPoint& pos);
template <class MemoryItemT>
QTreeWidgetItem* makeItem(const std::string& key, const MemoryItemT& memoryItem);
- QTreeWidgetItem* makeItem(const std::string& key, const std::string& levelName, const MemoryID& id);
+ QTreeWidgetItem*
+ makeItem(const std::string& key, const std::string& levelName, const MemoryID& id);
void updateItemItem(const armem::base::detail::MemoryItem& level, QTreeWidgetItem* item);
template <class ContainerT>
@@ -80,7 +82,6 @@ namespace armarx::armem::gui::memory
private:
-
MapTreeWidgetBuilder<std::string, const wm::Memory*> memoryBuilder;
TreeWidgetBuilder<wm::CoreSegment> coreSegmentBuilder;
TreeWidgetBuilder<wm::ProviderSegment> provSegmentBuilder;
@@ -103,7 +104,7 @@ namespace armarx::armem::gui::memory
};
};
-}
+} // namespace armarx::armem::gui::memory
namespace armarx::armem::gui
{
diff --git a/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp b/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp
index a32ced5e4ef945ff4426c75b074cc24434b889c3..79597a46775d204e45260f9a1397d7eea7f7b36c 100644
--- a/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.cpp
@@ -32,7 +32,6 @@
#include "TimestampInput.h"
-
namespace armarx::armem::gui
{
PredictionWidget::PredictionWidget(GetEntityInfoFn&& entityInfoRetriever) :
diff --git a/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.h b/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.h
index c853a9bb4b76181417dee47c59a5eeb5635fe6ff..b1c657d9037a1e47308032681fdf52442f04abad 100644
--- a/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/prediction_widget/PredictionWidget.h
@@ -38,22 +38,20 @@ class QLineEdit;
class QPushButton;
class QSpinBox;
-
namespace armarx::armem::gui
{
class TimestampInput;
-
class PredictionWidget : public QWidget
{
Q_OBJECT // NOLINT
- public:
- struct EntityInfo
+ public : struct EntityInfo
{
aron::type::ObjectPtr type = nullptr;
std::vector<PredictionEngine> engines;
};
+
using GetEntityInfoFn = std::function<EntityInfo(const MemoryID&)>;
public:
@@ -90,6 +88,5 @@ namespace armarx::armem::gui
void updateCurrentEntity();
void startPrediction();
-
};
} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.h b/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.h
index 59e5cfa207b82926bdca68357790297e4c52d31b..1d271ae41f710f3a4e4a0443c4a2871e9559a901 100644
--- a/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.h
+++ b/source/RobotAPI/libraries/armem_gui/prediction_widget/TimestampInput.h
@@ -28,6 +28,7 @@
class QDateTimeEdit;
class QDoubleSpinBox;
+
namespace armarx::gui
{
class LeadingZeroSpinBox;
@@ -40,17 +41,16 @@ namespace armarx::armem::gui
{
Q_OBJECT // NOLINT
- public:
- virtual armarx::DateTime retrieveTimeStamp() = 0;
+ public :
+ virtual armarx::DateTime
+ retrieveTimeStamp() = 0;
};
-
class AbsoluteTimestampInput : public TimestampInput
{
Q_OBJECT // NOLINT
- public:
- AbsoluteTimestampInput();
+ public : AbsoluteTimestampInput();
armarx::DateTime retrieveTimeStamp() override;
@@ -59,13 +59,11 @@ namespace armarx::armem::gui
armarx::gui::LeadingZeroSpinBox* microseconds;
};
-
class RelativeTimestampInput : public TimestampInput
{
Q_OBJECT // NOLINT
- public:
- RelativeTimestampInput();
+ public : RelativeTimestampInput();
armarx::DateTime retrieveTimeStamp() override;
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
index 3e1741e91c0d1d70c81c0780e8689be3f8d171fc..74a740f74aa0e9f26957bca936fdc2f7ec40d49c 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/QueryWidget.cpp
@@ -153,7 +153,8 @@ namespace armarx::armem::gui
std::vector<std::string> alreadyPresentMemoryCheckboxes;
// get information about memories already present and activate inactive ones if necessary
- int maxIndex = _availableMemoriesGroupBox->layout()->count(); //as the (de)select button is counted as well
+ int maxIndex = _availableMemoriesGroupBox->layout()
+ ->count(); //as the (de)select button is counted as well
for (int i = 1; i < maxIndex; ++i)
{
auto w = _availableMemoriesGroupBox->layout()->itemAt(i)->widget();
@@ -189,7 +190,8 @@ namespace armarx::armem::gui
{
// new memory available
auto box = new QCheckBox(QString::fromStdString(memoryName));
- box->setChecked(true); // we do not want all memories to be enabled on startup, to reduce communication overhead
+ box->setChecked(
+ true); // we do not want all memories to be enabled on startup, to reduce communication overhead
box->setVisible(true);
box->setEnabled(true);
_availableMemoriesGroupBox->layout()->addWidget(box);
@@ -286,16 +288,18 @@ namespace armarx::armem::gui
}
}
- void
+ void
QueryWidget::deSelectMemoryServers()
{
- if(!allMemoryServersSelected){
+ if (!allMemoryServersSelected)
+ {
//ARMARX_INFO << "Selecting all memory servers";
//not all servers are selected -> select all memory servers:
std::scoped_lock l(enabledMemoriesMutex);
// get information about memories already present and activate inactive ones if necessary
- int maxIndex = _availableMemoriesGroupBox->layout()->count(); //as the (de)select button is counted as well
+ int maxIndex = _availableMemoriesGroupBox->layout()
+ ->count(); //as the (de)select button is counted as well
for (int i = 1; i < maxIndex; ++i)
{
auto w = _availableMemoriesGroupBox->layout()->itemAt(i)->widget();
@@ -306,14 +310,16 @@ namespace armarx::armem::gui
//set variables and text:
allMemoryServersSelected = true;
_de_selectMemoryServers->setText(deselectText);
-
- } else {
+ }
+ else
+ {
//ARMARX_INFO << "Deselecting all memory servers";
//memories should be deselected:
std::scoped_lock l(enabledMemoriesMutex);
// get information about memories already present and activate inactive ones if necessary
- int maxIndex = _availableMemoriesGroupBox->layout()->count(); //as the (de)select button is counted as well
+ int maxIndex = _availableMemoriesGroupBox->layout()
+ ->count(); //as the (de)select button is counted as well
for (int i = 1; i < maxIndex; ++i)
{
auto w = _availableMemoriesGroupBox->layout()->itemAt(i)->widget();
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
index c6977d062d062d441d62337848634a903ead43d7..c39fc45b24d437fd9a25923520a3778b9675d84d 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
@@ -5,45 +5,45 @@
#include <QDateTimeEdit>
#include <QFormLayout>
#include <QGridLayout>
-#include <QLabel>
#include <QHBoxLayout>
-#include <QVBoxLayout>
+#include <QLabel>
#include <QSpinBox>
+#include <QVBoxLayout>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem_gui/gui_utils.h>
-
namespace armarx::armem::gui
{
- client::query::SnapshotSelector SnapshotForm::makeEntitySelector()
+ client::query::SnapshotSelector
+ SnapshotForm::makeEntitySelector()
{
client::query::SnapshotSelector s;
fillEntitySelector(s);
return s;
}
- void SnapshotForm::setDateTimeDisplayFormat(QDateTimeEdit* dt)
+ void
+ SnapshotForm::setDateTimeDisplayFormat(QDateTimeEdit* dt)
{
dt->setDisplayFormat("yyyy-MM-dd HH:mm:ss");
}
-
SnapshotFormAll::SnapshotFormAll()
{
}
- void SnapshotFormAll::fillEntitySelector(client::query::SnapshotSelector& selector)
+
+ void
+ SnapshotFormAll::fillEntitySelector(client::query::SnapshotSelector& selector)
{
selector.all();
}
-
-
SnapshotFormSingle::SnapshotFormSingle()
{
const QDateTime now = QDateTime::currentDateTime();
@@ -77,21 +77,23 @@ namespace armarx::armem::gui
connect(latest, &QCheckBox::toggled, microseconds, &QSpinBox::setDisabled);
connect(dateTime, &QDateTimeEdit::dateTimeChanged, this, &SnapshotForm::queryChanged);
- connect(microseconds, QOverload<int>::of(&QSpinBox::valueChanged), this, &SnapshotForm::queryChanged);
+ connect(microseconds,
+ QOverload<int>::of(&QSpinBox::valueChanged),
+ this,
+ &SnapshotForm::queryChanged);
connect(latest, &QCheckBox::toggled, this, &SnapshotForm::queryChanged);
}
-
- void SnapshotFormSingle::fillEntitySelector(client::query::SnapshotSelector& selector)
+ void
+ SnapshotFormSingle::fillEntitySelector(client::query::SnapshotSelector& selector)
{
const Time time = latest->isChecked()
- ? Time::Invalid()
- : (Time(Duration::Seconds(dateTime->dateTime().toSecsSinceEpoch()))
- + Duration::MicroSeconds(microseconds->value()));
+ ? Time::Invalid()
+ : (Time(Duration::Seconds(dateTime->dateTime().toSecsSinceEpoch())) +
+ Duration::MicroSeconds(microseconds->value()));
selector.atTime(time);
}
-
SnapshotFormTimeRange::SnapshotFormTimeRange()
{
const QDateTime now = QDateTime::currentDateTime();
@@ -136,7 +138,8 @@ namespace armarx::armem::gui
connect(toDateTime, &QDateTimeEdit::dateTimeChanged, this, &SnapshotForm::queryChanged);
}
- void SnapshotFormTimeRange::fillEntitySelector(client::query::SnapshotSelector& selector)
+ void
+ SnapshotFormTimeRange::fillEntitySelector(client::query::SnapshotSelector& selector)
{
Time defaults = Time::Invalid();
Time min = defaults, max = defaults;
@@ -147,14 +150,13 @@ namespace armarx::armem::gui
}
if (!toEnd->isChecked())
{
- max = Time(Duration::MilliSeconds(toDateTime->dateTime().toMSecsSinceEpoch()))
- + Duration::MicroSeconds(int(1e6) - 1);
+ max = Time(Duration::MilliSeconds(toDateTime->dateTime().toMSecsSinceEpoch())) +
+ Duration::MicroSeconds(int(1e6) - 1);
}
selector.timeRange(min, max);
}
-
SnapshotFormIndexRange::SnapshotFormIndexRange()
{
firstLabel = new QLabel("First:");
@@ -171,7 +173,7 @@ namespace armarx::armem::gui
lastEnd->setChecked(true);
lastSpinBox->setDisabled(lastEnd->isChecked());
- std::initializer_list<QSpinBox*> sbs { firstSpinBox, lastSpinBox };
+ std::initializer_list<QSpinBox*> sbs{firstSpinBox, lastSpinBox};
for (QSpinBox* sb : sbs)
{
sb->setMinimum(-1000);
@@ -180,7 +182,8 @@ namespace armarx::armem::gui
firstSpinBox->setValue(-2);
lastSpinBox->setValue(-1);
- QString tooltip = "Python index semantics: Negative indices count from the end (-1 is the last entry).";
+ QString tooltip =
+ "Python index semantics: Negative indices count from the end (-1 is the last entry).";
firstSpinBox->setToolTip(tooltip);
lastSpinBox->setToolTip(tooltip);
@@ -202,12 +205,19 @@ namespace armarx::armem::gui
connect(lastEnd, &QCheckBox::toggled, lastSpinBox, &QSpinBox::setDisabled);
connect(firstBegin, &QCheckBox::toggled, this, &SnapshotForm::queryChanged);
- connect(firstSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, &SnapshotForm::queryChanged);
+ connect(firstSpinBox,
+ QOverload<int>::of(&QSpinBox::valueChanged),
+ this,
+ &SnapshotForm::queryChanged);
connect(lastEnd, &QCheckBox::toggled, this, &SnapshotForm::queryChanged);
- connect(lastSpinBox, QOverload<int>::of(&QSpinBox::valueChanged), this, &SnapshotForm::queryChanged);
+ connect(lastSpinBox,
+ QOverload<int>::of(&QSpinBox::valueChanged),
+ this,
+ &SnapshotForm::queryChanged);
}
- void SnapshotFormIndexRange::fillEntitySelector(client::query::SnapshotSelector& selector)
+ void
+ SnapshotFormIndexRange::fillEntitySelector(client::query::SnapshotSelector& selector)
{
long first = 0, last = -1;
@@ -223,4 +233,4 @@ namespace armarx::armem::gui
selector.indexRange(first, last);
}
-}
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h
index 1e673a73ebc9dc1a0ad7189f4484fee8820a3b12..2308358cf5060609282bda16e887bab700936f1a 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.h
@@ -10,7 +10,6 @@ class QDateTimeEdit;
class QLabel;
class QSpinBox;
-
namespace armarx::armem::gui
{
@@ -29,11 +28,8 @@ namespace armarx::armem::gui
protected:
virtual void fillEntitySelector(client::query::SnapshotSelector& selector) = 0;
void setDateTimeDisplayFormat(QDateTimeEdit* dt);
-
};
-
-
class SnapshotFormAll : public SnapshotForm
{
public:
@@ -43,7 +39,6 @@ namespace armarx::armem::gui
void fillEntitySelector(client::query::SnapshotSelector& selector) override;
};
-
class SnapshotFormSingle : public SnapshotForm
{
public:
@@ -59,7 +54,6 @@ namespace armarx::armem::gui
QCheckBox* latest;
};
-
class SnapshotFormTimeRange : public SnapshotForm
{
public:
@@ -77,7 +71,6 @@ namespace armarx::armem::gui
QCheckBox* toEnd;
};
-
class SnapshotFormIndexRange : public SnapshotForm
{
public:
@@ -95,4 +88,4 @@ namespace armarx::armem::gui
QCheckBox* lastEnd;
};
-}
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
index f38c186b630e3d07fc867f1036159476b17bf767..828c601f1f012770e7e7c720eac36848ef9de1e7 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
@@ -3,15 +3,15 @@
#include <QCheckBox>
#include <QComboBox>
#include <QHBoxLayout>
-#include <QVBoxLayout>
#include <QPushButton>
+#include <QVBoxLayout>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::armem::gui
{
- client::query::SnapshotSelector SnapshotSelectorWidget::selector()
+ client::query::SnapshotSelector
+ SnapshotSelectorWidget::selector()
{
return _queryForms.at(_queryComboBox->currentText())->makeEntitySelector();
}
@@ -31,7 +31,10 @@ namespace armarx::armem::gui
typeLayout->addWidget(_queryComboBox);
- connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::showSelectedFormForQuery);
+ connect(_queryComboBox,
+ &QComboBox::currentTextChanged,
+ this,
+ &This::showSelectedFormForQuery);
connect(_queryComboBox, &QComboBox::currentTextChanged, this, &This::queryChanged);
// query type select box
@@ -51,7 +54,8 @@ namespace armarx::armem::gui
showSelectedFormForQuery(_queryComboBox->currentText());
}
- void SnapshotSelectorWidget::showSelectedFormForQuery(QString selected)
+ void
+ SnapshotSelectorWidget::showSelectedFormForQuery(QString selected)
{
for (auto& [name, form] : _queryForms)
{
@@ -59,14 +63,16 @@ namespace armarx::armem::gui
}
}
- void SnapshotSelectorWidget::addForm(const QString& key, SnapshotForm* form)
+ void
+ SnapshotSelectorWidget::addForm(const QString& key, SnapshotForm* form)
{
auto r = _queryForms.emplace(key, form);
_pageLayout->addWidget(r.first->second);
//connect(r.first->second, &SnapshotForm::queryChanged, this, &This::updateSelector);
}
- void SnapshotSelectorWidget::hideAllForms()
+ void
+ SnapshotSelectorWidget::hideAllForms()
{
for (auto& [_, form] : _queryForms)
{
@@ -75,4 +81,4 @@ namespace armarx::armem::gui
}
-}
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
index 23833f71ea0f297379385abee48f4a01698670aa..2d77e5624b69b5642a22b7e79bfe1b6e13538a3d 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
@@ -4,8 +4,8 @@
#include <QWidget>
-#include <RobotAPI/libraries/armem/core/query/DataMode.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/query/DataMode.h>
#include "SnapshotForm.h"
@@ -15,7 +15,6 @@ class QComboBox;
class QVBoxLayout;
class QPushButton;
-
namespace armarx::armem::gui
{
@@ -31,7 +30,6 @@ namespace armarx::armem::gui
using This = SnapshotSelectorWidget;
public:
-
SnapshotSelectorWidget();
@@ -56,7 +54,6 @@ namespace armarx::armem::gui
private:
-
void addForm(const QString& key, SnapshotForm* form);
@@ -65,7 +62,6 @@ namespace armarx::armem::gui
QComboBox* _queryComboBox;
/// The forms for the different query types. Hidden when not selected.
std::map<QString, SnapshotForm*> _queryForms;
-
};
-}
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_index/aron_conversions.cpp b/source/RobotAPI/libraries/armem_index/aron_conversions.cpp
index 8b91c50408bf30b099307156ffb30de1dfc34643..7b7d869e045a65987e8b10697ceebf042b012587 100644
--- a/source/RobotAPI/libraries/armem_index/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_index/aron_conversions.cpp
@@ -3,7 +3,6 @@
#include <RobotAPI/libraries/armem_index/aron/Named.aron.generated.h>
#include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
-
namespace armarx::armem::index
{
} // namespace armarx::armem::index
diff --git a/source/RobotAPI/libraries/armem_index/aron_conversions.h b/source/RobotAPI/libraries/armem_index/aron_conversions.h
index 6e774eaff84ee5e2de7f6e187603b497eefd53f2..e976e258e9beba313cc74e543a123315ab35293e 100644
--- a/source/RobotAPI/libraries/armem_index/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_index/aron_conversions.h
@@ -2,7 +2,6 @@
#include "aron_forward_declarations.h"
-
namespace armarx::armem::index
{
} // namespace armarx::armem::index
diff --git a/source/RobotAPI/libraries/armem_index/aron_forward_declarations.h b/source/RobotAPI/libraries/armem_index/aron_forward_declarations.h
index dc7606a373e03b3e804c602899bfaa0738ff0104..11f712e449fb66f285a515778c4f423084aa290d 100644
--- a/source/RobotAPI/libraries/armem_index/aron_forward_declarations.h
+++ b/source/RobotAPI/libraries/armem_index/aron_forward_declarations.h
@@ -1,6 +1,5 @@
#pragma once
-
namespace armarx::armem::index::arondto
{
class Named;
diff --git a/source/RobotAPI/libraries/armem_index/forward_declarations.h b/source/RobotAPI/libraries/armem_index/forward_declarations.h
index c6468e5a0b6999c7a6625f7042c1abcbd9f310b4..7aa2a3b76e382f918b859e112586f5d8ac0272a8 100644
--- a/source/RobotAPI/libraries/armem_index/forward_declarations.h
+++ b/source/RobotAPI/libraries/armem_index/forward_declarations.h
@@ -2,7 +2,6 @@
#include "aron_forward_declarations.h"
-
namespace armarx::armem::index
{
}
diff --git a/source/RobotAPI/libraries/armem_index/memory_ids.cpp b/source/RobotAPI/libraries/armem_index/memory_ids.cpp
index 246737408f13650d28f3f1290211a184c73305a5..429228815424fc8f28ee96e4a479e0782bbda476 100644
--- a/source/RobotAPI/libraries/armem_index/memory_ids.cpp
+++ b/source/RobotAPI/libraries/armem_index/memory_ids.cpp
@@ -22,13 +22,12 @@
#include "memory_ids.h"
-
namespace armarx::armem
{
- const MemoryID index::memoryID { "Index" };
+ const MemoryID index::memoryID{"Index"};
- const MemoryID index::namedSegmentID { memoryID.withCoreSegmentName("Named") };
- const MemoryID index::spatialSegmentID { memoryID.withCoreSegmentName("Spatial") };
+ const MemoryID index::namedSegmentID{memoryID.withCoreSegmentName("Named")};
+ const MemoryID index::spatialSegmentID{memoryID.withCoreSegmentName("Spatial")};
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_index/memory_ids.h b/source/RobotAPI/libraries/armem_index/memory_ids.h
index 6173e7a2a022710518934ddffe4f1d72c5c7b553..c32f64cfa43ad5b8bb94ddc84425a886ca5518a2 100644
--- a/source/RobotAPI/libraries/armem_index/memory_ids.h
+++ b/source/RobotAPI/libraries/armem_index/memory_ids.h
@@ -24,7 +24,6 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
namespace armarx::armem::index
{
@@ -33,4 +32,4 @@ namespace armarx::armem::index
extern const MemoryID spatialSegmentID;
extern const MemoryID namedSegmentID;
-} // namespace armarx::armem::objects
+} // namespace armarx::armem::index
diff --git a/source/RobotAPI/libraries/armem_index/server/server.cpp b/source/RobotAPI/libraries/armem_index/server/server.cpp
index 963efb65f78d449683e5db8f2db44a97c8e08a8d..b9fa2c0469c20b5fc03c158646471c88fecd6882 100644
--- a/source/RobotAPI/libraries/armem_index/server/server.cpp
+++ b/source/RobotAPI/libraries/armem_index/server/server.cpp
@@ -1,6 +1,5 @@
#include "server.h"
-
namespace armarx::armem::index::server
{
} // namespace armarx::armem::index::server
diff --git a/source/RobotAPI/libraries/armem_index/server/server.h b/source/RobotAPI/libraries/armem_index/server/server.h
index b0ac2558419db6e938a3215afb9569ab20cfb5e8..ef401309c1ce617758686fb877647984f1357696 100644
--- a/source/RobotAPI/libraries/armem_index/server/server.h
+++ b/source/RobotAPI/libraries/armem_index/server/server.h
@@ -1,6 +1,5 @@
#pragma once
-
namespace armarx::armem::index::server
{
} // namespace armarx::armem::index::server
diff --git a/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.cpp b/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.cpp
index 4cc4a87d2f003633f99b365f5b068b53efc2ac33..f4124a98a037f6f28ac0cbb807157909a8a7643d 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_laser_scans/aron_conversions.cpp
@@ -12,7 +12,6 @@
#include "types.h"
-
namespace armarx::armem::laser_scans
{
@@ -96,4 +95,4 @@ namespace armarx::armem::laser_scans
}
-} // namespace armarx::armem::vision
+} // namespace armarx::armem::laser_scans
diff --git a/source/RobotAPI/libraries/armem_laser_scans/constants.h b/source/RobotAPI/libraries/armem_laser_scans/constants.h
index eb934187159e74e0c13a8a0bc68c391d200c762b..cc4dcb31940147b9c4bfd4227b3b8f200cf65093 100644
--- a/source/RobotAPI/libraries/armem_laser_scans/constants.h
+++ b/source/RobotAPI/libraries/armem_laser_scans/constants.h
@@ -30,4 +30,4 @@ namespace armarx::armem::laser_scans::constants
// core segments
const inline std::string laserScanCoreSegment = "LaserScans";
-} // namespace armarx::armem::vision::constants
+} // namespace armarx::armem::laser_scans::constants
diff --git a/source/RobotAPI/libraries/armem_locations/aron_conversions.h b/source/RobotAPI/libraries/armem_locations/aron_conversions.h
index 93e57fbcecc4f1e8347559d300a7a9272b24555b..486d9eb70eb2da432b1162286fc7ef07f74fd377 100644
--- a/source/RobotAPI/libraries/armem_locations/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_locations/aron_conversions.h
@@ -1,6 +1,5 @@
#pragma once
-
namespace armarx::armem
{
diff --git a/source/RobotAPI/libraries/armem_motions/armem_motions.h b/source/RobotAPI/libraries/armem_motions/armem_motions.h
index f9dca33dd7b46131ee27c6d52c2cf7e4e579f9c4..0a296213def2dd09444f9d920f145338a351cf09 100644
--- a/source/RobotAPI/libraries/armem_motions/armem_motions.h
+++ b/source/RobotAPI/libraries/armem_motions/armem_motions.h
@@ -1,6 +1,5 @@
#pragma once
-
namespace armarx::armem::motions::mdb
{
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp
index 76c811b3f05438c5d880da647afc54eb461dd114..8e210ed9004706c6bd2e752522757e3f044e1ba7 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.cpp
@@ -2,38 +2,47 @@
#include "MotionSegment.h"
// ArmarX
-#include "mdb_conversions.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
+#include "mdb_conversions.h"
// STD / STL
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
-
namespace armarx::armem::server::motions::mdb::segment
{
MDBMotionSegment::MDBMotionSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- Base(memoryToIceAdapter, "MotionDatabase", "Motion", armarx::motion::mdb::arondto::MDBReference::ToAronType())
+ Base(memoryToIceAdapter,
+ "MotionDatabase",
+ "Motion",
+ armarx::motion::mdb::arondto::MDBReference::ToAronType())
{
}
- void MDBMotionSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ MDBMotionSegment::defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
Base::defineProperties(defs, prefix);
- defs->optional(properties.motionsPackage, prefix + "MotionsPackage", "Name of the prior knowledge package to load from.");
- defs->optional(properties.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup.");
+ defs->optional(properties.motionsPackage,
+ prefix + "MotionsPackage",
+ "Name of the prior knowledge package to load from.");
+ defs->optional(properties.loadFromMotionsPackage,
+ prefix + "LoadFromMotionsPackage",
+ "If true, load the motions from the motions package on startup.");
}
- void MDBMotionSegment::init()
+ void
+ MDBMotionSegment::init()
{
Base::init();
@@ -43,12 +52,13 @@ namespace armarx::armem::server::motions::mdb::segment
}
}
- void MDBMotionSegment::onConnect()
+ void
+ MDBMotionSegment::onConnect()
{
-
}
- int MDBMotionSegment::loadByMotionFinder(const std::string& packageName)
+ int
+ MDBMotionSegment::loadByMotionFinder(const std::string& packageName)
{
priorknowledge::motions::MotionFinder motionFinder(packageName, "motions/");
int loadedMotions = 0;
@@ -59,7 +69,8 @@ namespace armarx::armem::server::motions::mdb::segment
auto allMotions = motionFinder.findAll("mdb");
for (const auto& motionFinderInfo : allMotions)
{
- auto pathToInfoJson = motionFinderInfo.getFullPath() / motionFinderInfo.getID() / (motionFinderInfo.getID() + ".json");
+ auto pathToInfoJson = motionFinderInfo.getFullPath() / motionFinderInfo.getID() /
+ (motionFinderInfo.getID() + ".json");
if (auto op = conversion::createFromFile(pathToInfoJson); op.has_value())
{
std::stringstream ss;
@@ -71,7 +82,7 @@ namespace armarx::armem::server::motions::mdb::segment
ss << "\n" << attachedFile.fileName << " (" << key << ")";
}
}
- ARMARX_INFO << ss.str(); // ToDo @Fabian PK: Remove
+ ARMARX_INFO << ss.str(); // ToDo @Fabian PK: Remove
auto& entity = this->segmentPtr->addEntity(std::to_string(op->id));
auto& snapshot = entity.addSnapshot(op->createdDate);
@@ -93,4 +104,4 @@ namespace armarx::armem::server::motions::mdb::segment
return loadedMotions;
}
-}
+} // namespace armarx::armem::server::motions::mdb::segment
diff --git a/source/RobotAPI/libraries/armem_motions/server/MotionSegment.h b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.h
index de2fd746f7a9f528b093cafc4e92c7e8a6da6641..0347b14edec74a32872a4a6e6c1ccfae288f7aaf 100644
--- a/source/RobotAPI/libraries/armem_motions/server/MotionSegment.h
+++ b/source/RobotAPI/libraries/armem_motions/server/MotionSegment.h
@@ -11,7 +11,6 @@
// ArmarX
#include <RobotAPI/libraries/armem_motions/aron/MDBReference.aron.generated.h>
-
namespace armarx::armem::server::motions::mdb::segment
{
class MDBMotionSegment : public armem::server::segment::SpecializedProviderSegment
@@ -21,7 +20,8 @@ namespace armarx::armem::server::motions::mdb::segment
public:
MDBMotionSegment(armem::server::MemoryToIceAdapter& iceMemory);
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
virtual void init() override;
virtual void onConnect();
@@ -34,6 +34,7 @@ namespace armarx::armem::server::motions::mdb::segment
std::string motionsPackage = "PriorKnowledgeData";
bool loadFromMotionsPackage = true;
};
+
Properties properties;
};
-}
+} // namespace armarx::armem::server::motions::mdb::segment
diff --git a/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp
index d173538b588e3ab711e78e941fb7447cab60e34a..27b955c3159f80e22a84bca4c3ec810a4be61c0c 100644
--- a/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.cpp
@@ -3,15 +3,17 @@
#include <SimoxUtility/json/json.hpp>
-
namespace armarx::armem::server::motions::mdb::conversion
{
- std::optional<armarx::motion::mdb::arondto::MDBReference> createFromFile(const std::filesystem::path& pathToInfoJson)
+ std::optional<armarx::motion::mdb::arondto::MDBReference>
+ createFromFile(const std::filesystem::path& pathToInfoJson)
{
- if (std::filesystem::exists(pathToInfoJson) && std::filesystem::is_regular_file(pathToInfoJson))
+ if (std::filesystem::exists(pathToInfoJson) &&
+ std::filesystem::is_regular_file(pathToInfoJson))
{
std::ifstream ifs(pathToInfoJson);
- std::string file_content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
+ std::string file_content((std::istreambuf_iterator<char>(ifs)),
+ (std::istreambuf_iterator<char>()));
armarx::motion::mdb::arondto::MDBReference motionReference;
auto json = nlohmann::json::parse(file_content);
@@ -34,7 +36,8 @@ namespace armarx::armem::server::motions::mdb::conversion
{
armarx::motion::mdb::arondto::FileReference fileRef;
fileRef.attachedToId = attachedFile["attachedToId"];
- fileRef.createdDate = DateTime(Duration::MicroSeconds(attachedFile["createdDate"]));
+ fileRef.createdDate =
+ DateTime(Duration::MicroSeconds(attachedFile["createdDate"]));
fileRef.createdUser = attachedFile["createdUser"];
fileRef.description = attachedFile["description"];
fileRef.fileName = attachedFile["fileName"];
@@ -49,4 +52,4 @@ namespace armarx::armem::server::motions::mdb::conversion
return std::nullopt;
}
}
-}
+} // namespace armarx::armem::server::motions::mdb::conversion
diff --git a/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.h b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.h
index 146f0c660104e33d94353b3bd9a1a087abef04d6..b425a3ead2e341a2c9f116e2181665c84fed863d 100644
--- a/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.h
+++ b/source/RobotAPI/libraries/armem_motions/server/mdb_conversions.h
@@ -1,7 +1,7 @@
// STD / STL
#include <filesystem>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <optional>
// ArmarX
@@ -9,5 +9,6 @@
namespace armarx::armem::server::motions::mdb::conversion
{
- std::optional<armarx::motion::mdb::arondto::MDBReference> createFromFile(const std::filesystem::path& pathToInfoJson);
+ std::optional<armarx::motion::mdb::arondto::MDBReference>
+ createFromFile(const std::filesystem::path& pathToInfoJson);
}
diff --git a/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp b/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp
index 82d3369f3a18f88ec384a5f04677eb6deaea6d95..115719af64b4570a32443da153fb598182a63d42 100644
--- a/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp
+++ b/source/RobotAPI/libraries/armem_mps/StatechartListener.cpp
@@ -1,46 +1,54 @@
#include "StatechartListener.h"
-
namespace armarx
{
- void StatechartListener::setName(const std::string& name)
+ void
+ StatechartListener::setName(const std::string& name)
{
armarx::Component::setName(name);
}
- void StatechartListener::setTopicName(const std::string& name)
+ void
+ StatechartListener::setTopicName(const std::string& name)
{
this->topicName = name;
}
- std::string StatechartListener::getTopicName() const
+ std::string
+ StatechartListener::getTopicName() const
{
return topicName;
}
StatechartListener::~StatechartListener() = default;
- std::string StatechartListener::getDefaultName() const
+ std::string
+ StatechartListener::getDefaultName() const
{
return "StatechartListener";
}
- void StatechartListener::onInitComponent()
+ void
+ StatechartListener::onInitComponent()
{
ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
usingTopic(topicName);
}
- void StatechartListener::onConnectComponent()
+
+ void
+ StatechartListener::onConnectComponent()
{
ARMARX_INFO << getName() << "::" << __FUNCTION__ << "()";
}
- void StatechartListener::registerCallback(const StatechartListener::Callback& callback)
+ void
+ StatechartListener::registerCallback(const StatechartListener::Callback& callback)
{
callbacks.push_back(callback);
}
- void StatechartListener::publish(const std::vector<Transition>& message)
+ void
+ StatechartListener::publish(const std::vector<Transition>& message)
{
for (Callback& callback : callbacks)
{
@@ -49,15 +57,18 @@ namespace armarx
}
void
- StatechartListener::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& transition,
- const Ice::Current&)
+ StatechartListener::reportStatechartTransitionWithParameters(
+ const ProfilerStatechartTransitionWithParameters& transition,
+ const Ice::Current&)
{
publish({transition});
}
- void StatechartListener::reportStatechartTransitionWithParametersList(
- const ProfilerStatechartTransitionWithParametersList& transitions, const Ice::Current&)
+ void
+ StatechartListener::reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList& transitions,
+ const Ice::Current&)
{
publish(transitions);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_mps/StatechartListener.h b/source/RobotAPI/libraries/armem_mps/StatechartListener.h
index a0308773c3de18f0cced7f23325bab208e081898..dc32da6b2a9bf5a94cceb942b9dbfc462a680e8a 100644
--- a/source/RobotAPI/libraries/armem_mps/StatechartListener.h
+++ b/source/RobotAPI/libraries/armem_mps/StatechartListener.h
@@ -2,20 +2,20 @@
#include <ArmarXCore/core/Component.h>
-
#include <ArmarXCore/interface/core/Profiler.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-
namespace armarx
{
class StatechartListener :
- virtual public armarx::Component
- , virtual public armarx::ProfilerListener
+ virtual public armarx::Component,
+ virtual public armarx::ProfilerListener
{
public:
using Transition = armarx::ProfilerStatechartTransitionWithParameters;
- using Callback = std::function<void(const std::vector<StatechartListener::Transition>& transitions, StatechartListener& source)>;
+ using Callback =
+ std::function<void(const std::vector<StatechartListener::Transition>& transitions,
+ StatechartListener& source)>;
public:
~StatechartListener() override;
@@ -35,25 +35,100 @@ namespace armarx
// ProfilerListener interface
public:
- void reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&, const Ice::Current&) override;
- void reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList&, const Ice::Current&) override;
-
- void reportNetworkTraffic(const std::string&, const std::string&, Ice::Int, Ice::Int, const Ice::Current&) override {}
- void reportEvent(const ProfilerEvent&, const Ice::Current&) override {}
- void reportStatechartTransition(const ProfilerStatechartTransition& event, const Ice::Current&) override {}
- void reportStatechartInputParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
- void reportStatechartLocalParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
- void reportStatechartOutputParameters(const ProfilerStatechartParameters&, const Ice::Current&) override {}
- void reportProcessCpuUsage(const ProfilerProcessCpuUsage&, const Ice::Current&) override {}
- void reportProcessMemoryUsage(const ProfilerProcessMemoryUsage&, const Ice::Current&) override {}
-
- void reportEventList(const ProfilerEventList& events, const Ice::Current&) override {}
- void reportStatechartTransitionList(const ProfilerStatechartTransitionList&, const Ice::Current&) override {}
- void reportStatechartInputParametersList(const ProfilerStatechartParametersList& data, const Ice::Current&) override {}
- void reportStatechartLocalParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
- void reportStatechartOutputParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
- void reportProcessCpuUsageList(const ProfilerProcessCpuUsageList&, const Ice::Current&) override {}
- void reportProcessMemoryUsageList(const ProfilerProcessMemoryUsageList&, const Ice::Current&) override {}
+ void
+ reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&,
+ const Ice::Current&) override;
+ void reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList&,
+ const Ice::Current&) override;
+
+ void
+ reportNetworkTraffic(const std::string&,
+ const std::string&,
+ Ice::Int,
+ Ice::Int,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportEvent(const ProfilerEvent&, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartTransition(const ProfilerStatechartTransition& event,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartInputParameters(const ProfilerStatechartParameters& event,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartLocalParameters(const ProfilerStatechartParameters& event,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartOutputParameters(const ProfilerStatechartParameters&,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportProcessCpuUsage(const ProfilerProcessCpuUsage&, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportProcessMemoryUsage(const ProfilerProcessMemoryUsage&, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportEventList(const ProfilerEventList& events, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartTransitionList(const ProfilerStatechartTransitionList&,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartInputParametersList(const ProfilerStatechartParametersList& data,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartLocalParametersList(const ProfilerStatechartParametersList&,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartOutputParametersList(const ProfilerStatechartParametersList&,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportProcessCpuUsageList(const ProfilerProcessCpuUsageList&, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportProcessMemoryUsageList(const ProfilerProcessMemoryUsageList&,
+ const Ice::Current&) override
+ {
+ }
private:
@@ -62,4 +137,4 @@ namespace armarx
std::vector<Callback> callbacks;
void publish(const std::vector<Transition>& message);
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp b/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
index 4451bc8353fcee85342cf20e996a5376d887b4c8..070b4440e5c970fd6a5e9abd64cbc8160f06e68b 100644
--- a/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
@@ -1,70 +1,89 @@
#include "aron_conversions.h"
+
+#include <VirtualRobot/MathTools.h>
+
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
#include <dmp/representation/dmp/umitsmp.h>
-#include <VirtualRobot/MathTools.h>
namespace armarx::armem
{
-void fromAron(const arondto::Trajectory &dto, DMP::SampledTrajectoryV2 &bo, bool taskspace)
-{
- std::map<double, DMP::DVec> traj_map;
- if(taskspace){
- for(auto element : dto.taskSpace.steps){
- DMP::DVec pose;
- pose.push_back(element.pose(0,3));
- pose.push_back(element.pose(1,3));
- pose.push_back(element.pose(2,3));
- VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(element.pose);
- pose.push_back(quat.w);
- pose.push_back(quat.x);
- pose.push_back(quat.y);
- pose.push_back(quat.z);
- traj_map.insert(std::make_pair(element.timestep, pose));
+ void
+ fromAron(const arondto::Trajectory& dto, DMP::SampledTrajectoryV2& bo, bool taskspace)
+ {
+ std::map<double, DMP::DVec> traj_map;
+ if (taskspace)
+ {
+ for (auto element : dto.taskSpace.steps)
+ {
+ DMP::DVec pose;
+ pose.push_back(element.pose(0, 3));
+ pose.push_back(element.pose(1, 3));
+ pose.push_back(element.pose(2, 3));
+ VirtualRobot::MathTools::Quaternion quat =
+ VirtualRobot::MathTools::eigen4f2quat(element.pose);
+ pose.push_back(quat.w);
+ pose.push_back(quat.x);
+ pose.push_back(quat.y);
+ pose.push_back(quat.z);
+ traj_map.insert(std::make_pair(element.timestep, pose));
+ }
}
-
- }else{
- for(auto element : dto.jointSpace.steps){
- DMP::DVec jointvalues;
- for(auto angle: element.jointValues){
- jointvalues.push_back(double(angle));
+ else
+ {
+ for (auto element : dto.jointSpace.steps)
+ {
+ DMP::DVec jointvalues;
+ for (auto angle : element.jointValues)
+ {
+ jointvalues.push_back(double(angle));
+ }
+ traj_map.insert(std::make_pair(element.timestep, jointvalues));
}
- traj_map.insert(std::make_pair(element.timestep, jointvalues));
}
}
-}
-
-void toAron(arondto::Trajectory &dto, const DMP::SampledTrajectoryV2 &bo_taskspace, const DMP::SampledTrajectoryV2 &bo_jointspace, const std::string name)
-{
- dto.name = name;
- std::map<std::string, std::vector<float>> mapJointSpace;
- // taskspace
- std::map<double, DMP::DVec> ts_map = bo_taskspace.getPositionData();
- for(std::pair<double, DMP::DVec> element: ts_map){
- Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
- Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3));
- poseMatrix.block<3, 1>(0, 3) = vec;
- arondto::TSElement tselement;
- tselement.timestep = element.first;
- tselement.pose = poseMatrix;
- dto.taskSpace.steps.push_back(tselement);
+ void
+ toAron(arondto::Trajectory& dto,
+ const DMP::SampledTrajectoryV2& bo_taskspace,
+ const DMP::SampledTrajectoryV2& bo_jointspace,
+ const std::string name)
+ {
+ dto.name = name;
+ std::map<std::string, std::vector<float>> mapJointSpace;
- }
+ // taskspace
+ std::map<double, DMP::DVec> ts_map = bo_taskspace.getPositionData();
+ for (std::pair<double, DMP::DVec> element : ts_map)
+ {
+ Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
+ Eigen::Matrix<float, 4, 4> poseMatrix =
+ VirtualRobot::MathTools::quat2eigen4f(element.second.at(4),
+ element.second.at(5),
+ element.second.at(6),
+ element.second.at(3));
+ poseMatrix.block<3, 1>(0, 3) = vec;
+ arondto::TSElement tselement;
+ tselement.timestep = element.first;
+ tselement.pose = poseMatrix;
+ dto.taskSpace.steps.push_back(tselement);
+ }
- // jointspace
- std::map<double, DMP::DVec> js_map = bo_jointspace.getPositionData();
- for(std::pair<double, DMP::DVec> element: js_map){
- std::vector<float> configvec;
- for(double el: element.second){
- configvec.push_back(float(el));
+ // jointspace
+ std::map<double, DMP::DVec> js_map = bo_jointspace.getPositionData();
+ for (std::pair<double, DMP::DVec> element : js_map)
+ {
+ std::vector<float> configvec;
+ for (double el : element.second)
+ {
+ configvec.push_back(float(el));
+ }
+ arondto::JSElement jselement;
+ jselement.timestep = element.first;
+ jselement.jointValues = configvec;
+ dto.jointSpace.steps.push_back(jselement);
}
- arondto::JSElement jselement;
- jselement.timestep = element.first;
- jselement.jointValues = configvec;
- dto.jointSpace.steps.push_back(jselement);
}
-}
-
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.h b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
index 343975886fdf6b6041189500ae40b5662701c5a4..f4438ae82512727a346e33f8dc34e0886b46d819 100644
--- a/source/RobotAPI/libraries/armem_mps/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
@@ -6,12 +6,17 @@
#include <RobotAPI/libraries/aron/common/aron/trajectory.aron.generated.h>
#include <dmp/representation/trajectory.h>
+
//#include <dmp
namespace armarx
{
- void fromAron(const armarx::arondto::Trajectory& dto, DMP::SampledTrajectoryV2& bo, bool taskspace);
- void toAron(armarx::arondto::Trajectory& dto, const DMP::SampledTrajectoryV2& bo_taskspace, const DMP::SampledTrajectoryV2& bo_jointspace, const std::string name);
+ void
+ fromAron(const armarx::arondto::Trajectory& dto, DMP::SampledTrajectoryV2& bo, bool taskspace);
+ void toAron(armarx::arondto::Trajectory& dto,
+ const DMP::SampledTrajectoryV2& bo_taskspace,
+ const DMP::SampledTrajectoryV2& bo_jointspace,
+ const std::string name);
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
index 23b68572384fb24b7b4f28729fe1041f60d8c307..4e32728501810628ce8a07d4d6567ed1de15a048 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.cpp
@@ -2,21 +2,20 @@
#include "Segment.h"
// ArmarX
-#include "motionprimitives.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
+#include "motionprimitives.h"
// STD / STL
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
-
namespace armarx::armem::server::motions::mps::segment
{
MPSegment::MPSegment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
@@ -24,15 +23,21 @@ namespace armarx::armem::server::motions::mps::segment
{
}
- void MPSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ MPSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
Base::defineProperties(defs, prefix);
- defs->optional(properties.motionsPackage, prefix + "MotionsPackage", "Name of the prior knowledge package to load from.");
- defs->optional(properties.loadFromMotionsPackage, prefix + "LoadFromMotionsPackage", "If true, load the motions from the motions package on startup.");
+ defs->optional(properties.motionsPackage,
+ prefix + "MotionsPackage",
+ "Name of the prior knowledge package to load from.");
+ defs->optional(properties.loadFromMotionsPackage,
+ prefix + "LoadFromMotionsPackage",
+ "If true, load the motions from the motions package on startup.");
}
- void MPSegment::init()
+ void
+ MPSegment::init()
{
Base::init();
@@ -42,12 +47,13 @@ namespace armarx::armem::server::motions::mps::segment
}
}
- void MPSegment::onConnect()
+ void
+ MPSegment::onConnect()
{
-
}
- int MPSegment::loadByMotionFinder(const std::string& packageName)
+ int
+ MPSegment::loadByMotionFinder(const std::string& packageName)
{
priorknowledge::motions::MotionFinder motionFinder(packageName, "motions/");
int loadedMotions = 0;
@@ -56,14 +62,17 @@ namespace armarx::armem::server::motions::mps::segment
auto allMotions = motionFinder.findAll("trajectories");
for (const auto& motionFinderInfo : allMotions)
{
- auto pathToInfoJson = motionFinderInfo.getFullPath() / motionFinderInfo.getID();// / (motionFinderInfo.getID() + ".csv"); // todo: needs to be adapted, account for task and joint space
- for (const auto & entry: std::filesystem::directory_iterator(pathToInfoJson))
+ auto pathToInfoJson =
+ motionFinderInfo.getFullPath() /
+ motionFinderInfo
+ .getID(); // / (motionFinderInfo.getID() + ".csv"); // todo: needs to be adapted, account for task and joint space
+ for (const auto& entry : std::filesystem::directory_iterator(pathToInfoJson))
{
if (std::string(entry.path().filename()).rfind("taskspace", 0) == 0)
{
- //ARMARX_IMPORTANT << entry.path().filename();
- loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), true);
- loadedMotions += allMotions.size();
+ //ARMARX_IMPORTANT << entry.path().filename();
+ loadSingleMotionFinder(entry.path(), motionFinderInfo.getID(), true);
+ loadedMotions += allMotions.size();
}
/*else if (std::string(entry.path().filename()).rfind("joint-trajectory", 0) == 0)
{
@@ -79,7 +88,10 @@ namespace armarx::armem::server::motions::mps::segment
return loadedMotions;
}
- void MPSegment::loadSingleMotionFinder(const std::string &pathToInfoJson, const std::string &entityName, bool taskspace)
+ void
+ MPSegment::loadSingleMotionFinder(const std::string& pathToInfoJson,
+ const std::string& entityName,
+ bool taskspace)
{
if (auto op = util::createFromFile(pathToInfoJson, taskspace); op.has_value())
{
@@ -87,20 +99,22 @@ namespace armarx::armem::server::motions::mps::segment
ss << "Found valid instance at: " << pathToInfoJson << ". The motionID is: ";
armem::wm::EntityInstance instance;
- instance.metadata().referencedTime = armem::Time::Now(); //op->createdDate;
+ instance.metadata().referencedTime = armem::Time::Now(); //op->createdDate;
instance.metadata().sentTime = armem::Time::Now();
instance.metadata().arrivedTime = armem::Time::Now();
instance.metadata().confidence = 1.0;
- if(taskspace)
+ if (taskspace)
{
std::filesystem::path path(pathToInfoJson);
- for (const auto & entry: std::filesystem::directory_iterator(path.parent_path()))
+ for (const auto& entry : std::filesystem::directory_iterator(path.parent_path()))
{
- std::string newname = "joint-trajectory" + std::string(path.filename()).erase(0, 20);
+ std::string newname =
+ "joint-trajectory" + std::string(path.filename()).erase(0, 20);
if (std::string(entry.path().filename()).rfind(newname, 0) == 0)
{
- if (auto op2 = util::createFromFile(entry.path(), false); op.has_value()) // here now mps::createFromFile(pathToInfoJson)
+ if (auto op2 = util::createFromFile(entry.path(), false);
+ op.has_value()) // here now mps::createFromFile(pathToInfoJson)
{
op->jointSpace = op2->jointSpace;
instance.data() = op->toAron();
@@ -124,7 +138,7 @@ namespace armarx::armem::server::motions::mps::segment
}
else
{
- ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
+ ARMARX_WARNING << "Found an invalid path to a motion file: " << pathToInfoJson;
}
}
-}
+} // namespace armarx::armem::server::motions::mps::segment
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
index f38c3c114b49e33b7f411450d11f0996f8ac5eb8..750c3577dc6127faccdb726fa6913361fe238ecb 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/Segment.h
@@ -19,13 +19,15 @@ namespace armarx::armem::server::motions::mps::segment
public:
MPSegment(armem::server::MemoryToIceAdapter& iceMemory);
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
virtual void init() override;
virtual void onConnect();
private:
int loadByMotionFinder(const std::string&);
- void loadSingleMotionFinder(const std::string&, const std::string &entityName, bool taskspace);
+ void
+ loadSingleMotionFinder(const std::string&, const std::string& entityName, bool taskspace);
private:
struct Properties
@@ -33,6 +35,7 @@ namespace armarx::armem::server::motions::mps::segment
std::string motionsPackage = "PriorKnowledgeData";
bool loadFromMotionsPackage = true;
};
+
Properties properties;
};
-}
+} // namespace armarx::armem::server::motions::mps::segment
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
index 32f0302a2b33486f6c4336b278059c6db5cf9208..eddc1fcb5ce49d144bdbc3f44b6074d21e15a462 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.cpp
@@ -4,92 +4,94 @@
// #include <fstream>
#include <SimoxUtility/algorithm/string.h>
+#include <VirtualRobot/MathTools.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <dmp/representation/trajectory.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <VirtualRobot/MathTools.h>
-
namespace armarx::armem::server::motions::mps::segment::util
{
-std::optional<armarx::arondto::Trajectory> createFromFile(const std::filesystem::__cxx11::path &pathToInfoJson, bool taskspace)
-{
-
- if (std::filesystem::exists(pathToInfoJson) && std::filesystem::is_regular_file(pathToInfoJson))
+ std::optional<armarx::arondto::Trajectory>
+ createFromFile(const std::filesystem::__cxx11::path& pathToInfoJson, bool taskspace)
{
- DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
- DMP::SampledTrajectoryV2 traj;
- std::string absPath;
- ArmarXDataPath::getAbsolutePath(pathToInfoJson, absPath);
- traj.readFromCSVFile(absPath);
-
- //traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
- std::map<double, DMP::DVec> currentTraj = traj.getPositionData();//todo
- trajs.push_back(traj);
- armarx::arondto::Trajectory trajectory;
- std::string name = pathToInfoJson.filename();
- std::string toErase = "taskspace-trajectory-";
- size_t pos = name.find(toErase);
- if (pos != std::string::npos)
+ if (std::filesystem::exists(pathToInfoJson) &&
+ std::filesystem::is_regular_file(pathToInfoJson))
{
- name.erase(pos, toErase.length());
- }
- trajectory.name = name;
- std::map<std::string, std::vector<float>> mapJointSpace;
- for(DMP::SampledTrajectoryV2 traj: trajs)
- {
- std::map<double, DMP::DVec> currentTraj = traj.getPositionData(); // todo: add config making data structure clear
-
- if(taskspace)
- {
- for(std::pair<double, DMP::DVec> element: currentTraj)
- {
- Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
- Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3));
+ DMP::Vec<DMP::SampledTrajectoryV2> trajs;
+ DMP::SampledTrajectoryV2 traj;
+ std::string absPath;
+ ArmarXDataPath::getAbsolutePath(pathToInfoJson, absPath);
+ traj.readFromCSVFile(absPath);
+
+ //traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
+ std::map<double, DMP::DVec> currentTraj = traj.getPositionData(); //todo
+ trajs.push_back(traj);
+
+ armarx::arondto::Trajectory trajectory;
+ std::string name = pathToInfoJson.filename();
+ std::string toErase = "taskspace-trajectory-";
+ size_t pos = name.find(toErase);
+ if (pos != std::string::npos)
+ {
+ name.erase(pos, toErase.length());
+ }
+ trajectory.name = name;
+ std::map<std::string, std::vector<float>> mapJointSpace;
+ for (DMP::SampledTrajectoryV2 traj : trajs)
+ {
+ std::map<double, DMP::DVec> currentTraj =
+ traj.getPositionData(); // todo: add config making data structure clear
+
+ if (taskspace)
+ {
+ for (std::pair<double, DMP::DVec> element : currentTraj)
+ {
+ Eigen::Vector3f vec(
+ element.second.at(0), element.second.at(1), element.second.at(2));
+ Eigen::Matrix<float, 4, 4> poseMatrix =
+ VirtualRobot::MathTools::quat2eigen4f(element.second.at(4),
+ element.second.at(5),
+ element.second.at(6),
+ element.second.at(3));
poseMatrix.block<3, 1>(0, 3) = vec;
armarx::arondto::TSElement tselement;
tselement.timestep = element.first;
tselement.pose = poseMatrix;
trajectory.taskSpace.steps.push_back(tselement);
-
}
-
-
- }
- else
- {
- for(std::pair<double, DMP::DVec> element: currentTraj)
- {
- std::vector<float> configvec;
- for(double el: element.second)
- {
- configvec.push_back(float(el));
+ }
+ else
+ {
+ for (std::pair<double, DMP::DVec> element : currentTraj)
+ {
+ std::vector<float> configvec;
+ for (double el : element.second)
+ {
+ configvec.push_back(float(el));
+ }
+ armarx::arondto::JSElement jselement;
+ jselement.timestep = element.first;
+ jselement.jointValues = configvec;
+ trajectory.jointSpace.steps.push_back(jselement);
}
- armarx::arondto::JSElement jselement;
- jselement.timestep = element.first;
- jselement.jointValues = configvec;
- trajectory.jointSpace.steps.push_back(jselement);
}
}
+ return trajectory;
+ }
+ else
+ {
+ return std::nullopt;
}
-
- return trajectory;
- }
- else
- {
- return std::nullopt;
}
-}
-
-}
+} // namespace armarx::armem::server::motions::mps::segment::util
diff --git a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
index 72372ca2fa79a39e1a8ed022f4104627353f9727..49c4590401b480246963dd293bd6538a1933243f 100644
--- a/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
+++ b/source/RobotAPI/libraries/armem_mps/server/MotionPrimitives/motionprimitives.h
@@ -2,8 +2,8 @@
#define MOTIONPRIMITIVES_H
#include <filesystem>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <optional>
// ArmarX
@@ -12,7 +12,8 @@
namespace armarx::armem::server::motions::mps::segment::util
{
- std::optional<armarx::arondto::Trajectory> createFromFile(const std::filesystem::path& pathToInfoJson, bool taskspace);
+ std::optional<armarx::arondto::Trajectory>
+ createFromFile(const std::filesystem::path& pathToInfoJson, bool taskspace);
}
#endif // MOTIONPRIMITIVES_H
diff --git a/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp b/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp
index 274cdc091e561f32ecaf5b16f729a2982752690c..021ff6a89d318df909edd79b02536b2f111b0a5e 100644
--- a/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_mps/traj_conversions.cpp
@@ -1,10 +1,7 @@
#include "traj_conversions.h"
- /* std::optional<DMP::SampledTrajectoryV2> mps::convertTrajectory(const armem_mps::arondto::Trajectory)
+/* std::optional<DMP::SampledTrajectoryV2> mps::convertTrajectory(const armem_mps::arondto::Trajectory)
{
}*/
-
-
-
diff --git a/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h b/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
index 6a57255f1019c433f87d7f1f66faa3919a9087c8..cedab6159e37f91834d9bdeb12fac890914117fa 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_forward_declarations.h
@@ -1,10 +1,8 @@
#pragma once
-
namespace armarx::armem::arondto
{
class ObjectClass;
class ObjectInstance;
class Marker;
-}
-
+} // namespace armarx::armem::arondto
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index f0ec3136995b80a3a8efedb5132e73df04801849..5864efbaf09d35f6f9c5686ad903397303bf5353 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -69,7 +69,8 @@ namespace armarx::armem::articulated_object
if (not objectState)
{
ARMARX_VERBOSE << deactivateSpam(5) << "Querying object state failed for object `"
- << object.getName() << "` " << "(type `" << object.getType() << "`)!";
+ << object.getName() << "` "
+ << "(type `" << object.getType() << "`)!";
return false;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
index 9f69b93009bed3d41c34e0d99d2952443e399671..fdb948721af1d837b83285170a6c943d6024eb4b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -3,9 +3,9 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
index 4afc5a0196a9715d7cb7efd14d3b4c88e81ccf2b..d2ac94c2e16aee6efcdf5f41d115167d252fb292 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
@@ -1,10 +1,10 @@
#include "utils.h"
-#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
-
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+
namespace armarx::armem::articulated_object
{
std::optional<robot_state::description::RobotDescription>
@@ -34,5 +34,5 @@ namespace armarx::armem::articulated_object
return robotDescription;
}
-
+
} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
index c724a3fd78bff4674428dcd4e4d21ac69ffff9a9..60aaedbdfe70184f60acc2c2e5b4a3de1df69b70 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.cpp
@@ -3,12 +3,12 @@
#include <mutex>
#include <optional>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
@@ -18,7 +18,6 @@
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
namespace armarx::armem::attachment
{
@@ -26,51 +25,55 @@ namespace armarx::armem::attachment
{
template <typename AronClass, typename ArmemClass>
- auto getAttachments(const armarx::armem::wm::Memory& memory)
+ auto
+ getAttachments(const armarx::armem::wm::Memory& memory)
{
// using ArmemClass = decltype(fromAron(AronClass()));
using ArmemClassVector = std::vector<ArmemClass>;
ArmemClassVector attachments;
- memory.forEachEntity([&attachments](const wm::Entity & entity)
- {
- if (entity.empty())
+ memory.forEachEntity(
+ [&attachments](const wm::Entity& entity)
{
- ARMARX_WARNING << "No entity snapshot found";
- return true;
- }
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity snapshot found";
+ return true;
+ }
- const armem::wm::EntityInstance& instance = entity.getFirstSnapshot().getInstance(0);
- try
- {
- AronClass aronAttachment;
- aronAttachment.fromAron(instance.data());
+ const armem::wm::EntityInstance& instance =
+ entity.getFirstSnapshot().getInstance(0);
+ try
+ {
+ AronClass aronAttachment;
+ aronAttachment.fromAron(instance.data());
- ArmemClass attachment;
- fromAron(aronAttachment, attachment);
+ ArmemClass attachment;
+ fromAron(aronAttachment, attachment);
- if (attachment.active)
+ if (attachment.active)
+ {
+ attachments.push_back(attachment);
+ }
+ }
+ catch (const armarx::aron::error::AronException&)
{
- attachments.push_back(attachment);
+ return true;
}
-
- }
- catch (const armarx::aron::error::AronException&)
- {
return true;
- }
- return true;
- });
+ });
return attachments;
}
- } // namespace
+ } // namespace
Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
- {}
+ {
+ }
- void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
@@ -83,8 +86,8 @@ namespace armarx::armem::attachment
"Name of the memory core segment to use for object attachments.");
}
-
- void Reader::connect()
+ void
+ Reader::connect()
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
@@ -100,8 +103,8 @@ namespace armarx::armem::attachment
}
}
-
- std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp) const
+ std::vector<ObjectAttachment>
+ Reader::queryObjectAttachments(const armem::Time& timestamp) const
{
// Query all entities from all provider.
armem::client::query::Builder qb;
@@ -123,12 +126,13 @@ namespace armarx::armem::attachment
return {};
}
- return getAttachments <
- ::armarx::armem::arondto::attachment::ObjectAttachment,
- ::armarx::armem::attachment::ObjectAttachment > (qResult.memory);
+ return getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment,
+ ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
}
- std::vector<ObjectAttachment> Reader::queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
+ std::vector<ObjectAttachment>
+ Reader::queryObjectAttachments(const armem::Time& timestamp,
+ const std::string& providerName) const
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -150,10 +154,12 @@ namespace armarx::armem::attachment
return {};
}
- return getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment, ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
+ return getAttachments<::armarx::armem::arondto::attachment::ObjectAttachment,
+ ::armarx::armem::attachment::ObjectAttachment>(qResult.memory);
}
- std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp) const
+ std::vector<ArticulatedObjectAttachment>
+ Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp) const
{
// Query all entities from all provider.
armem::client::query::Builder qb;
@@ -175,10 +181,14 @@ namespace armarx::armem::attachment
return {};
}
- return getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
+ return getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment,
+ ::armarx::armem::attachment::ArticulatedObjectAttachment>(
+ qResult.memory);
}
- std::vector<ArticulatedObjectAttachment> Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const
+ std::vector<ArticulatedObjectAttachment>
+ Reader::queryArticulatedObjectAttachments(const armem::Time& timestamp,
+ const std::string& providerName) const
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -200,9 +210,10 @@ namespace armarx::armem::attachment
return {};
}
- return getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment, ::armarx::armem::attachment::ArticulatedObjectAttachment>(qResult.memory);
+ return getAttachments<::armarx::armem::arondto::attachment::ArticulatedObjectAttachment,
+ ::armarx::armem::attachment::ArticulatedObjectAttachment>(
+ qResult.memory);
}
-
-} // namespace armarx::armem::attachment
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
index 1080156c8bbbdb95afc62c0467d53dbe700c49f9..06c0c681dfb9a97b315223da475c66843520bba9 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Reader.h
@@ -30,7 +30,6 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem_objects/types.h>
-
namespace armarx::armem::attachment
{
class Reader
@@ -43,18 +42,20 @@ namespace armarx::armem::attachment
void connect();
std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp) const;
- std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const;
+ std::vector<ObjectAttachment> queryObjectAttachments(const armem::Time& timestamp,
+ const std::string& providerName) const;
- std::vector<ArticulatedObjectAttachment> queryArticulatedObjectAttachments(const armem::Time& timestamp) const;
- std::vector<ArticulatedObjectAttachment> queryArticulatedObjectAttachments(const armem::Time& timestamp, const std::string& providerName) const;
+ std::vector<ArticulatedObjectAttachment>
+ queryArticulatedObjectAttachments(const armem::Time& timestamp) const;
+ std::vector<ArticulatedObjectAttachment>
+ queryArticulatedObjectAttachments(const armem::Time& timestamp,
+ const std::string& providerName) const;
private:
-
-
struct Properties
{
- std::string memoryName = "Object";
- std::string coreAttachmentsSegmentName = "Attachments";
+ std::string memoryName = "Object";
+ std::string coreAttachmentsSegmentName = "Attachments";
} properties;
const std::string propertyPrefix = "mem.obj.attachment.";
@@ -65,4 +66,4 @@ namespace armarx::armem::attachment
};
-} // namespace armarx::armem::attachment
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
index 067d53dc8e9bbd237d9d6d8db34f98105d18f8aa..be6a77a258e00abfd195160a0d3779fbfd2ed776 100644
--- a/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/attachment/Writer.h
@@ -22,17 +22,17 @@
#pragma once
#include <mutex>
+
#include <ArmarXCore/core/application/properties/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
// #include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
#include <RobotAPI/libraries/armem_objects/types.h>
-
namespace armarx::armem::attachment
{
@@ -50,14 +50,13 @@ namespace armarx::armem::attachment
std::optional<armem::MemoryID> commit(const ArticulatedObjectAttachment&);
private:
-
struct Properties
{
- std::string memoryName = "Object";
- std::string coreAttachmentsSegmentName = "Attachments";
- std::string providerName = "AttachmentProvider";
+ std::string memoryName = "Object";
+ std::string coreAttachmentsSegmentName = "Attachments";
+ std::string providerName = "AttachmentProvider";
- bool allowClassCreation = false;
+ bool allowClassCreation = false;
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
@@ -72,4 +71,4 @@ namespace armarx::armem::attachment
};
-} // namespace armarx::armem::attachment
+} // namespace armarx::armem::attachment
diff --git a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp
index de1a50c49b2ba21bf256c74dfca3886fa1b12dc9..4fda433d7f8634a37909de6c6c3d7af76af86a5f 100644
--- a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp
@@ -71,7 +71,7 @@ namespace armarx::armem::obj::clazz
ClassReader::getAllObjectClasses()
{
armem::client::query::Builder builder;
-
+
// clang-format off
builder
.coreSegments().withName(properties().coreSegmentName)
diff --git a/source/RobotAPI/libraries/armem_objects/client/class/ClassWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/class/ClassWriter.cpp
index 6c1d36c77eb7360567b11665dd7d530cdbbd191c..77223d2779a3bfae19fb3d851ae9f69558144cc2 100644
--- a/source/RobotAPI/libraries/armem_objects/client/class/ClassWriter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/class/ClassWriter.cpp
@@ -2,13 +2,12 @@
namespace armarx::armem::obj::clazz
{
- ClassWriter::ClassWriter(const std::string& p) :
- providerName(p)
+ ClassWriter::ClassWriter(const std::string& p) : providerName(p)
{
-
}
- void ClassWriter::setProviderName(const std::string& pName)
+ void
+ ClassWriter::setProviderName(const std::string& pName)
{
this->providerName = pName;
}
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
index 05e850869fac0ce879b1a9b186b5acea99f98890..8c72ba71bd3c9edeef8b468049092d3019b3e879 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp
@@ -53,7 +53,6 @@ namespace armarx::armem::obj::instance
this->readingPrx = r.readingPrx;
ARMARX_INFO << "Connected to Memory '" << properties.memoryName << "'";
-
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
index 41769504c5bcf3096371dd7c4da774b3133ff7e6..af9e9cb08c50ad1c4c1fcd3fac318aa43fd193ea 100644
--- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h
@@ -32,9 +32,9 @@
#include <RobotAPI/libraries/armem_objects/types.h>
// The object pose provider. As long as the provider is not connected to armem we need the second proxy
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
namespace armarx::armem::obj::instance
{
diff --git a/source/RobotAPI/libraries/armem_objects/constants.h b/source/RobotAPI/libraries/armem_objects/constants.h
index 517d68bcf18af3a6e2e03a29d76c46d118157acb..61c727498b87537626d45a91748ed4237baf2de0 100644
--- a/source/RobotAPI/libraries/armem_objects/constants.h
+++ b/source/RobotAPI/libraries/armem_objects/constants.h
@@ -28,4 +28,4 @@ namespace armarx::armem::objects::constants
inline const std::string CoreFamiliarObjectInstanceSegmentName = "FamiliarObjectInstance";
inline const std::string CoreClassSegmentName = "Class";
-} // namespace armarx::armem_objects::constants
+} // namespace armarx::armem::objects::constants
diff --git a/source/RobotAPI/libraries/armem_objects/memory_ids.cpp b/source/RobotAPI/libraries/armem_objects/memory_ids.cpp
index 0ee0f6c5a9030eaa7e6be4cfd773a25d349bc100..7584603d7eb2549ae8f8a170996d5bbd02aa5dd8 100644
--- a/source/RobotAPI/libraries/armem_objects/memory_ids.cpp
+++ b/source/RobotAPI/libraries/armem_objects/memory_ids.cpp
@@ -22,15 +22,15 @@
#include "memory_ids.h"
-
namespace armarx::armem
{
- const MemoryID objects::memoryID { "Object" };
+ const MemoryID objects::memoryID{"Object"};
const MemoryID objects::attachmentsSegmentID = memoryID.withCoreSegmentName("Attachments");
const MemoryID objects::classSegmentID = memoryID.withCoreSegmentName("Class");
const MemoryID objects::instaceSegmentID = memoryID.withCoreSegmentName("Instance");
- const MemoryID objects::familiarObjectInstaceSegmentID = memoryID.withCoreSegmentName("FamiliarObjectInstance");
+ const MemoryID objects::familiarObjectInstaceSegmentID =
+ memoryID.withCoreSegmentName("FamiliarObjectInstance");
-}
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_objects/memory_ids.h b/source/RobotAPI/libraries/armem_objects/memory_ids.h
index a39fb87ceee2b08de59664fa7f3fe2cbf619e183..d1e1e4db82b4be8e800a7e48098845625f04f0d4 100644
--- a/source/RobotAPI/libraries/armem_objects/memory_ids.h
+++ b/source/RobotAPI/libraries/armem_objects/memory_ids.h
@@ -24,7 +24,6 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
namespace armarx::armem::objects
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
index 03988d61d5d42ecb4c039bbf93ea028f6a1d298b..d44f9306ff537da9ecc4aea70dfa59dcde81ab64 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.cpp
@@ -2,25 +2,22 @@
#include <sstream>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/armem/util/util.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-
-#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::server::obj::attachments
{
@@ -32,21 +29,29 @@ namespace armarx::armem::server::obj::attachments
Segment::~Segment() = default;
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
- defs->optional(p.maxHistorySize, prefix + "MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
+ defs->optional(p.coreSegmentName,
+ prefix + "CoreSegmentName",
+ "Name of the object instance core segment.");
+ defs->optional(p.maxHistorySize,
+ prefix + "MaxHistorySize",
+ "Maximal size of object poses history (-1 for infinite).");
}
- void Segment::init()
+ void
+ Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::Robot::ToAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName,
+ arondto::Robot::ToAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
- void Segment::connect()
+ void
+ Segment::connect()
{
}
@@ -54,31 +59,37 @@ namespace armarx::armem::server::obj::attachments
Segment::getAttachments(const armem::Time& timestamp) const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
- return coreSegment->doLocked([this]()
- {
- std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
- coreSegment->forEachEntity([this, &attachments](const wm::Entity & entity)
+ return coreSegment->doLocked(
+ [this]()
{
- const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
-
- const auto aronAttachment = tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(entityInstance);
- if (not aronAttachment)
- {
- ARMARX_WARNING << "Could not convert entity instance to 'ObjectAttachment'";
- return true;
- }
-
- ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
-
- armarx::armem::attachment::ObjectAttachment attachment;
- fromAron(*aronAttachment, attachment);
-
- attachments.push_back(attachment);
- return true;
+ std::vector<armarx::armem::attachment::ObjectAttachment> attachments;
+ coreSegment->forEachEntity(
+ [this, &attachments](const wm::Entity& entity)
+ {
+ const wm::EntityInstance& entityInstance =
+ entity.getLatestSnapshot().getInstance(0);
+
+ const auto aronAttachment =
+ tryCast<armarx::armem::arondto::attachment::ObjectAttachment>(
+ entityInstance);
+ if (not aronAttachment)
+ {
+ ARMARX_WARNING
+ << "Could not convert entity instance to 'ObjectAttachment'";
+ return true;
+ }
+
+ ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+
+ armarx::armem::attachment::ObjectAttachment attachment;
+ fromAron(*aronAttachment, attachment);
+
+ attachments.push_back(attachment);
+ return true;
+ });
+
+ return attachments;
});
-
- return attachments;
- });
}
-} // namespace armarx::armem::server::obj::attachments
+} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
index 9f4636fa9484d8c9d4fdd4d0a825442f06340637..e42069c71af3ffc19e2cf1e26a8c69132d619648 100644
--- a/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/attachments/Segment.h
@@ -21,17 +21,16 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-#include <RobotAPI/libraries/armem/server/forward_declarations.h>
-#include <RobotAPI/libraries/armem_objects/types.h>
-#include <RobotAPI/libraries/armem_objects/memory_ids.h>
+#include <string>
#include <ArmarXCore/core/application/properties/forward_declarations.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <string>
-
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/server/forward_declarations.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem::server::obj::attachments
{
@@ -39,7 +38,6 @@ namespace armarx::armem::server::obj::attachments
class Segment : public armarx::Logging
{
public:
-
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment();
@@ -48,11 +46,11 @@ namespace armarx::armem::server::obj::attachments
void init();
void connect();
- std::vector<armarx::armem::attachment::ObjectAttachment> getAttachments(const armem::Time& timestamp) const;
+ std::vector<armarx::armem::attachment::ObjectAttachment>
+ getAttachments(const armem::Time& timestamp) const;
private:
-
server::MemoryToIceAdapter& iceMemory;
wm::CoreSegment* coreSegment = nullptr;
@@ -61,8 +59,8 @@ namespace armarx::armem::server::obj::attachments
std::string coreSegmentName = objects::attachmentsSegmentID.coreSegmentName;
int64_t maxHistorySize = -1;
};
- Properties p;
+ Properties p;
};
-} // namespace armarx::armem::server::obj::attachments
+} // namespace armarx::armem::server::obj::attachments
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
index f0be74213236e8cc13ea2eb0716bcdff98065ff8..f8ee63f22c2a77ddf8e95c141a09bff4f66e2766 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.cpp
@@ -1,12 +1,11 @@
#include "FloorVis.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-
namespace armarx::armem::server::obj::clazz
{
@@ -14,20 +13,20 @@ namespace armarx::armem::server::obj::clazz
{
}
-
- void FloorVis::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ FloorVis::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
properties.define(defs, prefix);
}
-
- void FloorVis::setArViz(armarx::viz::Client arviz)
+ void
+ FloorVis::setArViz(armarx::viz::Client arviz)
{
this->arviz = arviz;
}
-
- void FloorVis::updateFloorObject(const wm::CoreSegment& classCoreSegment)
+ void
+ FloorVis::updateFloorObject(const wm::CoreSegment& classCoreSegment)
{
viz::Layer layer = arviz.layer(properties.layerName);
if (properties.show)
@@ -46,8 +45,8 @@ namespace armarx::armem::server::obj::clazz
arviz.commit({layer});
}
-
- armarx::viz::Object FloorVis::makeFloorObject(const wm::Entity& classEntity)
+ armarx::viz::Object
+ FloorVis::makeFloorObject(const wm::Entity& classEntity)
{
const wm::EntityInstance& instance = classEntity.getLatestSnapshot().getInstance(0);
arondto::ObjectClass data;
@@ -55,28 +54,26 @@ namespace armarx::armem::server::obj::clazz
return makeFloorObject(classEntity.name(), data);
}
-
- armarx::viz::Object FloorVis::makeFloorObject(
- const std::string& name,
- const arondto::ObjectClass& objectClass)
+ armarx::viz::Object
+ FloorVis::makeFloorObject(const std::string& name, const arondto::ObjectClass& objectClass)
{
ARMARX_TRACE;
return armarx::viz::Object(name)
- .file(objectClass.simoxXmlPath.package, objectClass.simoxXmlPath.path)
- .position(Eigen::Vector3f(0, 0, properties.height));
+ .file(objectClass.simoxXmlPath.package, objectClass.simoxXmlPath.path)
+ .position(Eigen::Vector3f(0, 0, properties.height));
}
-
- void FloorVis::Properties::define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ FloorVis::Properties::define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(show, prefix + "Show", "Whether to show the floor.");
- defs->optional(entityName, prefix + "EntityName", "Object class entity of the floor.");
- defs->optional(layerName, prefix + "LayerName", "Layer to draw the floor on.");
- defs->optional(height, prefix + "Height",
- "Height (z) of the floor plane. \n"
- "Set slightly below 0 to avoid z-fighting when drawing planes on the ground.");
+ defs->optional(entityName, prefix + "EntityName", "Object class entity of the floor.");
+ defs->optional(layerName, prefix + "LayerName", "Layer to draw the floor on.");
+ defs->optional(
+ height,
+ prefix + "Height",
+ "Height (z) of the floor plane. \n"
+ "Set slightly below 0 to avoid z-fighting when drawing planes on the ground.");
}
-}
-
-
+} // namespace armarx::armem::server::obj::clazz
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
index 49f8a6ddd71f5c24172ddfbbfd8e312dcea2fd3f..ed54fb2cf09b3ae1c619b24dd32b0bda3edd0b5d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
@@ -5,23 +5,22 @@
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/armem_objects/aron_forward_declarations.h>
-
namespace armarx
{
using PropertyDefinitionsPtr = IceUtil::Handle<class PropertyDefinitionContainer>;
}
+
namespace armarx::armem::server::wm
{
class CoreSegment;
class Entity;
-}
+} // namespace armarx::armem::server::wm
namespace armarx::armem::server::obj::clazz
{
class FloorVis
{
public:
-
FloorVis();
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
@@ -32,11 +31,11 @@ namespace armarx::armem::server::obj::clazz
void updateFloorObject(const wm::CoreSegment& classCoreSegment);
armarx::viz::Object makeFloorObject(const wm::Entity& classEntity);
- armarx::viz::Object makeFloorObject(const std::string& name, const arondto::ObjectClass& objectClass);
+ armarx::viz::Object makeFloorObject(const std::string& name,
+ const arondto::ObjectClass& objectClass);
public:
-
struct Properties
{
bool show = true;
@@ -49,10 +48,8 @@ namespace armarx::armem::server::obj::clazz
};
private:
-
Properties properties;
armarx::viz::Client arviz;
-
};
-}
+} // namespace armarx::armem::server::obj::clazz
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index b2da5cd1b44faf2382c3a31eba5fcf9c3486029a..23bc3eda5442e0dd6c8cf36d390647834f94f46f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -6,30 +6,29 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
#include <RobotAPI/libraries/armem_objects/server/class/FloorVis.h>
-
namespace armarx::armem::arondto
{
class ObjectClass;
}
+
namespace armarx::armem::server::obj::clazz
{
class Segment : public segment::SpecializedCoreSegment
{
public:
-
Segment(armem::server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
void init() override;
void connect(viz::Client arviz);
@@ -47,23 +46,21 @@ namespace armarx::armem::server::obj::clazz
private:
-
ObjectFinder objectFinder;
viz::Client arviz;
FloorVis floorVis;
-
struct Properties
{
std::string objectsPackage = ObjectFinder::DefaultObjectsPackageName;
bool loadFromObjectsPackage = true;
};
+
Properties p;
public:
-
struct RemoteGui
{
armarx::RemoteGui::Client::GroupBox group;
@@ -81,6 +78,7 @@ namespace armarx::armem::server::obj::clazz
bool rebuild = false;
};
+
Data data;
struct Visu
@@ -94,13 +92,12 @@ namespace armarx::armem::server::obj::clazz
void setup(const Segment& segment);
void update(Segment& segment);
};
+
Visu visu;
void setup(const Segment& segment);
void update(Segment& segment);
-
};
-
};
-}
+} // namespace armarx::armem::server::obj::clazz
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.cpp
index a24a023ec12f5edf595769949b1b5403dcdf8608..dd79ac8a5e3af56c644abda61f9252e7c8646bdc 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.cpp
@@ -2,30 +2,32 @@
#include <SimoxUtility/math/scale_value.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/DateTime.h>
-
+#include <ArmarXCore/core/time/TimeUtil.h>
namespace armarx::armem::server::obj::familiar_object_instance
{
- void Decay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Decay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(enabled, prefix + "enabled",
+ defs->optional(enabled,
+ prefix + "enabled",
"If true, object poses decay over time when not localized anymore.");
- defs->optional(delaySeconds, prefix + "delaySeconds",
+ defs->optional(delaySeconds,
+ prefix + "delaySeconds",
"Duration after latest localization before decay starts.");
- defs->optional(durationSeconds, prefix + "durationSeconds",
- "How long to reach minimal confidence.");
- defs->optional(maxConfidence, prefix + "maxConfidence",
- "Confidence when decay starts.");
- defs->optional(minConfidence, prefix + "minConfidence",
- "Confidence after decay duration.");
- defs->optional(removeObjectsBelowConfidence, prefix + "removeObjectsBelowConfidence",
+ defs->optional(
+ durationSeconds, prefix + "durationSeconds", "How long to reach minimal confidence.");
+ defs->optional(maxConfidence, prefix + "maxConfidence", "Confidence when decay starts.");
+ defs->optional(minConfidence, prefix + "minConfidence", "Confidence after decay duration.");
+ defs->optional(removeObjectsBelowConfidence,
+ prefix + "removeObjectsBelowConfidence",
"Remove objects whose confidence is lower than this value.");
}
- void Decay::updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const
+ void
+ Decay::updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const
{
if (pose.attachment or pose.isStatic)
{
@@ -37,7 +39,8 @@ namespace armarx::armem::server::obj::familiar_object_instance
}
}
- void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const
+ void
+ Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const
{
for (objpose::ObjectPose& pose : objectPoses)
{
@@ -45,7 +48,8 @@ namespace armarx::armem::server::obj::familiar_object_instance
}
}
- float Decay::calculateConfidence(const DateTime& localization, const DateTime& now) const
+ float
+ Decay::calculateConfidence(const DateTime& localization, const DateTime& now) const
{
const float duration = static_cast<float>((now - localization).toSecondsDouble());
if (duration < delaySeconds)
@@ -58,16 +62,16 @@ namespace armarx::armem::server::obj::familiar_object_instance
}
else
{
- return simox::math::scale_value_from_to(
- duration,
- delaySeconds, delaySeconds + this->durationSeconds,
- maxConfidence, minConfidence);
+ return simox::math::scale_value_from_to(duration,
+ delaySeconds,
+ delaySeconds + this->durationSeconds,
+ maxConfidence,
+ minConfidence);
}
}
-
-
- void Decay::RemoteGui::setup(const Decay& decay)
+ void
+ Decay::RemoteGui::setup(const Decay& decay)
{
using namespace armarx::RemoteGui::Client;
@@ -76,14 +80,14 @@ namespace armarx::armem::server::obj::familiar_object_instance
float max = 1e6;
delaySeconds.setRange(0, max);
delaySeconds.setDecimals(2);
- delaySeconds.setSteps(int(10 * max)); // = 0.1 steps
+ delaySeconds.setSteps(int(10 * max)); // = 0.1 steps
delaySeconds.setValue(decay.delaySeconds);
}
{
float max = 1e6;
durationSeconds.setRange(0, max);
durationSeconds.setDecimals(2);
- durationSeconds.setSteps(int(10 * max)); // = 0.1 steps
+ durationSeconds.setSteps(int(10 * max)); // = 0.1 steps
durationSeconds.setValue(decay.durationSeconds);
}
{
@@ -114,14 +118,16 @@ namespace armarx::armem::server::obj::familiar_object_instance
row++;
grid.add(Label("Min Confidence"), {row, 0}).add(minConfidence, {row, 1});
row++;
- grid.add(Label("Remove Objects with Confidence < "), {row, 0}).add(removeObjectsBelowConfidence, {row, 1});
+ grid.add(Label("Remove Objects with Confidence < "), {row, 0})
+ .add(removeObjectsBelowConfidence, {row, 1});
row++;
group.setLabel("Decay");
group.addChild(grid);
}
- void Decay::RemoteGui::update(Decay& decay)
+ void
+ Decay::RemoteGui::update(Decay& decay)
{
decay.enabled = enabled.getValue();
decay.delaySeconds = delaySeconds.getValue();
@@ -131,4 +137,4 @@ namespace armarx::armem::server::obj::familiar_object_instance
decay.removeObjectsBelowConfidence = removeObjectsBelowConfidence.getValue();
}
-}
+} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.h
index 6fc4967cf94bff14ac0ff2d0c576ab130525575f..313230646d867f623b6e17d8b4ddf1593dd96b52 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.h
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Decay.h
@@ -8,7 +8,6 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
namespace armarx::armem::server::obj::familiar_object_instance
{
@@ -19,19 +18,17 @@ namespace armarx::armem::server::obj::familiar_object_instance
class Decay : public armarx::Logging
{
public:
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "decay.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "decay.");
void updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const;
void updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const;
private:
-
float calculateConfidence(const DateTime& localization, const DateTime& now) const;
public:
-
bool enabled = false;
/// Duration after latest localization before decay starts.
@@ -44,7 +41,6 @@ namespace armarx::armem::server::obj::familiar_object_instance
float removeObjectsBelowConfidence = 0.1f;
-
struct RemoteGui
{
armarx::RemoteGui::Client::GroupBox group;
@@ -61,7 +57,6 @@ namespace armarx::armem::server::obj::familiar_object_instance
void setup(const Decay& decay);
void update(Decay& decay);
};
-
};
-}
+} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
index fcb2a94487dc7034ce7a02c24478a35890e5ff77..733ac2352137ff9b33179c62b93d01f5118836fe 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
@@ -142,7 +142,6 @@ namespace armarx::armem::server::obj::familiar_object_instance
iceMemory.commit(commit);
}
-
std::vector<armarx::armem::arondto::FamiliarObjectInstance>
Segment::getFamiliarObjects(const DateTime& now)
{
@@ -200,7 +199,6 @@ namespace armarx::armem::server::obj::familiar_object_instance
return familiarObjectsByProvider;
}
-
VirtualRobot::RobotPtr
Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h
index f5fa90ad48d7b7a8d5529fc7b46e5d876f6b845f..7aa727720f591c7d20ffc80b0d97c858b12691c9 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h
@@ -77,7 +77,6 @@ namespace armarx::armem::server::obj::familiar_object_instance
getFamiliarObjectsByProvider(const DateTime& now);
private:
-
friend struct DetachVisitor;
@@ -109,15 +108,12 @@ namespace armarx::armem::server::obj::familiar_object_instance
private:
struct Properties
{
-
};
Properties p;
-
- static const std::string timestampPlaceholder;
-
+ static const std::string timestampPlaceholder;
};
} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.h
index 572939d96701734be8d972a0b31ecfaa918aac6d..a88d96c658cbe948d665ae052bca800c60f53518 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.h
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.h
@@ -59,10 +59,11 @@ namespace armarx::armem::server::obj::familiar_object_instance
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
void init();
- void connect(std::experimental::observer_ptr<robot_state::VirtualRobotReader> virtualRobotReader,
- // KinematicUnitObserverInterfacePrx kinematicUnitObserver,
- viz::Client arviz,
- DebugObserverInterfacePrx debugObserver);
+ void
+ connect(std::experimental::observer_ptr<robot_state::VirtualRobotReader> virtualRobotReader,
+ // KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver);
// ObjectPoseTopic interface
@@ -104,8 +105,6 @@ namespace armarx::armem::server::obj::familiar_object_instance
familiar_object_instance::Visu visu;
std::mutex visuMutex;
-
-
};
} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
index 84d9e402e1f6816faa9b6a4494d7bf68af9ce2f8..e45d41dd9bc27b4bf6a04d6c009035209f3d963a 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
@@ -47,8 +47,9 @@ namespace armarx::armem::server::obj::familiar_object_instance
defs->optional(objectFrames, prefix + "objectFrames", "Enable showing object frames.");
defs->optional(
objectFramesScale, prefix + "objectFramesScale", "Scaling of object frames.");
- defs->optional(
- visualizationDuration, prefix + "maxAgeSeconds", "Maximum age in seconds for visualization.");
+ defs->optional(visualizationDuration,
+ prefix + "maxAgeSeconds",
+ "Maximum age in seconds for visualization.");
}
void
@@ -118,7 +119,8 @@ namespace armarx::armem::server::obj::familiar_object_instance
// FIXME derive from bounding box
pose.translation().z() += 200;
- const std::string name = familiarObject.objectID.className + "/" + familiarObject.objectID.instanceName;
+ const std::string name = familiarObject.objectID.className + "/" +
+ familiarObject.objectID.instanceName;
layerLabels.add(viz::Text(name).pose(pose).scale(20));
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
index d92017ae1b729eedc20a58ff63b2481df32cd997..94d666f55b62a924aa47edd43cecb134ee9135b6 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
@@ -62,7 +62,6 @@ namespace armarx::armem::server::obj::familiar_object_instance
SimpleRunningTask<>::pointer_type updateTask;
-
};
} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
index a8060557502fa895f8d632288e32a1c3e0abd5f0..5a0fdca816191cf62b81ddd568694cab184a6d7f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
@@ -4,8 +4,8 @@
#include <SimoxUtility/math/pose.h>
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -16,15 +16,17 @@
namespace armarx::armem::server::obj::instance
{
- void ArticulatedObjectVisu::defineProperties(armarx::PropertyDefinitionsPtr defs,
- const std::string& prefix)
+ void
+ ArticulatedObjectVisu::defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
defs->optional(
p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
}
- viz::Layer ArticulatedObjectVisu::visualizeProvider(
+ viz::Layer
+ ArticulatedObjectVisu::visualizeProvider(
const std::string& providerName,
const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
@@ -35,12 +37,13 @@ namespace armarx::armem::server::obj::instance
return layer;
}
- void ArticulatedObjectVisu::visualizeObjects(
+ void
+ ArticulatedObjectVisu::visualizeObjects(
viz::Layer& layer,
const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
const auto visualizeObject =
- [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
+ [&](const armarx::armem::articulated_object::ArticulatedObject& obj)
{
const auto xmlPath = obj.description.xml.serialize();
@@ -60,15 +63,18 @@ namespace armarx::armem::server::obj::instance
std::for_each(objects.begin(), objects.end(), visualizeObject);
}
- void ArticulatedObjectVisu::init()
+ void
+ ArticulatedObjectVisu::init()
{
- updateTask = new PeriodicTask<ArticulatedObjectVisu>(this, &ArticulatedObjectVisu::visualizeRun, 1000 / p.frequencyHz);
+ updateTask = new PeriodicTask<ArticulatedObjectVisu>(
+ this, &ArticulatedObjectVisu::visualizeRun, 1000 / p.frequencyHz);
ARMARX_INFO << "ArticulatedObjectVisu: init";
updateTask->start();
}
- void ArticulatedObjectVisu::visualizeRun()
+ void
+ ArticulatedObjectVisu::visualizeRun()
{
// std::scoped_lock lock(visuMutex);
ARMARX_INFO << "Update task";
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
index 9c66fd7fb613d41e36f65fe1d8273dc8bd8b0e13..49c3077460759b90546793c46ffefa8f4f3073c6 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
@@ -69,7 +69,7 @@ namespace armarx::armem::server::obj::instance
struct Properties
{
- bool enabled = true;
+ bool enabled = true;
float frequencyHz = 25;
} p;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
index ac072e19693c213989cd5f6fc8acfb3de1cc8380..c9b0fdc997f425d83fd872dc9d1ce727ab74688e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
@@ -2,30 +2,32 @@
#include <SimoxUtility/math/scale_value.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/DateTime.h>
-
+#include <ArmarXCore/core/time/TimeUtil.h>
namespace armarx::armem::server::obj::instance
{
- void Decay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Decay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(enabled, prefix + "enabled",
+ defs->optional(enabled,
+ prefix + "enabled",
"If true, object poses decay over time when not localized anymore.");
- defs->optional(delaySeconds, prefix + "delaySeconds",
+ defs->optional(delaySeconds,
+ prefix + "delaySeconds",
"Duration after latest localization before decay starts.");
- defs->optional(durationSeconds, prefix + "durationSeconds",
- "How long to reach minimal confidence.");
- defs->optional(maxConfidence, prefix + "maxConfidence",
- "Confidence when decay starts.");
- defs->optional(minConfidence, prefix + "minConfidence",
- "Confidence after decay duration.");
- defs->optional(removeObjectsBelowConfidence, prefix + "removeObjectsBelowConfidence",
+ defs->optional(
+ durationSeconds, prefix + "durationSeconds", "How long to reach minimal confidence.");
+ defs->optional(maxConfidence, prefix + "maxConfidence", "Confidence when decay starts.");
+ defs->optional(minConfidence, prefix + "minConfidence", "Confidence after decay duration.");
+ defs->optional(removeObjectsBelowConfidence,
+ prefix + "removeObjectsBelowConfidence",
"Remove objects whose confidence is lower than this value.");
}
- void Decay::updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const
+ void
+ Decay::updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const
{
if (pose.attachment or pose.isStatic)
{
@@ -37,7 +39,8 @@ namespace armarx::armem::server::obj::instance
}
}
- void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const
+ void
+ Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const
{
for (objpose::ObjectPose& pose : objectPoses)
{
@@ -45,7 +48,8 @@ namespace armarx::armem::server::obj::instance
}
}
- float Decay::calculateConfidence(const DateTime& localization, const DateTime& now) const
+ float
+ Decay::calculateConfidence(const DateTime& localization, const DateTime& now) const
{
const float duration = static_cast<float>((now - localization).toSecondsDouble());
if (duration < delaySeconds)
@@ -58,16 +62,16 @@ namespace armarx::armem::server::obj::instance
}
else
{
- return simox::math::scale_value_from_to(
- duration,
- delaySeconds, delaySeconds + this->durationSeconds,
- maxConfidence, minConfidence);
+ return simox::math::scale_value_from_to(duration,
+ delaySeconds,
+ delaySeconds + this->durationSeconds,
+ maxConfidence,
+ minConfidence);
}
}
-
-
- void Decay::RemoteGui::setup(const Decay& decay)
+ void
+ Decay::RemoteGui::setup(const Decay& decay)
{
using namespace armarx::RemoteGui::Client;
@@ -76,14 +80,14 @@ namespace armarx::armem::server::obj::instance
float max = 1e6;
delaySeconds.setRange(0, max);
delaySeconds.setDecimals(2);
- delaySeconds.setSteps(int(10 * max)); // = 0.1 steps
+ delaySeconds.setSteps(int(10 * max)); // = 0.1 steps
delaySeconds.setValue(decay.delaySeconds);
}
{
float max = 1e6;
durationSeconds.setRange(0, max);
durationSeconds.setDecimals(2);
- durationSeconds.setSteps(int(10 * max)); // = 0.1 steps
+ durationSeconds.setSteps(int(10 * max)); // = 0.1 steps
durationSeconds.setValue(decay.durationSeconds);
}
{
@@ -114,14 +118,16 @@ namespace armarx::armem::server::obj::instance
row++;
grid.add(Label("Min Confidence"), {row, 0}).add(minConfidence, {row, 1});
row++;
- grid.add(Label("Remove Objects with Confidence < "), {row, 0}).add(removeObjectsBelowConfidence, {row, 1});
+ grid.add(Label("Remove Objects with Confidence < "), {row, 0})
+ .add(removeObjectsBelowConfidence, {row, 1});
row++;
group.setLabel("Decay");
group.addChild(grid);
}
- void Decay::RemoteGui::update(Decay& decay)
+ void
+ Decay::RemoteGui::update(Decay& decay)
{
decay.enabled = enabled.getValue();
decay.delaySeconds = delaySeconds.getValue();
@@ -131,4 +137,4 @@ namespace armarx::armem::server::obj::instance
decay.removeObjectsBelowConfidence = removeObjectsBelowConfidence.getValue();
}
-}
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
index 4adf757da588b0c0525a526e70ccd707f95de9a2..37125ff70f483e0e3c21fcd57c6cf1b937077c41 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.h
@@ -8,7 +8,6 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
namespace armarx::armem::server::obj::instance
{
@@ -19,19 +18,17 @@ namespace armarx::armem::server::obj::instance
class Decay : public armarx::Logging
{
public:
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "decay.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "decay.");
void updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const;
void updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const;
private:
-
float calculateConfidence(const DateTime& localization, const DateTime& now) const;
public:
-
bool enabled = false;
/// Duration after latest localization before decay starts.
@@ -44,7 +41,6 @@ namespace armarx::armem::server::obj::instance
float removeObjectsBelowConfidence = 0.1f;
-
struct RemoteGui
{
armarx::RemoteGui::Client::GroupBox group;
@@ -61,7 +57,6 @@ namespace armarx::armem::server::obj::instance
void setup(const Decay& decay);
void update(Decay& decay);
};
-
};
-}
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
index d1a6c91672c595f2ceba9fa5840f30347d586fff..eda2107079ccc98547792de6bb26c51a31d153cb 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.cpp
@@ -4,21 +4,27 @@
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-
namespace armarx::armem::server::obj::instance
{
- void RobotHeadMovement::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ RobotHeadMovement::defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
- defs->optional(checkHeadVelocity, prefix + "checkHeadVelocity",
- "If true, check whether the head is moving and discard updates in the meantime.");
- defs->optional(maxJointVelocity, prefix + "maxJointVelocity",
+ defs->optional(
+ checkHeadVelocity,
+ prefix + "checkHeadVelocity",
+ "If true, check whether the head is moving and discard updates in the meantime.");
+ defs->optional(maxJointVelocity,
+ prefix + "maxJointVelocity",
"If a head joint's velocity is higher, the head is considered moving.");
- defs->optional(discardIntervalAfterMoveMS, prefix + "discardIntervalAfterMoveMS",
+ defs->optional(discardIntervalAfterMoveMS,
+ prefix + "discardIntervalAfterMoveMS",
"For how long new updates are ignored after moving the head.");
}
- void RobotHeadMovement::fetchDatafields()
+ void
+ RobotHeadMovement::fetchDatafields()
{
if (kinematicUnitObserver)
{
@@ -27,7 +33,8 @@ namespace armarx::armem::server::obj::instance
std::string error = "";
try
{
- DatafieldRefBasePtr datafield = kinematicUnitObserver->getDatafieldRefByName(jointVelocitiesChannelName, jointName);
+ DatafieldRefBasePtr datafield = kinematicUnitObserver->getDatafieldRefByName(
+ jointVelocitiesChannelName, jointName);
DatafieldRefPtr casted = DatafieldRefPtr::dynamicCast(datafield);
if (casted)
{
@@ -44,18 +51,21 @@ namespace armarx::armem::server::obj::instance
}
if (error.size() > 0)
{
- ARMARX_WARNING << "Could not get datafield for joint '" << jointName << "' in channel '" << jointVelocitiesChannelName << "': \n "
+ ARMARX_WARNING << "Could not get datafield for joint '" << jointName
+ << "' in channel '" << jointVelocitiesChannelName << "': \n "
<< error;
}
}
}
else
{
- ARMARX_INFO << "Cannot fetch joint velocity datafields because there is no kinematic unit observer.";
+ ARMARX_INFO << "Cannot fetch joint velocity datafields because there is no kinematic "
+ "unit observer.";
}
}
- bool RobotHeadMovement::isMoving() const
+ bool
+ RobotHeadMovement::isMoving() const
{
for (DatafieldRefPtr df : jointVelocitiesDatafields)
{
@@ -72,19 +82,26 @@ namespace armarx::armem::server::obj::instance
return false;
}
- void RobotHeadMovement::movementStarts(long discardIntervalMs)
+ void
+ RobotHeadMovement::movementStarts(long discardIntervalMs)
{
return movementStarts(Duration::MilliSeconds(discardIntervalMs));
}
- void RobotHeadMovement::movementStarts(const Duration& discardInterval)
+
+ void
+ RobotHeadMovement::movementStarts(const Duration& discardInterval)
{
discardUpdatesUntil = DateTime::Now() + discardInterval;
}
- void RobotHeadMovement::movementStops(long discardIntervalMs)
+
+ void
+ RobotHeadMovement::movementStops(long discardIntervalMs)
{
return movementStops(Duration::MilliSeconds(discardIntervalMs));
}
- void RobotHeadMovement::movementStops(const Duration& discardInterval)
+
+ void
+ RobotHeadMovement::movementStops(const Duration& discardInterval)
{
if (discardInterval.toMilliSeconds() < 0)
{
@@ -98,15 +115,16 @@ namespace armarx::armem::server::obj::instance
}
}
- objpose::SignalHeadMovementOutput RobotHeadMovement::signalHeadMovement(const objpose::SignalHeadMovementInput& input)
+ objpose::SignalHeadMovementOutput
+ RobotHeadMovement::signalHeadMovement(const objpose::SignalHeadMovementInput& input)
{
objpose::SignalHeadMovementOutput output;
switch (input.action)
{
case objpose::HeadMovementAction::HeadMovementAction_Starting:
this->movementStarts(input.discardUpdatesIntervalMilliSeconds < 0
- ? this->discardIntervalAfterMoveMS
- : input.discardUpdatesIntervalMilliSeconds);
+ ? this->discardIntervalAfterMoveMS
+ : input.discardUpdatesIntervalMilliSeconds);
break;
case objpose::HeadMovementAction::HeadMovementAction_Stopping:
this->movementStops(input.discardUpdatesIntervalMilliSeconds);
@@ -115,12 +133,13 @@ namespace armarx::armem::server::obj::instance
ARMARX_UNEXPECTED_ENUM_VALUE(objpose::HeadMovementAction, input.action);
break;
}
- output.discardUpdatesUntilMilliSeconds = this->discardUpdatesUntil.toMilliSecondsSinceEpoch();
+ output.discardUpdatesUntilMilliSeconds =
+ this->discardUpdatesUntil.toMilliSecondsSinceEpoch();
return output;
}
-
- RobotHeadMovement::Discard RobotHeadMovement::getDiscard()
+ RobotHeadMovement::Discard
+ RobotHeadMovement::getDiscard()
{
Discard discard;
if (checkHeadVelocity)
@@ -144,8 +163,8 @@ namespace armarx::armem::server::obj::instance
return discard;
}
-
- void RobotHeadMovement::RemoteGui::setup(const RobotHeadMovement& rhm)
+ void
+ RobotHeadMovement::RemoteGui::setup(const RobotHeadMovement& rhm)
{
using namespace armarx::RemoteGui::Client;
@@ -154,7 +173,7 @@ namespace armarx::armem::server::obj::instance
float max = 10.0;
maxJointVelocity.setRange(0, max);
maxJointVelocity.setDecimals(3);
- maxJointVelocity.setSteps(int(100 * max)); // = 0.01 steps
+ maxJointVelocity.setSteps(int(100 * max)); // = 0.01 steps
maxJointVelocity.setValue(rhm.maxJointVelocity);
}
{
@@ -169,18 +188,20 @@ namespace armarx::armem::server::obj::instance
row++;
grid.add(Label("Max Joint Velocity"), {row, 0}).add(maxJointVelocity, {row, 1});
row++;
- grid.add(Label("Discard Interval after Move [ms]"), {row, 0}).add(discardIntervalAfterMoveMS, {row, 1});
+ grid.add(Label("Discard Interval after Move [ms]"), {row, 0})
+ .add(discardIntervalAfterMoveMS, {row, 1});
row++;
group.setLabel("Robot Head Movement");
group.addChild(grid);
}
- void RobotHeadMovement::RemoteGui::update(RobotHeadMovement& rhm)
+ void
+ RobotHeadMovement::RemoteGui::update(RobotHeadMovement& rhm)
{
rhm.checkHeadVelocity = checkHeadVelocity.getValue();
rhm.maxJointVelocity = maxJointVelocity.getValue();
rhm.discardIntervalAfterMoveMS = discardIntervalAfterMoveMS.getValue();
}
-}
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
index b4da82e8239494f191cad7a1da79c39585f4b13d..969f17654c8d395b35448e975d10db033c93af88 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h
@@ -1,33 +1,32 @@
#pragma once
+#include <optional>
#include <string>
#include <vector>
-#include <optional>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
-#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
-
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
namespace armarx
{
class PropertyDefinitionContainer;
using PropertyDefinitionsPtr = IceUtil::Handle<PropertyDefinitionContainer>;
-}
+} // namespace armarx
namespace armarx::armem::server::obj::instance
{
class RobotHeadMovement : public armarx::Logging
{
public:
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "head.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "head.");
void fetchDatafields();
bool isMoving() const;
@@ -37,19 +36,19 @@ namespace armarx::armem::server::obj::instance
void movementStops(long discardIntervalMs);
void movementStops(const Duration& discardInterval);
- objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input);
-
+ objpose::SignalHeadMovementOutput
+ signalHeadMovement(const objpose::SignalHeadMovementInput& input);
struct Discard
{
std::optional<DateTime> updatesUntil;
bool all = false;
};
+
Discard getDiscard();
public:
-
bool checkHeadVelocity = true;
std::string jointVelocitiesChannelName = "jointvelocities";
@@ -63,7 +62,6 @@ namespace armarx::armem::server::obj::instance
DebugObserverInterfacePrx debugObserver;
-
struct RemoteGui
{
armarx::RemoteGui::Client::GroupBox group;
@@ -75,7 +73,6 @@ namespace armarx::armem::server::obj::instance
void setup(const RobotHeadMovement& rhm);
void update(RobotHeadMovement& rhm);
};
-
};
-}
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 5f0487883fcff6ddb80b6eabf74126d1d491f345..64de69af2462c1828fcbf67e3050baedfb4de155 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -246,11 +246,11 @@ namespace armarx::armem::server::obj::instance
p += sizeof(struct inotify_event) + event->len;
const bool lockMemory = true;
- commitSceneSnapshotFromFilename(scene, lockMemory);
+ commitSceneSnapshotFromFilename(scene, lockMemory);
}
}
});
-
+
fileWatcherTask->start();
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 458117502eca9380eb5fc93903b5b6ed04b13adc..af1d9772a553d5e040dee9a5148c0cb44f8e3bb2 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -7,6 +7,7 @@
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
+
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 818a04491c9d6f3d3d520c4f64d8ffdc40011d7c..7474e7b1d701f3c2cc9a2e8c720a56321690b6a8 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -22,13 +22,8 @@
#include "SegmentAdapter.h"
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/aron_conversions.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <VirtualRobot/Robot.h>
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
@@ -37,27 +32,29 @@
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/Variant.h>
-#include <VirtualRobot/Robot.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/aron_conversions.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
namespace armarx::armem::server::obj::instance
{
- SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory) :
- segment(iceMemory)
+ SegmentAdapter::SegmentAdapter(MemoryToIceAdapter& iceMemory) : segment(iceMemory)
{
}
-
- std::string SegmentAdapter::getName() const
+ std::string
+ SegmentAdapter::getName() const
{
return Logging::tag.tagName;
}
-
- void SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ SegmentAdapter::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
calibration.defineProperties(defs, prefix + "calibration.");
segment.defineProperties(defs, prefix);
@@ -65,8 +62,8 @@ namespace armarx::armem::server::obj::instance
visu.defineProperties(defs, prefix + "visu.");
}
-
- void SegmentAdapter::init()
+ void
+ SegmentAdapter::init()
{
segment.setTag(getName());
segment.decay.setTag(getName());
@@ -77,13 +74,11 @@ namespace armarx::armem::server::obj::instance
segment.init();
}
-
- void SegmentAdapter::connect(
- robot_state::VirtualRobotReader* virtualRobotReader,
- KinematicUnitObserverInterfacePrx kinematicUnitObserver,
- viz::Client arviz,
- DebugObserverInterfacePrx debugObserver
- )
+ void
+ SegmentAdapter::connect(robot_state::VirtualRobotReader* virtualRobotReader,
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver)
{
this->debugObserver = debugObserver;
this->arviz = arviz;
@@ -98,57 +93,61 @@ namespace armarx::armem::server::obj::instance
if (!visu.updateTask)
{
- visu.updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
+ visu.updateTask = new SimpleRunningTask<>([this]() { this->visualizeRun(); });
visu.updateTask->start();
}
segment.connect(arviz);
}
-
- void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
+ void
+ SegmentAdapter::reportProviderAvailable(const std::string& providerName,
+ const objpose::ProviderInfo& info,
+ const Ice::Current&)
{
- ARMARX_IMPORTANT << "Provider `" << providerName << "` is (now) available.";
+ ARMARX_IMPORTANT << "Provider `" << providerName << "` is (now) available.";
updateProviderInfo(providerName, info);
}
-
- void SegmentAdapter::reportObjectPoses(
- const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
+ void
+ SegmentAdapter::reportObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ const Ice::Current&)
{
- ARMARX_VERBOSE << "Received " << providedPoses.size() << " object poses from provider '" << providerName << "'.";
+ ARMARX_VERBOSE << "Received " << providedPoses.size() << " object poses from provider '"
+ << providerName << "'.";
updateObjectPoses(providerName, providedPoses);
}
-
- void SegmentAdapter::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
+ void
+ SegmentAdapter::updateProviderInfo(const std::string& providerName,
+ const objpose::ProviderInfo& info)
{
if (!info.proxy)
{
ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
- << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
+ << "with invalid provider proxy.\nIgnoring provider '" << providerName
+ << "'.";
return;
}
- segment.doLocked([this, &providerName, &info]()
- {
- std::stringstream ss;
- for (const auto& id : info.supportedObjects)
+ segment.doLocked(
+ [this, &providerName, &info]()
{
- ss << "- " << id << "\n";
- }
- ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
- << "Supported objects: \n" << ss.str();
- segment.providers[providerName] = info;
- });
+ std::stringstream ss;
+ for (const auto& id : info.supportedObjects)
+ {
+ ss << "- " << id << "\n";
+ }
+ ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
+ << "Supported objects: \n"
+ << ss.str();
+ segment.providers[providerName] = info;
+ });
}
-
- void SegmentAdapter::updateObjectPoses(
- const std::string& providerName,
- const objpose::data::ProvidedObjectPoseSeq& providedPoses)
+ void
+ SegmentAdapter::updateObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses)
{
TIMING_START(tReportObjectPoses);
@@ -173,40 +172,45 @@ namespace armarx::armem::server::obj::instance
return;
}
- segment.doLocked([&]()
- {
- const auto timestamp = armarx::Clock::Now();
+ segment.doLocked(
+ [&]()
+ {
+ const auto timestamp = armarx::Clock::Now();
- TIMING_START(tCommitObjectPoses);
- Segment::CommitStats stats =
- segment.commitObjectPoses(providerName, providedPoses, calibration, discard.updatesUntil);
- TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
+ TIMING_START(tCommitObjectPoses);
+ Segment::CommitStats stats = segment.commitObjectPoses(
+ providerName, providedPoses, calibration, discard.updatesUntil);
+ TIMING_END_STREAM(tCommitObjectPoses, ARMARX_VERBOSE);
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
+ if (debugObserver)
{
- { "Discarding All Updates", new Variant(discard.all ? 1 : 0) },
- { "Proportion Updated Poses", new Variant(static_cast<float>(stats.numUpdated) / providedPoses.size()) }
- });
- }
+ debugObserver->setDebugChannel(
+ getName(),
+ {{"Discarding All Updates", new Variant(discard.all ? 1 : 0)},
+ {"Proportion Updated Poses",
+ new Variant(static_cast<float>(stats.numUpdated) /
+ providedPoses.size())}});
+ }
- handleProviderUpdate(providerName);
+ handleProviderUpdate(providerName);
- TIMING_END_STREAM(tReportObjectPoses, ARMARX_VERBOSE);
- if (debugObserver)
- {
- debugObserver->setDebugChannel(getName(),
+ TIMING_END_STREAM(tReportObjectPoses, ARMARX_VERBOSE);
+ if (debugObserver)
{
- { "t ReportObjectPoses [ms]", new Variant(tReportObjectPoses.toMilliSecondsDouble()) },
- { "t MemorySetObjectPoses [ms]", new Variant(tCommitObjectPoses.toMilliSecondsDouble()) },
- });
- }
- });
+ debugObserver->setDebugChannel(
+ getName(),
+ {
+ {"t ReportObjectPoses [ms]",
+ new Variant(tReportObjectPoses.toMilliSecondsDouble())},
+ {"t MemorySetObjectPoses [ms]",
+ new Variant(tCommitObjectPoses.toMilliSecondsDouble())},
+ });
+ }
+ });
}
-
- void SegmentAdapter::handleProviderUpdate(const std::string& providerName)
+ void
+ SegmentAdapter::handleProviderUpdate(const std::string& providerName)
{
// Initialized to 0 on first access.
if (segment.providers.count(providerName) == 0)
@@ -215,40 +219,41 @@ namespace armarx::armem::server::obj::instance
}
}
-
- objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPoses(const Ice::Current&)
+ objpose::data::ObjectPoseSeq
+ SegmentAdapter::getObjectPoses(const Ice::Current&)
{
TIMING_START(tGetObjectPoses);
const Time now = Time::Now();
- const objpose::ObjectPoseSeq objectPoses = segment.doLocked([this, &now]()
- {
- return simox::alg::get_values(segment.getObjectPoses(now));
- });
+ const objpose::ObjectPoseSeq objectPoses = segment.doLocked(
+ [this, &now]() { return simox::alg::get_values(segment.getObjectPoses(now)); });
const objpose::data::ObjectPoseSeq result = objpose::toIce(objectPoses);
TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
if (debugObserver)
{
- debugObserver->setDebugChannel(getName(),
- {
- { "t GetObjectPoses() [ms]", new Variant(tGetObjectPoses.toMilliSecondsDouble()) },
- });
+ debugObserver->setDebugChannel(
+ getName(),
+ {
+ {"t GetObjectPoses() [ms]",
+ new Variant(tGetObjectPoses.toMilliSecondsDouble())},
+ });
}
return result;
}
- objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
+ objpose::data::ObjectPoseSeq
+ SegmentAdapter::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
{
TIMING_START(GetObjectPoses);
const Time now = Time::Now();
- const objpose::ObjectPoseSeq objectPoses = segment.doLocked([this, &now, &providerName]()
- {
- return simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now));
- });
+ const objpose::ObjectPoseSeq objectPoses = segment.doLocked(
+ [this, &now, &providerName]() {
+ return simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now));
+ });
const objpose::data::ObjectPoseSeq result = objpose::toIce(objectPoses);
TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
@@ -256,17 +261,18 @@ namespace armarx::armem::server::obj::instance
if (debugObserver)
{
debugObserver->setDebugChannel(getName(),
- {
- { "t GetObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- });
+ {
+ {"t GetObjectPosesByProvider() [ms]",
+ new Variant(GetObjectPoses.toMilliSecondsDouble())},
+ });
}
return result;
}
-
- objpose::observer::RequestObjectsOutput SegmentAdapter::requestObjects(
- const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
+ objpose::observer::RequestObjectsOutput
+ SegmentAdapter::requestObjects(const objpose::observer::RequestObjectsInput& input,
+ const Ice::Current&)
{
ARMARX_INFO << "Requesting objects ...";
@@ -275,7 +281,7 @@ namespace armarx::armem::server::obj::instance
objpose::observer::RequestObjectsOutput output;
- auto updateProxy = [&](const std::string & providerName)
+ auto updateProxy = [&](const std::string& providerName)
{
if (proxies.count(providerName) == 0)
{
@@ -298,33 +304,38 @@ namespace armarx::armem::server::obj::instance
}
else
{
- segment.doLocked([&]()
- {
-
- ARMARX_INFO << "Known providers are: " << simox::alg::get_keys(segment.providers);
-
- for (const auto& objectID : input.request.objectIDs)
+ segment.doLocked(
+ [&]()
{
- bool found = true;
- for (const auto& [providerName, info] : segment.providers)
+ ARMARX_INFO << "Known providers are: "
+ << simox::alg::get_keys(segment.providers);
+
+ for (const auto& objectID : input.request.objectIDs)
{
- // ToDo: optimize look up.
- if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
+ bool found = true;
+ for (const auto& [providerName, info] : segment.providers)
+ {
+ // ToDo: optimize look up.
+ if (std::find(info.supportedObjects.begin(),
+ info.supportedObjects.end(),
+ objectID) != info.supportedObjects.end())
+ {
+ ARMARX_INFO << "Found provider for " << objectID << ": "
+ << providerName;
+ providerRequests[providerName].relativeTimeoutMS =
+ input.request.relativeTimeoutMS;
+ providerRequests[providerName].objectIDs.push_back(objectID);
+ updateProxy(providerName);
+ break;
+ }
+ }
+ if (!found)
{
- ARMARX_INFO << "Found provider for " << objectID << ": " << providerName;
- providerRequests[providerName].relativeTimeoutMS = input.request.relativeTimeoutMS;
- providerRequests[providerName].objectIDs.push_back(objectID);
- updateProxy(providerName);
- break;
+ ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
+ output.results[objectID].providerName = "";
}
}
- if (!found)
- {
- ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
- output.results[objectID].providerName = "";
- }
- }
- });
+ });
}
ARMARX_INFO << VAROUT(simox::alg::get_keys(providerRequests));
@@ -335,7 +346,8 @@ namespace armarx::armem::server::obj::instance
{
ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
<< providerName << "' for " << request.relativeTimeoutMS << " ms.";
- objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
+ objpose::provider::RequestObjectsOutput providerOutput =
+ proxy->requestObjects(request);
int successful = 0;
for (const auto& [objectID, result] : providerOutput.results)
@@ -345,97 +357,84 @@ namespace armarx::armem::server::obj::instance
res.result = result;
successful += int(result.success);
}
- ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
+ ARMARX_INFO << successful << " of " << request.objectIDs.size()
+ << " object requests successful.";
}
}
return output;
}
-
- objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
+ objpose::ProviderInfoMap
+ SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
{
- return segment.doLocked([this]()
- {
- return segment.providers;
- });
+ return segment.doLocked([this]() { return segment.providers; });
}
-
- Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
+ Ice::StringSeq
+ SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
{
- return segment.doLocked([this]()
- {
- return simox::alg::get_keys(segment.providers);
- });
+ return segment.doLocked([this]() { return simox::alg::get_keys(segment.providers); });
}
-
- objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
+ objpose::ProviderInfo
+ SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
return segment.doLocked([this, &providerName]()
- {
- return segment.getProviderInfo(providerName);
- });
+ { return segment.getProviderInfo(providerName); });
}
-
- bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
+ bool
+ SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
{
return segment.doLocked([this, &providerName]()
- {
- return segment.providers.count(providerName) > 0;
- });
+ { return segment.providers.count(providerName) > 0; });
}
-
- objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
- const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
+ objpose::AttachObjectToRobotNodeOutput
+ SegmentAdapter::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input,
+ const Ice::Current&)
{
return segment.doLocked([this, &input]()
- {
- return segment.attachObjectToRobotNode(input);
- });
+ { return segment.attachObjectToRobotNode(input); });
}
-
- objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
- const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
+ objpose::DetachObjectFromRobotNodeOutput
+ SegmentAdapter::detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input,
+ const Ice::Current&)
{
return segment.doLocked([this, &input]()
- {
- return segment.detachObjectFromRobotNode(input);
- });
+ { return segment.detachObjectFromRobotNode(input); });
}
-
- objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
- const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
+ objpose::DetachAllObjectsFromRobotNodesOutput
+ SegmentAdapter::detachAllObjectsFromRobotNodes(
+ const objpose::DetachAllObjectsFromRobotNodesInput& input,
+ const Ice::Current&)
{
return segment.doLocked([this, &input]()
- {
- return segment.detachAllObjectsFromRobotNodes(input);
- });
+ { return segment.detachAllObjectsFromRobotNodes(input); });
}
-
- objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
+ objpose::AgentFramesSeq
+ SegmentAdapter::getAttachableFrames(const Ice::Current&)
{
- return segment.doLocked([this]()
- {
- objpose::AgentFramesSeq output;
- for (const auto& [name, agent] : segment.robots.loaded)
+ return segment.doLocked(
+ [this]()
{
- objpose::AgentFrames& frames = output.emplace_back();
- frames.agent = agent->getName();
- frames.frames = agent->getRobotNodeNames();
- }
- return output;
- });
+ objpose::AgentFramesSeq output;
+ for (const auto& [name, agent] : segment.robots.loaded)
+ {
+ objpose::AgentFrames& frames = output.emplace_back();
+ frames.agent = agent->getName();
+ frames.frames = agent->getRobotNodeNames();
+ }
+ return output;
+ });
}
-
objpose::SignalHeadMovementOutput
- SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
+ SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input,
+ const Ice::Current&)
{
std::scoped_lock lock(robotHeadMutex);
return robotHead.signalHeadMovement(input);
@@ -497,10 +496,10 @@ namespace armarx::armem::server::obj::instance
if (result.success)
{
armem::PredictionSettings settings =
- armem::PredictionSettings::fromIce(request.settings);
+ armem::PredictionSettings::fromIce(request.settings);
- if (settings.predictionEngineID.empty()
- or settings.predictionEngineID == linearPredictionEngineID)
+ if (settings.predictionEngineID.empty() or
+ settings.predictionEngineID == linearPredictionEngineID)
{
result = objpose::predictObjectPoseLinear(
poses.at(i),
@@ -543,39 +542,40 @@ namespace armarx::armem::server::obj::instance
std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>> poseHistories;
visu.minConfidence = -1;
- segment.doLocked([this, &objectPoses, &poseHistories, &objectFinder]()
- {
- const Time now = Time::Now();
-
- // Also include decayed objects in result
- // Store original setting.
- const float minConf = segment.decay.removeObjectsBelowConfidence;
- segment.decay.removeObjectsBelowConfidence = -1;
- // Get result.
- objectPoses = segment.getObjectPoses(now);
- // Restore original setting.
- segment.decay.removeObjectsBelowConfidence = minConf;
-
- objectFinder = segment.objectFinder;
- if (segment.decay.enabled)
+ segment.doLocked(
+ [this, &objectPoses, &poseHistories, &objectFinder]()
{
- visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
- }
+ const Time now = Time::Now();
+
+ // Also include decayed objects in result
+ // Store original setting.
+ const float minConf = segment.decay.removeObjectsBelowConfidence;
+ segment.decay.removeObjectsBelowConfidence = -1;
+ // Get result.
+ objectPoses = segment.getObjectPoses(now);
+ // Restore original setting.
+ segment.decay.removeObjectsBelowConfidence = minConf;
+
+ objectFinder = segment.objectFinder;
+ if (segment.decay.enabled)
+ {
+ visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
+ }
- // Get histories.
- for (const auto& [id, objectPose] : objectPoses)
- {
- const wm::Entity* entity = segment.findObjectEntity(id);
- if (entity != nullptr)
+ // Get histories.
+ for (const auto& [id, objectPose] : objectPoses)
{
- poseHistories[id] = instance::Segment::getObjectPosesInRange(
- *entity,
- Time::Now() - Duration::SecondsDouble(
- visu.linearPredictions.timeWindowSeconds),
- Time::Invalid());
+ const wm::Entity* entity = segment.findObjectEntity(id);
+ if (entity != nullptr)
+ {
+ poseHistories[id] = instance::Segment::getObjectPosesInRange(
+ *entity,
+ Time::Now() - Duration::SecondsDouble(
+ visu.linearPredictions.timeWindowSeconds),
+ Time::Invalid());
+ }
}
- }
- });
+ });
const std::vector<viz::Layer> layers =
visu.visualizeCommit(objectPoses, poseHistories, objectFinder);
@@ -590,7 +590,8 @@ namespace armarx::armem::server::obj::instance
debugValues["t Visualize [ms]"] = new Variant(tVisu.toMilliSecondsDouble());
for (const auto& [id, pose] : objectPoses)
{
- debugValues["confidence(" + id.str() + ")"] = new Variant(pose.confidence);
+ debugValues["confidence(" + id.str() + ")"] =
+ new Variant(pose.confidence);
}
debugObserver->setDebugChannel(getName(), debugValues);
}
@@ -599,13 +600,13 @@ namespace armarx::armem::server::obj::instance
cycle.waitForCycleDuration();
}
}
-
const std::string SegmentAdapter::linearPredictionEngineID = "Linear Position Regression";
- const std::vector<PredictionEngine> SegmentAdapter::predictionEngines{{linearPredictionEngineID}};
-
+ const std::vector<PredictionEngine> SegmentAdapter::predictionEngines{
+ {linearPredictionEngineID}};
- void SegmentAdapter::RemoteGui::setup(const SegmentAdapter& adapter)
+ void
+ SegmentAdapter::RemoteGui::setup(const SegmentAdapter& adapter)
{
using namespace armarx::RemoteGui::Client;
@@ -614,19 +615,19 @@ namespace armarx::armem::server::obj::instance
this->decay.setup(adapter.segment.decay);
this->robotHead.setup(adapter.robotHead);
- layout = VBoxLayout
- {
- this->visu.group, this->segment.group, this->decay.group, this->robotHead.group,
- VSpacer()
- };
+ layout = VBoxLayout{this->visu.group,
+ this->segment.group,
+ this->decay.group,
+ this->robotHead.group,
+ VSpacer()};
group = {};
group.setLabel("Instance");
group.addChild(layout);
}
-
- void SegmentAdapter::RemoteGui::update(SegmentAdapter& adapter)
+ void
+ SegmentAdapter::RemoteGui::update(SegmentAdapter& adapter)
{
// Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
{
@@ -636,9 +637,7 @@ namespace armarx::armem::server::obj::instance
this->segment.update(adapter.segment);
{
adapter.segment.doLocked([this, &adapter]()
- {
- this->decay.update(adapter.segment.decay);
- });
+ { this->decay.update(adapter.segment.decay); });
}
{
std::scoped_lock lock(adapter.robotHeadMutex);
@@ -646,4 +645,4 @@ namespace armarx::armem::server::obj::instance
}
}
-}
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
index 88ab9ca34e6dd554ed536d732e00da71cb7d8ada..af45d9af79ec4fd9a382424c8da038a545346fd7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
@@ -29,21 +29,18 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
-
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem_objects/server/instance/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/instance/Decay.h>
-#include <RobotAPI/libraries/armem_objects/server/instance/Visu.h>
#include <RobotAPI/libraries/armem_objects/server/instance/RobotHeadMovement.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Segment.h>
+#include <RobotAPI/libraries/armem_objects/server/instance/Visu.h>
#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
-
namespace armarx::armem::server::obj::instance
{
@@ -51,61 +48,72 @@ namespace armarx::armem::server::obj::instance
* @brief Helps implementing the `armarx::armem::server::ObjectInstanceSegmentInterface`.
*/
class SegmentAdapter :
- virtual public armarx::Logging
- , virtual public armarx::armem::server::ObjectInstanceSegmentInterface
+ virtual public armarx::Logging,
+ virtual public armarx::armem::server::ObjectInstanceSegmentInterface
{
public:
-
SegmentAdapter(MemoryToIceAdapter& iceMemory);
std::string getName() const;
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
void init();
- void connect(
- robot_state::VirtualRobotReader* virtualRobotReader,
- KinematicUnitObserverInterfacePrx kinematicUnitObserver,
- viz::Client arviz,
- DebugObserverInterfacePrx debugObserver
- );
+ void connect(robot_state::VirtualRobotReader* virtualRobotReader,
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver,
+ viz::Client arviz,
+ DebugObserverInterfacePrx debugObserver);
// ObjectPoseTopic interface
public:
- virtual void reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, ICE_CURRENT_ARG) override;
- virtual void reportObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& objectPoses, ICE_CURRENT_ARG) override;
+ virtual void reportProviderAvailable(const std::string& providerName,
+ const objpose::ProviderInfo& info,
+ ICE_CURRENT_ARG) override;
+ virtual void reportObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& objectPoses,
+ ICE_CURRENT_ARG) override;
// ObjectInstanceSegmentInterface interface
public:
-
// OBJECT POSES
virtual objpose::data::ObjectPoseSeq getObjectPoses(ICE_CURRENT_ARG) override;
- virtual objpose::data::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
+ virtual objpose::data::ObjectPoseSeq
+ getObjectPosesByProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
// PROVIDER INFORMATION
virtual bool hasProvider(const std::string& providerName, ICE_CURRENT_ARG) override;
- virtual objpose::ProviderInfo getProviderInfo(const std::string& providerName, ICE_CURRENT_ARG) override;
+ virtual objpose::ProviderInfo getProviderInfo(const std::string& providerName,
+ ICE_CURRENT_ARG) override;
virtual Ice::StringSeq getAvailableProviderNames(ICE_CURRENT_ARG) override;
virtual objpose::ProviderInfoMap getAvailableProvidersInfo(ICE_CURRENT_ARG) override;
// REQUESTING
- virtual objpose::observer::RequestObjectsOutput requestObjects(const objpose::observer::RequestObjectsInput& input, ICE_CURRENT_ARG) override;
+ virtual objpose::observer::RequestObjectsOutput
+ requestObjects(const objpose::observer::RequestObjectsInput& input,
+ ICE_CURRENT_ARG) override;
// ATTACHING
- virtual objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input, ICE_CURRENT_ARG) override;
- virtual objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input, ICE_CURRENT_ARG) override;
- virtual objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input, ICE_CURRENT_ARG) override;
+ virtual objpose::AttachObjectToRobotNodeOutput
+ attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input,
+ ICE_CURRENT_ARG) override;
+ virtual objpose::DetachObjectFromRobotNodeOutput
+ detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input,
+ ICE_CURRENT_ARG) override;
+ virtual objpose::DetachAllObjectsFromRobotNodesOutput
+ detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input,
+ ICE_CURRENT_ARG) override;
virtual objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
// HEAD MOVEMENT SIGNALS
- virtual objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
+ virtual objpose::SignalHeadMovementOutput
+ signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
// PREDICTING
@@ -118,10 +126,10 @@ namespace armarx::armem::server::obj::instance
private:
-
void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
- void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
+ void updateObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses);
void handleProviderUpdate(const std::string& providerName);
@@ -131,13 +139,11 @@ namespace armarx::armem::server::obj::instance
public:
-
static const std::string linearPredictionEngineID;
static const std::vector<PredictionEngine> predictionEngines;
private:
-
viz::Client arviz;
DebugObserverInterfacePrx debugObserver;
@@ -153,7 +159,6 @@ namespace armarx::armem::server::obj::instance
public:
-
struct RemoteGui
{
armarx::RemoteGui::Client::GroupBox group;
@@ -167,9 +172,8 @@ namespace armarx::armem::server::obj::instance
void setup(const SegmentAdapter& adapter);
void update(SegmentAdapter& adapter);
};
-
};
-}
+} // namespace armarx::armem::server::obj::instance
#undef ICE_CURRENT_ARG
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index b9abf459ec14789b7e4fe3fccf42ba73db0f5eed..0edf905a5b4475877dbaba52f39ddb095df233e0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -3,63 +3,63 @@
#include <SimoxUtility/math/pose.h>
#include <SimoxUtility/math/rescale.h>
-#include <ArmarXCore/core/time/ice_conversions.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
-
namespace armarx::armem::server::obj::instance
{
- void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(enabled, prefix + "enabled",
- "Enable or disable visualization of objects.");
- defs->optional(frequencyHz, prefix + "frequenzyHz",
- "Frequency of visualization.");
- defs->optional(inGlobalFrame, prefix + "inGlobalFrame",
+ defs->optional(enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
+ defs->optional(inGlobalFrame,
+ prefix + "inGlobalFrame",
"If true, show global poses. If false, show poses in robot frame.");
- defs->optional(alpha, prefix + "alpha",
- "Alpha of objects (1 = solid, 0 = transparent).");
- defs->optional(alphaByConfidence, prefix + "alphaByConfidence",
+ defs->optional(alpha, prefix + "alpha", "Alpha of objects (1 = solid, 0 = transparent).");
+ defs->optional(alphaByConfidence,
+ prefix + "alphaByConfidence",
"If true, use the pose confidence as alpha (if < 1.0).");
- defs->optional(oobbs, prefix + "oobbs",
- "Enable showing oriented bounding boxes.");
+ defs->optional(oobbs, prefix + "oobbs", "Enable showing oriented bounding boxes.");
- defs->optional(objectFrames, prefix + "objectFrames",
- "Enable showing object frames.");
- defs->optional(objectFramesScale, prefix + "objectFramesScale",
- "Scaling of object frames.");
+ defs->optional(objectFrames, prefix + "objectFrames", "Enable showing object frames.");
+ defs->optional(
+ objectFramesScale, prefix + "objectFramesScale", "Scaling of object frames.");
gaussians.defineProperties(defs, prefix + "gaussians.");
- defs->optional(useArticulatedModels, prefix + "useArticulatedModels",
+ defs->optional(useArticulatedModels,
+ prefix + "useArticulatedModels",
"Prefer articulated object models if available.");
linearPredictions.defineProperties(defs, prefix + "predictions.linear.");
}
-
- void Visu::Gaussians::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Visu::Gaussians::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(position, prefix + "position",
- "Enable showing pose gaussians (position part).");
- defs->optional(positionScale, prefix + "positionScale",
- "Scaling of pose gaussians (position part).");
-
- defs->optional(orientation, prefix + "position",
- "Enable showing pose gaussians (orientation part).");
- defs->optional(orientationScale, prefix + "positionScale",
+ defs->optional(
+ position, prefix + "position", "Enable showing pose gaussians (position part).");
+ defs->optional(
+ positionScale, prefix + "positionScale", "Scaling of pose gaussians (position part).");
+
+ defs->optional(
+ orientation, prefix + "position", "Enable showing pose gaussians (orientation part).");
+ defs->optional(orientationScale,
+ prefix + "positionScale",
"Scaling of pose gaussians (orientation part).");
- defs->optional(orientationDisplaced, prefix + "positionDisplaced",
- "Displace center orientation (co)variance circle arrows along their rotation axis.");
+ defs->optional(
+ orientationDisplaced,
+ prefix + "positionDisplaced",
+ "Displace center orientation (co)variance circle arrows along their rotation axis.");
}
-
std::vector<viz::Layer>
Visu::visualizeCommit(
const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
@@ -70,7 +70,7 @@ namespace armarx::armem::server::obj::instance
std::vector<viz::Layer> layers;
for (const auto& [name, poses] : objectPoses)
{
- if(name.empty())
+ if (name.empty())
{
continue;
}
@@ -78,7 +78,8 @@ namespace armarx::armem::server::obj::instance
auto poseHistoryMap = poseHistories.find(name);
if (poseHistoryMap != poseHistories.end())
{
- layers.push_back(visualizeProvider(name, poses, poseHistoryMap->second, objectFinder));
+ layers.push_back(
+ visualizeProvider(name, poses, poseHistoryMap->second, objectFinder));
}
else
{
@@ -88,7 +89,6 @@ namespace armarx::armem::server::obj::instance
return layers;
}
-
std::vector<viz::Layer>
Visu::visualizeCommit(const objpose::ObjectPoseSeq& objectPoses,
const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
@@ -97,7 +97,7 @@ namespace armarx::armem::server::obj::instance
std::map<std::string, viz::Layer> stage;
for (size_t i = 0; i < objectPoses.size(); ++i)
{
- if(objectPoses.at(i).providerName.empty())
+ if (objectPoses.at(i).providerName.empty())
{
ARMARX_INFO << "Object pose provider not set!";
continue;
@@ -111,7 +111,6 @@ namespace armarx::armem::server::obj::instance
return simox::alg::get_values(stage);
}
-
std::vector<viz::Layer>
Visu::visualizeCommit(
const objpose::ObjectPoseMap& objectPoses,
@@ -121,7 +120,7 @@ namespace armarx::armem::server::obj::instance
std::map<std::string, viz::Layer> stage;
for (const auto& [id, pose] : objectPoses)
{
-
+
auto poseHistoryMap = poseHistories.find(id);
if (poseHistoryMap != poseHistories.end())
{
@@ -136,10 +135,8 @@ namespace armarx::armem::server::obj::instance
return simox::alg::get_values(stage);
}
-
- viz::Layer& Visu::getLayer(
- const std::string& providerName,
- std::map<std::string, viz::Layer>& stage) const
+ viz::Layer&
+ Visu::getLayer(const std::string& providerName, std::map<std::string, viz::Layer>& stage) const
{
auto it = stage.find(providerName);
if (it == stage.end())
@@ -149,7 +146,6 @@ namespace armarx::armem::server::obj::instance
return it->second;
}
-
viz::Layer
Visu::visualizeProvider(
const std::string& providerName,
@@ -165,11 +161,11 @@ namespace armarx::armem::server::obj::instance
return layer;
}
- void Visu::visualizeObjectPose(
- viz::Layer& layer,
- const objpose::ObjectPose& objectPose,
- const std::map<DateTime, objpose::ObjectPose>& poseHistory,
- const ObjectFinder& objectFinder) const
+ void
+ Visu::visualizeObjectPose(viz::Layer& layer,
+ const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
+ const ObjectFinder& objectFinder) const
{
const bool show = objectPose.confidence >= minConfidence;
if (not show)
@@ -179,7 +175,8 @@ namespace armarx::armem::server::obj::instance
const armarx::ObjectID id = objectPose.objectID;
const std::string key = id.str();
- const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ const Eigen::Matrix4f pose =
+ inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
auto makeObject = [&objectInfo, &id](const std::string& key)
@@ -230,17 +227,17 @@ namespace armarx::armem::server::obj::instance
if (oobbs and objectPose.localOOBB)
{
- const simox::OrientedBoxf oobb = inGlobalFrame
- ? objectPose.oobbGlobal().value()
- : objectPose.oobbRobot().value();
+ const simox::OrientedBoxf oobb =
+ inGlobalFrame ? objectPose.oobbGlobal().value() : objectPose.oobbRobot().value();
layer.add(viz::Box(key + " OOBB").set(oobb).color(simox::Color::lime(255, 64)));
}
if (objectFrames)
{
layer.add(viz::Pose(key + " Pose").pose(pose).scale(objectFramesScale));
}
- if (gaussians.showAny()
- and (inGlobalFrame ? objectPose.objectPoseGlobalGaussian.has_value() : objectPose.objectPoseRobotGaussian.has_value()))
+ if (gaussians.showAny() and
+ (inGlobalFrame ? objectPose.objectPoseGlobalGaussian.has_value()
+ : objectPose.objectPoseRobotGaussian.has_value()))
{
gaussians.draw(layer, objectPose, inGlobalFrame);
}
@@ -250,35 +247,33 @@ namespace armarx::armem::server::obj::instance
}
}
- bool Visu::Gaussians::showAny() const
+ bool
+ Visu::Gaussians::showAny() const
{
return position or orientation;
}
- void Visu::Gaussians::draw(
- viz::Layer& layer,
- const objpose::ObjectPose& objectPose,
- bool inGlobalFrame) const
+ void
+ Visu::Gaussians::draw(viz::Layer& layer,
+ const objpose::ObjectPose& objectPose,
+ bool inGlobalFrame) const
{
const std::string key = objectPose.objectID.str();
const objpose::PoseManifoldGaussian& gaussian =
- inGlobalFrame
- ? objectPose.objectPoseGlobalGaussian.value()
- : objectPose.objectPoseRobotGaussian.value()
- ;
+ inGlobalFrame ? objectPose.objectPoseGlobalGaussian.value()
+ : objectPose.objectPoseRobotGaussian.value();
objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getPositionEllipsoid();
if (position)
{
layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
- .position(ellipsoid.center)
- .orientation(ellipsoid.orientation)
- .axisLengths(positionScale * ellipsoid.size)
- .color(viz::Color::azure(255, 32))
- );
+ .position(ellipsoid.center)
+ .orientation(ellipsoid.orientation)
+ .axisLengths(positionScale * ellipsoid.size)
+ .color(viz::Color::azure(255, 32)));
- if (false) // Arrows can be visualized for debugging.
+ if (false) // Arrows can be visualized for debugging.
{
for (int i = 0; i < 3; ++i)
{
@@ -286,13 +281,11 @@ namespace armarx::armem::server::obj::instance
color(i) = 255;
layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
- .fromTo(ellipsoid.center,
- ellipsoid.center + positionScale * ellipsoid.size(i)
- * ellipsoid.orientation.col(i)
- )
- .width(5)
- .color(simox::Color(color))
- );
+ .fromTo(ellipsoid.center,
+ ellipsoid.center + positionScale * ellipsoid.size(i) *
+ ellipsoid.orientation.col(i))
+ .width(5)
+ .color(simox::Color(color)));
}
}
}
@@ -309,21 +302,20 @@ namespace armarx::armem::server::obj::instance
color(3) = 64;
layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
- .position(orientationDisplaced
- ? ellipsoid.center + orientationScale * rot.axis()
- : ellipsoid.center)
- .normal(rot.axis())
- .radius(orientationScale)
- .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
- .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
- .color(simox::Color(color))
- );
+ .position(orientationDisplaced
+ ? ellipsoid.center + orientationScale * rot.axis()
+ : ellipsoid.center)
+ .normal(rot.axis())
+ .radius(orientationScale)
+ .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
+ .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
+ .color(simox::Color(color)));
}
}
}
-
- void Visu::RemoteGui::setup(const Visu& visu)
+ void
+ Visu::RemoteGui::setup(const Visu& visu)
{
using namespace armarx::RemoteGui::Client;
@@ -388,8 +380,8 @@ namespace armarx::armem::server::obj::instance
group.addChild(grid);
}
-
- void Visu::RemoteGui::Gaussians::setup(const Visu& data)
+ void
+ Visu::RemoteGui::Gaussians::setup(const Visu& data)
{
using namespace armarx::RemoteGui::Client;
@@ -410,8 +402,8 @@ namespace armarx::armem::server::obj::instance
group.addChild(grid);
}
-
- void Visu::RemoteGui::Gaussians::update(Visu::Gaussians& data)
+ void
+ Visu::RemoteGui::Gaussians::update(Visu::Gaussians& data)
{
data.position = position.getValue();
data.positionScale = positionScale.getValue();
@@ -421,8 +413,8 @@ namespace armarx::armem::server::obj::instance
data.orientationDisplaced = orientationDisplaced.getValue();
}
-
- void Visu::RemoteGui::update(Visu& visu)
+ void
+ Visu::RemoteGui::update(Visu& visu)
{
visu.enabled = enabled.getValue();
visu.inGlobalFrame = inGlobalFrame.getValue();
@@ -440,4 +432,4 @@ namespace armarx::armem::server::obj::instance
linearPredictions.update(visu.linearPredictions);
}
-}
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index 13a27228aa1198d91d1c0db4c1f809ce93b6f5f1..7905c56f2283ecf4efc5d23a940eea873e717e43 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -11,11 +11,11 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h>
-
namespace armarx
{
class ObjectFinder;
}
+
namespace armarx::armem::server::obj::instance
{
@@ -26,8 +26,8 @@ namespace armarx::armem::server::obj::instance
class Visu : public armarx::Logging
{
public:
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "visu.");
std::vector<viz::Layer> visualizeCommit(
const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
@@ -36,40 +36,34 @@ namespace armarx::armem::server::obj::instance
const ObjectFinder& objectFinder) const;
/// Visualize the given object poses, with one layer per provider.
- std::vector<viz::Layer> visualizeCommit(
- const objpose::ObjectPoseSeq& objectPoses,
- const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
- const ObjectFinder& objectFinder
- ) const;
+ std::vector<viz::Layer>
+ visualizeCommit(const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
+ const ObjectFinder& objectFinder) const;
std::vector<viz::Layer> visualizeCommit(
const objpose::ObjectPoseMap& objectPoses,
const std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>>& poseHistories,
- const ObjectFinder& objectFinder
- ) const;
+ const ObjectFinder& objectFinder) const;
- viz::Layer visualizeProvider(
- const std::string& providerName,
- const objpose::ObjectPoseSeq& objectPoses,
- const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
- const ObjectFinder& objectFinder
- ) const;
+ viz::Layer
+ visualizeProvider(const std::string& providerName,
+ const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
+ const ObjectFinder& objectFinder) const;
- void visualizeObjectPose(
- viz::Layer& layer,
- const objpose::ObjectPose& objectPose,
- const std::map<DateTime, objpose::ObjectPose>& poseHistory,
- const ObjectFinder& objectFinder
- ) const;
+ void visualizeObjectPose(viz::Layer& layer,
+ const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
+ const ObjectFinder& objectFinder) const;
private:
-
- viz::Layer& getLayer(const std::string& providerName, std::map<std::string, viz::Layer>& stage) const;
+ viz::Layer& getLayer(const std::string& providerName,
+ std::map<std::string, viz::Layer>& stage) const;
public:
-
viz::Client arviz;
bool enabled = true;
@@ -99,6 +93,7 @@ namespace armarx::armem::server::obj::instance
const objpose::ObjectPose& objectPose,
bool inGlobalFrame) const;
};
+
Gaussians gaussians;
/// Prefer articulated models if available.
@@ -137,6 +132,7 @@ namespace armarx::armem::server::obj::instance
void setup(const Visu& data);
void update(Visu::Gaussians& data);
};
+
Gaussians gaussians;
armarx::RemoteGui::Client::CheckBox useArticulatedModels;
@@ -146,7 +142,6 @@ namespace armarx::armem::server::obj::instance
void setup(const Visu& visu);
void update(Visu& visu);
};
-
};
-}
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp
index 789b597d51ab1845ca75375bb13a7ec105dadc3d..176e8f3281ac1bac9e016f7b014ee6b14963d366 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.cpp
@@ -4,79 +4,79 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/ice_conversions.h>
-#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
-
namespace armarx::armem::server::obj::instance::visu
{
- void LinearPredictions::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ LinearPredictions::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(showGhost, prefix + "showGhost",
- "Show ghosts at linearly predicted object poses.");
- defs->optional(ghostAlpha, prefix + "ghostAlpha",
- "Alpha of linear prediction ghosts.");
- defs->optional(showFrame, prefix + "showFrame",
- "Show frames at linearly predicted object poses.");
- defs->optional(showArrow, prefix + "showArrow",
+ defs->optional(
+ showGhost, prefix + "showGhost", "Show ghosts at linearly predicted object poses.");
+ defs->optional(ghostAlpha, prefix + "ghostAlpha", "Alpha of linear prediction ghosts.");
+ defs->optional(
+ showFrame, prefix + "showFrame", "Show frames at linearly predicted object poses.");
+ defs->optional(showArrow,
+ prefix + "showArrow",
"Show arrows from current object poses to the linearly predicted ones.");
- defs->optional(timeOffsetSeconds, prefix + "timeOffset",
+ defs->optional(timeOffsetSeconds,
+ prefix + "timeOffset",
"The offset (in seconds) to the current time to make predictions for.");
- defs->optional(timeWindowSeconds, prefix + "timeWindow",
+ defs->optional(timeWindowSeconds,
+ prefix + "timeWindow",
"The time window (in seconds) into the past to perform the regression on.");
-
}
-
- bool LinearPredictions::showAny() const
+ bool
+ LinearPredictions::showAny() const
{
return showGhost or showFrame or showArrow;
}
- void LinearPredictions::draw(
- viz::Layer& layer,
- std::function<viz::Object(const std::string& key)> makeObjectFn,
- const objpose::ObjectPose& objectPose,
- const std::map<DateTime, objpose::ObjectPose>& poseHistory,
- bool inGlobalFrame
- ) const
+ void
+ LinearPredictions::draw(viz::Layer& layer,
+ std::function<viz::Object(const std::string& key)> makeObjectFn,
+ const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
+ bool inGlobalFrame) const
{
const std::string key = objectPose.objectID.str();
- const Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ const Eigen::Matrix4f pose =
+ inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
auto predictionResult = objpose::predictObjectPoseLinear(
- poseHistory,
- DateTime::Now() + Duration::SecondsDouble(timeOffsetSeconds),
- objectPose);
+ poseHistory, DateTime::Now() + Duration::SecondsDouble(timeOffsetSeconds), objectPose);
if (predictionResult.success)
{
auto predictedObjectPose =
armarx::fromIce<objpose::ObjectPose>(predictionResult.prediction);
- const Eigen::Matrix4f& predictedPose =
- inGlobalFrame ? predictedObjectPose.objectPoseGlobal : predictedObjectPose.objectPoseRobot;
+ const Eigen::Matrix4f& predictedPose = inGlobalFrame
+ ? predictedObjectPose.objectPoseGlobal
+ : predictedObjectPose.objectPoseRobot;
if (showGhost)
{
layer.add(makeObjectFn(key + " Linear Prediction Ghost")
- .pose(predictedPose)
- .overrideColor(simox::Color::white().with_alpha(ghostAlpha)));
+ .pose(predictedPose)
+ .overrideColor(simox::Color::white().with_alpha(ghostAlpha)));
}
if (showFrame)
{
- layer.add(viz::Pose(key + " Linear Prediction Pose")
- .pose(predictedPose));
+ layer.add(viz::Pose(key + " Linear Prediction Pose").pose(predictedPose));
}
if (showArrow)
{
- layer.add(viz::Arrow(key + " Linear Prediction Arrw")
- .fromTo(simox::math::position(pose), simox::math::position(predictedPose))
- .width(10)
- .color(viz::Color::azure()));
+ layer.add(
+ viz::Arrow(key + " Linear Prediction Arrw")
+ .fromTo(simox::math::position(pose), simox::math::position(predictedPose))
+ .width(10)
+ .color(viz::Color::azure()));
}
}
else
@@ -86,8 +86,8 @@ namespace armarx::armem::server::obj::instance::visu
}
}
-
- void LinearPredictions::RemoteGui::setup(const LinearPredictions& data)
+ void
+ LinearPredictions::RemoteGui::setup(const LinearPredictions& data)
{
using namespace armarx::RemoteGui::Client;
@@ -110,12 +110,14 @@ namespace armarx::armem::server::obj::instance::visu
GridLayout grid;
int row = 0;
- HBoxLayout showBoxes(
- {Label("Ghost"), showGhost,
- Label(" Frame"), showFrame,
- Label(" Arrow"), showArrow,
- Label(" Ghost Alpha"), ghostAlpha
- });
+ HBoxLayout showBoxes({Label("Ghost"),
+ showGhost,
+ Label(" Frame"),
+ showFrame,
+ Label(" Arrow"),
+ showArrow,
+ Label(" Ghost Alpha"),
+ ghostAlpha});
grid.add(showBoxes, {row, 0}, {1, 2});
row++;
@@ -132,8 +134,8 @@ namespace armarx::armem::server::obj::instance::visu
group.addChild(grid);
}
-
- void LinearPredictions::RemoteGui::update(LinearPredictions& data)
+ void
+ LinearPredictions::RemoteGui::update(LinearPredictions& data)
{
data.showGhost = showGhost.getValue();
data.ghostAlpha = ghostAlpha.getValue();
@@ -143,4 +145,4 @@ namespace armarx::armem::server::obj::instance::visu
data.timeWindowSeconds = timeWindowSeconds.getValue();
}
-}
+} // namespace armarx::armem::server::obj::instance::visu
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h
index 9ede135125c6d44ac62bdd5ecef2fc6eeb2f353c..fff5c4be411439d958589b771e5008632200c0a1 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/visu/LinearPredictionsVisu.h
@@ -10,7 +10,6 @@
#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
-
namespace armarx::armem::server::obj::instance::visu
{
@@ -35,7 +34,6 @@ namespace armarx::armem::server::obj::instance::visu
const std::map<DateTime, objpose::ObjectPose>& poseHistory,
bool inGlobalFrame) const;
-
struct RemoteGui
{
armarx::RemoteGui::Client::CheckBox showGhost;
@@ -52,8 +50,7 @@ namespace armarx::armem::server::obj::instance::visu
void setup(const LinearPredictions& data);
void update(LinearPredictions& data);
};
-
};
-}
+} // namespace armarx::armem::server::obj::instance::visu
diff --git a/source/RobotAPI/libraries/armem_objects/test/ArMemObjectsTest.cpp b/source/RobotAPI/libraries/armem_objects/test/ArMemObjectsTest.cpp
index 3624c35b18ff85c1b71bdf58ad5a37c245f7959e..652e4bf89316034b33443553c58f76a0d87865ab 100644
--- a/source/RobotAPI/libraries/armem_objects/test/ArMemObjectsTest.cpp
+++ b/source/RobotAPI/libraries/armem_objects/test/ArMemObjectsTest.cpp
@@ -28,8 +28,6 @@
#include <iostream>
-
-
BOOST_AUTO_TEST_CASE(test_armem_objects)
{
BOOST_CHECK(true);
diff --git a/source/RobotAPI/libraries/armem_objects/utils.cpp b/source/RobotAPI/libraries/armem_objects/utils.cpp
index e3025f5264ebcbc5a50de330cb229f792b6f421f..de43eefa8606e370310d9c82fb4a9a24b8a97610 100644
--- a/source/RobotAPI/libraries/armem_objects/utils.cpp
+++ b/source/RobotAPI/libraries/armem_objects/utils.cpp
@@ -26,7 +26,6 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
-
namespace armarx::armem
{
diff --git a/source/RobotAPI/libraries/armem_objects/utils.h b/source/RobotAPI/libraries/armem_objects/utils.h
index bc0068234ac8c55d4985d25ab17fab8040c987d7..c88a6c2cc8e3591c5c80fa39a5140ecb6c0face8 100644
--- a/source/RobotAPI/libraries/armem_objects/utils.h
+++ b/source/RobotAPI/libraries/armem_objects/utils.h
@@ -22,15 +22,13 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
-
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
namespace armarx::armem::objects
{
- armem::MemoryID
- reconstructObjectInstanceID(const objpose::ObjectPose& objectPose);
+ armem::MemoryID reconstructObjectInstanceID(const objpose::ObjectPose& objectPose);
} // namespace armarx::armem::objects
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp
index bf2d89bb748ccf788288bc9632acf55937877125..6c6c3ce9dd5cfebe53077598de0bb1b0e40655b3 100644
--- a/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp
+++ b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.cpp
@@ -29,11 +29,12 @@ namespace armarx::armem::server::reasoning
AnticipationSegment::AnticipationSegment(armem::server::MemoryToIceAdapter& iceMemory) :
Base(iceMemory, CORE_SEGMENT_NAME)
{
-
}
- void AnticipationSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ AnticipationSegment::defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
Base::defineProperties(defs, prefix);
}
-}
+} // namespace armarx::armem::server::reasoning
diff --git a/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h
index 3a05c7adb5b9e7d96d51c2e07a6c18f016bc506a..08a7b2249c5865e0ba346fe0c6a09596a46b33e6 100644
--- a/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h
+++ b/source/RobotAPI/libraries/armem_reasoning/server/AnticipationSegment.h
@@ -26,7 +26,6 @@
#include <string>
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
-
#include <RobotAPI/libraries/armem_reasoning/aron/Anticipation.aron.generated.h>
namespace armarx::armem::server::reasoning
@@ -38,9 +37,10 @@ namespace armarx::armem::server::reasoning
AnticipationSegment(armem::server::MemoryToIceAdapter& iceMemory);
- virtual void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ virtual void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
public:
const std::string CORE_SEGMENT_NAME = "Anticipation";
};
-}
+} // namespace armarx::armem::server::reasoning
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index d100459622099a65dded88c216950cae1ff1841f..f5963c311a2ee7044cd510b9a2c9614e954b8d52 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -86,9 +86,9 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) const
{
Robot robot{.description = description,
- .instance = "", // TODO(fabian.reister):
- .config = {}, // will be populated by synchronize
- .timestamp = timestamp};
+ .instance = "", // TODO(fabian.reister):
+ .config = {}, // will be populated by synchronize
+ .timestamp = timestamp};
ARMARX_CHECK(synchronize(robot, timestamp));
@@ -187,8 +187,7 @@ namespace armarx::armem::robot_state
}
std::optional<RobotState>
- RobotReader::queryState(const std::string& robotName,
- const armem::Time& timestamp) const
+ RobotReader::queryState(const std::string& robotName, const armem::Time& timestamp) const
{
std::optional<RobotState> robotState = queryJointState(robotName, timestamp);
@@ -207,28 +206,25 @@ namespace armarx::armem::robot_state
}
std::optional<RobotState>
- RobotReader::queryJointState(const std::string& robotName,
- const armem::Time& timestamp) const
+ RobotReader::queryJointState(const std::string& robotName, const armem::Time& timestamp) const
{
const auto proprioception = queryProprioception(robotName, timestamp);
if (not proprioception.has_value())
{
- ARMARX_VERBOSE << "Failed to query proprioception for robot '" << robotName
- << "'.";
+ ARMARX_VERBOSE << "Failed to query proprioception for robot '" << robotName << "'.";
return std::nullopt;
}
const auto jointMap = proprioception->joints.position;
return RobotState{.timestamp = timestamp,
- .globalPose = RobotState::Pose::Identity(),
- .jointMap = jointMap,
- .proprioception = proprioception};
+ .globalPose = RobotState::Pose::Identity(),
+ .jointMap = jointMap,
+ .proprioception = proprioception};
}
std::optional<::armarx::armem::robot_state::localization::Transform>
- RobotReader::queryOdometryPose(const std::string& robotName,
- const armem::Time& timestamp) const
+ RobotReader::queryOdometryPose(const std::string& robotName, const armem::Time& timestamp) const
{
robot_state::localization::TransformQuery query{
@@ -379,13 +375,12 @@ namespace armarx::armem::robot_state
}
std::optional<RobotState::Pose>
- RobotReader::queryGlobalPose(const std::string& robotName,
- const armem::Time& timestamp) const
+ RobotReader::queryGlobalPose(const std::string& robotName, const armem::Time& timestamp) const
{
try
{
- const auto result = transformReader.getGlobalPose(
- robotName, constants::robotRootNodeName, timestamp);
+ const auto result =
+ transformReader.getGlobalPose(robotName, constants::robotRootNodeName, timestamp);
if (not result)
{
return std::nullopt;
@@ -525,8 +520,7 @@ namespace armarx::armem::robot_state
// force torque for left and right
std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
- RobotReader::queryForceTorque(const std::string& robotName,
- const armem::Time& timestamp) const
+ RobotReader::queryForceTorque(const std::string& robotName, const armem::Time& timestamp) const
{
// Query all entities from provider.
@@ -609,8 +603,7 @@ namespace armarx::armem::robot_state
}
std::optional<std::map<RobotReader::Hand, exteroception::ToF>>
- RobotReader::queryToF(const std::string& robotName,
- const armem::Time& timestamp) const
+ RobotReader::queryToF(const std::string& robotName, const armem::Time& timestamp) const
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -738,7 +731,8 @@ namespace armarx::armem::robot_state
RobotReader::getForceTorques(const armarx::armem::wm::Memory& memory,
const std::string& name) const
{
- std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>> forceTorques;
+ std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>
+ forceTorques;
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
@@ -809,7 +803,7 @@ namespace armarx::armem::robot_state
std::optional<description::RobotDescription>
RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
- const std::string& name) const
+ const std::string& name) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -896,4 +890,4 @@ namespace armarx::armem::robot_state
return {};
}
-} // namespace armarx::armem::robot_state::client::common
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
index 89caaa7826d69c454761d515092e0d72c58c8ca4..fbcdfcd9b7a3b44508af3784c7114151f7a0097a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.cpp
@@ -190,4 +190,4 @@ namespace armarx::armem::robot_state
}
-} // namespace armarx::armem::robot_state::client::common
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 505d2b44b59d52ff9515adeea173b1684e3aaf74..a7af99a2603bdddbe1d8a1787d7bf2e641fb820a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -6,7 +6,6 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -25,7 +24,8 @@ namespace armarx::armem::robot_state
if (not robotState)
{
ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
- << robot.getName() << "` " << "(type `" << robot.getType() << "`)!";
+ << robot.getName() << "` "
+ << "(type `" << robot.getType() << "`)!";
return false;
}
@@ -43,7 +43,8 @@ namespace armarx::armem::robot_state
if (not robotState)
{
ARMARX_VERBOSE << deactivateSpam(5) << "Querying robot state failed for robot `"
- << robot.getName() << "` " << "(type `" << robot.getType() << "`)!";
+ << robot.getName() << "` "
+ << "(type `" << robot.getType() << "`)!";
return false;
}
@@ -100,9 +101,11 @@ namespace armarx::armem::robot_state
if (robot == nullptr)
{
ARMARX_WARNING << deactivateSpam(1) << "Failed to query robot `" << name
- << "`. At the moment, a blocking behavior is not possible in onConnectComponent()."
- << "Therefore, a nullptr will be returned. This will likely cause problems downstream.";
- // << "`. Will try again ...";
+ << "`. At the moment, a blocking behavior is not possible in "
+ "onConnectComponent()."
+ << "Therefore, a nullptr will be returned. This will likely cause "
+ "problems downstream.";
+ // << "`. Will try again ...";
}
else
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index 83a17cf03a6059ce9effddc40f5777a191a0a37f..7b7a0815db3273b1c7809bafdd55fa95d6fe478f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -73,9 +73,9 @@ namespace armarx::armem::robot_state
*/
[[nodiscard]] VirtualRobot::RobotPtr
getRobotWaiting(const std::string& name,
- const armem::Time& timestamp = armem::Time::Invalid(),
- const VirtualRobot::RobotIO::RobotDescription& loadMode =
- VirtualRobot::RobotIO::RobotDescription::eStructure);
+ const armem::Time& timestamp = armem::Time::Invalid(),
+ const VirtualRobot::RobotIO::RobotDescription& loadMode =
+ VirtualRobot::RobotIO::RobotDescription::eStructure);
private:
[[nodiscard]] VirtualRobot::RobotPtr
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
index b0a9552b82a11d0f44e6d3d7c75c297169bf7bff..adcbb775e6c491e997f33979e8fc02d356f0f37e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
@@ -52,8 +52,8 @@ namespace armarx::armem::robot_state
[[nodiscard]] bool storeState(const VirtualRobot::Robot& robot,
const armem::Time& timestamp);
- using RobotWriter::storeState;
using RobotWriter::storeDescription;
+ using RobotWriter::storeState;
};
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 8613833baf8a001359f4878a5ff69a7d993658ce..927fb76beb897eb5bfb4bbb7c1f4ec9f4994202f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -62,7 +62,6 @@ namespace armarx::armem::robot_state::client::localization
TransformReader::TransformReader(const TransformReader& t) :
memoryReader(t.memoryReader), properties(t.properties), propertyPrefix(t.propertyPrefix)
{
-
}
TransformReader::~TransformReader() = default;
@@ -190,4 +189,4 @@ namespace armarx::armem::robot_state::client::localization
}
}
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::robot_state::client::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 884d85255c22a19d84f72e2041b6134fa06573c9..aafb7c26bf9e73a603eca36ead0459dcf40bce0c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -60,7 +60,11 @@ namespace armarx::armem::robot_state::client::localization
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
- armem::client::Reader getMemoryReader(){return memoryReader;}
+ armem::client::Reader
+ getMemoryReader()
+ {
+ return memoryReader;
+ }
private:
armem::client::Reader memoryReader;
@@ -75,4 +79,4 @@ namespace armarx::armem::robot_state::client::localization
const std::string propertyPrefix = "mem.robot_state.";
};
-} // namespace armarx::armem::client::robot_state::localization
+} // namespace armarx::armem::robot_state::client::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index ac13c1fa3180d3bfe22edce4e5e8c0a54d7585db..772e0d258e0172d6c5c84d563b96553e5deee8ea 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -71,13 +71,16 @@ namespace armarx::armem::robot_state::localization
ARMARX_INFO << deactivateSpam(1) << "No transform available.";
return {.transform = {.header = query.header},
.status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF lookup: '" + query.header.parentFrame +
- " -> " + query.header.frame +
- "'. No memory data in time range. Reference time " + sanitizedTimestamp.value().toTimeString()};
+ .errorMessage = "Error in TF lookup: '" + query.header.parentFrame + " -> " +
+ query.header.frame +
+ "'. No memory data in time range. Reference time " +
+ sanitizedTimestamp.value().toTimeString()};
}
- const Eigen::Isometry3f transform = std::accumulate(
- transforms.begin(), transforms.end(), Eigen::Isometry3f::Identity(), std::multiplies<>());
+ const Eigen::Isometry3f transform = std::accumulate(transforms.begin(),
+ transforms.end(),
+ Eigen::Isometry3f::Identity(),
+ std::multiplies<>());
ARMARX_DEBUG << "Found valid transform";
@@ -109,14 +112,13 @@ namespace armarx::armem::robot_state::localization
if (transforms.empty())
{
ARMARX_INFO << deactivateSpam(1) << "No transform available.";
- return
- {
- .header = query.header,
- .transforms = {},
- .status = TransformChainResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF lookup: '" + query.header.parentFrame + " -> " +
- query.header.frame + "'. No memory data in time range. Reference time " + query.header.timestamp.toTimeString()
- };
+ return {.header = query.header,
+ .transforms = {},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF lookup: '" + query.header.parentFrame + " -> " +
+ query.header.frame +
+ "'. No memory data in time range. Reference time " +
+ query.header.timestamp.toTimeString()};
}
@@ -334,8 +336,9 @@ namespace armarx::armem::robot_state::localization
}
auto
- findFirstElementAfter(const std::vector<::armarx::armem::robot_state::localization::Transform>& transforms,
- const armem::Time& timestamp)
+ findFirstElementAfter(
+ const std::vector<::armarx::armem::robot_state::localization::Transform>& transforms,
+ const armem::Time& timestamp)
{
const auto comp = [](const armem::Time& timestamp, const auto& transform)
{ return transform.header.timestamp < timestamp; };
@@ -430,4 +433,4 @@ namespace armarx::armem::robot_state::localization
}
-} // namespace armarx::armem::common::robot_state::localization
+} // namespace armarx::armem::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
index 8bd428ce382fea070e611bde7893be991910b5db..869db0711f2312d2a4f873adc831120dd4ad8865 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
@@ -42,7 +42,8 @@ namespace armarx::armem::robot_state::localization
ErrorFrameNotAvailable
} status;
- explicit operator bool() const
+ explicit
+ operator bool() const
{
return status == Status::Success;
}
@@ -63,7 +64,8 @@ namespace armarx::armem::robot_state::localization
ErrorFrameNotAvailable
} status;
- explicit operator bool() const
+ explicit
+ operator bool() const
{
return status == Status::Success;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
index 042315b10fa3d8137874acbffd46a6fda96c4f94..081e6745435b890615079a7e44ff041e6c00bd7d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.cpp
@@ -34,8 +34,7 @@ namespace armarx::armem::robot_state
.name = "",
.xml = ::armarx::PackagePath("", fs::path("")), // initialize empty, no default c'tor
.visualization = {},
- .info = {}
- };
+ .info = {}};
fromAron(aronRobotDescription, robotDescription);
diff --git a/source/RobotAPI/libraries/armem_robot_state/robot_conversions.h b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.h
index 0e4f9a4fb3a8858963a23c6f84f679f730405698..5018ef44175dcf0f178ebc504961034bfb83049c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/robot_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/robot_conversions.h
@@ -11,6 +11,7 @@ namespace armarx::armem::wm
namespace armarx::armem::robot_state
{
- std::optional<description::RobotDescription> convertRobotDescription(const armem::wm::EntityInstance& instance);
+ std::optional<description::RobotDescription>
+ convertRobotDescription(const armem::wm::EntityInstance& instance);
std::optional<RobotState> convertRobotState(const armem::wm::EntityInstance& instance);
-} // namespace armarx::armem::articulated_object
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 2627328d9a0f7efb5270b7b3dbf9a803d6a66c5e..a7a322bdb3f4eb43ab589691d519388e3bdf16f8 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -95,20 +95,19 @@ namespace armarx::armem::server::robot_state
const armarx::data::PackagePath xmlPath = robot.description.xml.serialize();
const auto* robotModel = getRobotModel(robot.description);
-
+
std::map<std::string, float> jointMap = robot.config.jointMap;
// we override the jointMap with the joint values from the robot model if it has not been set explicitly
- if(robotModel != nullptr)
+ if (robotModel != nullptr)
{
- for(const auto& [jointName, jointValue]: robotModel->model->getJointValues())
+ for (const auto& [jointName, jointValue] : robotModel->model->getJointValues())
{
- if(jointMap.count(jointName) == 0)
+ if (jointMap.count(jointName) == 0)
{
jointMap[jointName] = jointValue;
}
}
-
}
// clang-format off
@@ -251,9 +250,10 @@ namespace armarx::armem::server::robot_state
// - global pose
// - joint positions
// => this is nothing but an armem::Robot
- ARMARX_DEBUG << "Combining robot ..." << "\n- " << robotDescriptions.size()
- << " descriptions" << "\n- " << globalPoses.size() << " global poses" << "\n- "
- << sensorValues.size() << " joint positions";
+ ARMARX_DEBUG << "Combining robot ..."
+ << "\n- " << robotDescriptions.size() << " descriptions"
+ << "\n- " << globalPoses.size() << " global poses"
+ << "\n- " << sensorValues.size() << " joint positions";
const armem::robot_state::Robots robots =
combine(robotDescriptions, globalPoses, sensorValues, timestamp);
@@ -372,8 +372,8 @@ namespace armarx::armem::server::robot_state
{
Visu::RobotModel* robotModel = getRobotModel(robot.description);
-
- if(robotModel == nullptr)
+
+ if (robotModel == nullptr)
{
// ARMARX_WARNING << "Robot model is null";
return;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index d3857eb673ae7e3a6419d22d8467000ed0a4a7d3..c2ba52d3a92291d682b326aa367f4d48d781230e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -79,7 +79,8 @@ namespace armarx::armem::server::robot_state
struct RobotModel; // forward declaration
- RobotModel* getRobotModel(const armem::robot_state::description::RobotDescription& robotDescription);
+ RobotModel*
+ getRobotModel(const armem::robot_state::description::RobotDescription& robotDescription);
private:
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index ef4c89379d9f243e1c7b22699b93c983370f5614..c8c1d71c7cd41b17522b07ab5ca690deacabd9e9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -189,8 +189,7 @@ namespace armarx::armem::server::robot_state::description
e.torso_kinematic_chain =
tryGet([&]() { return arm.getTorsoKinematicChain(); });
- e.hand_unit =
- tryGet([&]() { return arm.getHandUnitName(); });
+ e.hand_unit = tryGet([&]() { return arm.getHandUnitName(); });
info.parts.emplace(side + "Arm", e);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
index bb74546dd41aacb7460101f146fdf7145cfdc0eb..e0a47de8ae046173fb0215a1f6ca47601a03a3be 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.cpp
@@ -16,15 +16,14 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
namespace armarx::armem::server::robot_state::exteroception
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
index 3b6d0042909223d93e7c69555ea96a2fe89d06f6..54b065cc8e119110101bb449bbe5213fd23b8a50 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/Segment.h
@@ -32,7 +32,6 @@
#include <RobotAPI/libraries/armem_robot_state/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
-
namespace armarx::armem::server::robot_state::exteroception
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
index b2bb1785d34e7d75089946264fc1dd8be6fb0c98..b749e34a24ea2077be0ddfc5d9bed4d21147101b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ArmarDEConverter.cpp
@@ -56,7 +56,7 @@ namespace armarx::armem::server::robot_state::exteroception
// FIXME change if unsigned
dto.tof["Right"].sigma.resize(singleDimSize, singleDimSize);
dto.tof["Right"].sigma.setConstant(-1); // in order to unset fields faults
-
+
// FIXME change if unsigned
dto.tof["Right"].targetDetected.resize(singleDimSize, singleDimSize);
dto.tof["Right"].targetDetected.setConstant(-1); // in order to unset fields faults
@@ -146,8 +146,8 @@ namespace armarx::armem::server::robot_state::exteroception
void
ArmarDEConverter::processToFDepthEntry(armarx::armem::exteroception::arondto::ToF& tof,
- const std::vector<std::string>& split,
- const ConverterValue& value)
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
{
// split, e.g., element_12
const std::vector<std::string> elements = simox::alg::split(split.back(), "_");
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
index 4ba33e6c5f308cd6213c8ccf02018ad63ba9440b..2bf5b099f585b7046f121bc5e0d7e03b444705d2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.cpp
@@ -1,6 +1,5 @@
#include "ConverterInterface.h"
-
namespace armarx::armem::server::robot_state::exteroception
{
@@ -8,4 +7,4 @@ namespace armarx::armem::server::robot_state::exteroception
{
}
-}
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
index 5ba0ecc3c8aeaf5c2b3f3c03d9f36ed83269fb5e..49eed30862007db1f32d3210ba41488e58a49016 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterInterface.h
@@ -9,7 +9,8 @@ namespace armarx::RobotUnitDataStreaming
struct TimeStep;
struct DataStreamingDescription;
struct DataEntry;
-}
+} // namespace armarx::RobotUnitDataStreaming
+
namespace armarx::armem::arondto
{
class Proprioception;
@@ -20,13 +21,10 @@ namespace armarx::armem::server::robot_state::exteroception
class ConverterInterface
{
public:
-
virtual ~ConverterInterface();
- virtual
- aron::data::DictPtr convert(
- const RobotUnitDataStreaming::TimeStep& data,
- const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
-
+ virtual aron::data::DictPtr
+ convert(const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
};
-}
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
index 235c5efd988e40d89c5d10b051c773abd41eaf5b..01dca799cb63ae312dc4f24e14c16772c682db8d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.cpp
@@ -1,9 +1,8 @@
#include "ConverterRegistry.h"
-#include "ArmarDEConverter.h"
-
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include "ArmarDEConverter.h"
namespace armarx::armem::server::robot_state::exteroception
{
@@ -14,7 +13,6 @@ namespace armarx::armem::server::robot_state::exteroception
add<ArmarDEConverter>("Armar7");
}
-
ConverterInterface*
ConverterRegistry::get(const std::string& key) const
{
@@ -22,10 +20,10 @@ namespace armarx::armem::server::robot_state::exteroception
return it != converters.end() ? it->second.get() : nullptr;
}
-
- std::vector<std::string> ConverterRegistry::getKeys() const
+ std::vector<std::string>
+ ConverterRegistry::getKeys() const
{
return simox::alg::get_keys(converters);
}
-}
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
index 1e1cac9a18eb3d901b0ca0f61eccf08395a279b5..716dab8716ffab1e53c82b68e311f6ebf53f8b58 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterRegistry.h
@@ -7,30 +7,25 @@
#include "ConverterInterface.h"
-
namespace armarx::armem::server::robot_state::exteroception
{
class ConverterRegistry
{
public:
-
ConverterRegistry();
-
- template <class ConverterT, class ...Args>
- void add(const std::string& key, Args... args)
+ template <class ConverterT, class... Args>
+ void
+ add(const std::string& key, Args... args)
{
converters[key].reset(new ConverterT(args...));
}
-
ConverterInterface* get(const std::string& key) const;
std::vector<std::string> getKeys() const;
private:
-
std::map<std::string, std::unique_ptr<ConverterInterface>> converters;
-
};
-}
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
index 6086a9aee5f2adb8d997a65302084e0c902d747c..feb4f335dce1e6e66d32d7b866d76dd68ecdeff2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.cpp
@@ -1,12 +1,11 @@
#include "ConverterTools.h"
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/advanced.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
namespace armarx::armem::server::robot_state
{
@@ -22,104 +21,57 @@ namespace armarx::armem::server::robot_state
}
return std::nullopt;
}
-}
-
+} // namespace armarx::armem::server::robot_state
namespace armarx::armem::server::robot_state::exteroception
{
ConverterTools::ConverterTools()
{
{
- vector3fSetters["X"] = [](Eigen::Vector3f & v, float f)
- {
- v.x() = f;
- };
- vector3fSetters["Y"] = [](Eigen::Vector3f & v, float f)
- {
- v.y() = f;
- };
- vector3fSetters["Z"] = [](Eigen::Vector3f & v, float f)
- {
- v.z() = f;
- };
+ vector3fSetters["X"] = [](Eigen::Vector3f& v, float f) { v.x() = f; };
+ vector3fSetters["Y"] = [](Eigen::Vector3f& v, float f) { v.y() = f; };
+ vector3fSetters["Z"] = [](Eigen::Vector3f& v, float f) { v.z() = f; };
vector3fSetters["x"] = vector3fSetters["X"];
vector3fSetters["y"] = vector3fSetters["Y"];
vector3fSetters["z"] = vector3fSetters["Z"];
vector3fSetters["Rotation"] = vector3fSetters["Z"];
}
{
- platformPoseGetters["acceleration"] = [](prop::arondto::Platform & p)
- {
- return &p.acceleration;
- };
- platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
- {
- return &p.relativePosition;
- };
- platformPoseGetters["velocity"] = [](prop::arondto::Platform & p)
- {
- return &p.velocity;
- };
+ platformPoseGetters["acceleration"] = [](prop::arondto::Platform& p)
+ { return &p.acceleration; };
+ platformPoseGetters["relativePosition"] = [](prop::arondto::Platform& p)
+ { return &p.relativePosition; };
+ platformPoseGetters["velocity"] = [](prop::arondto::Platform& p)
+ { return &p.velocity; };
platformIgnored.insert("absolutePosition");
}
{
- ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
- {
- return &ft.gravityCompensationForce;
- };
- ftGetters["gravCompT"] = [](prop::arondto::ForceTorque & ft)
- {
- return &ft.gravityCompensationTorque;
- };
- ftGetters["f"] = [](prop::arondto::ForceTorque & ft)
- {
- return &ft.force;
- };
- ftGetters["t"] = [](prop::arondto::ForceTorque & ft)
- {
- return &ft.torque;
- };
+ ftGetters["gravCompF"] = [](prop::arondto::ForceTorque& ft)
+ { return &ft.gravityCompensationForce; };
+ ftGetters["gravCompT"] = [](prop::arondto::ForceTorque& ft)
+ { return &ft.gravityCompensationTorque; };
+ ftGetters["f"] = [](prop::arondto::ForceTorque& ft) { return &ft.force; };
+ ftGetters["t"] = [](prop::arondto::ForceTorque& ft) { return &ft.torque; };
}
{
- jointGetters["acceleration"] = [](prop::arondto::Joints & j)
- {
- return &j.acceleration;
- };
- jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
- {
- return &j.gravityTorque;
- };
- jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
- {
- return &j.inertiaTorque;
- };
- jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
- {
- return &j.inverseDynamicsTorque;
- };
- jointGetters["position"] = [](prop::arondto::Joints & j)
- {
- return &j.position;
- };
- jointGetters["torque"] = [](prop::arondto::Joints & j)
- {
- return &j.torque;
- };
- jointGetters["velocity"] = [](prop::arondto::Joints & j)
- {
- return &j.velocity;
- };
+ jointGetters["acceleration"] = [](prop::arondto::Joints& j) { return &j.acceleration; };
+ jointGetters["gravityTorque"] = [](prop::arondto::Joints& j)
+ { return &j.gravityTorque; };
+ jointGetters["inertiaTorque"] = [](prop::arondto::Joints& j)
+ { return &j.inertiaTorque; };
+ jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints& j)
+ { return &j.inverseDynamicsTorque; };
+ jointGetters["position"] = [](prop::arondto::Joints& j) { return &j.position; };
+ jointGetters["torque"] = [](prop::arondto::Joints& j) { return &j.torque; };
+ jointGetters["velocity"] = [](prop::arondto::Joints& j) { return &j.velocity; };
}
{
-#define ADD_SCALAR_SETTER(container, name, type) \
- container [ #name ] = []( \
- prop::arondto::Joints & dto, \
- const std::vector<std::string>& split, \
- const ConverterValue & value) \
- { \
- dto. name [split.at(1)] = getValueAs< type >(value); \
- }
+#define ADD_SCALAR_SETTER(container, name, type) \
+ container[#name] = [](prop::arondto::Joints& dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue& value) \
+ { dto.name[split.at(1)] = getValueAs<type>(value); }
ADD_SCALAR_SETTER(jointSetters, position, float);
ADD_SCALAR_SETTER(jointSetters, velocity, float);
@@ -158,15 +110,14 @@ namespace armarx::armem::server::robot_state::exteroception
ADD_SCALAR_SETTER(jointSetters, JointStatusOperation, int);
-#define ADD_VECTOR3_SETTER(container, name, type) \
- container [ #name ] = [this]( \
- prop::arondto::Joints & dto, \
- const std::vector<std::string>& split, \
- const ConverterValue & value) \
- { \
- auto& vec = dto. name [split.at(1)]; \
- auto& setter = this->vector3fSetters.at(split.at(3)); \
- setter(vec, getValueAs< type >(value)); \
+#define ADD_VECTOR3_SETTER(container, name, type) \
+ container[#name] = [this](prop::arondto::Joints& dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue& value) \
+ { \
+ auto& vec = dto.name[split.at(1)]; \
+ auto& setter = this -> vector3fSetters.at(split.at(3)); \
+ setter(vec, getValueAs<type>(value)); \
}
ADD_VECTOR3_SETTER(jointSetters, angularVelocity, float);
@@ -176,4 +127,4 @@ namespace armarx::armem::server::robot_state::exteroception
}
}
-}
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
index 7460ebb133a01387c0f29a472961366877254506..2e184a6ac77313d9c4d203fbe25bf9020dd858cd 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/exteroception/converters/ConverterTools.h
@@ -8,12 +8,11 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include "ConverterInterface.h"
-
namespace armarx::armem::server::robot_state::exteroception
{
@@ -23,7 +22,6 @@ namespace armarx::armem::server::robot_state::exteroception
const RobotUnitDataStreaming::DataEntry& entry;
};
-
template <class T>
T
getValueAs(const ConverterValue& value)
@@ -31,16 +29,14 @@ namespace armarx::armem::server::robot_state::exteroception
return RobotUnitDataStreamingReceiver::GetAs<T>(value.data, value.entry);
}
-
/**
* @brief Search
* @param key
* @param prefixes
* @return
*/
- std::optional<std::string>
- findByPrefix(const std::string& key, const std::set<std::string>& prefixes);
-
+ std::optional<std::string> findByPrefix(const std::string& key,
+ const std::set<std::string>& prefixes);
template <class ValueT>
ValueT
@@ -56,7 +52,6 @@ namespace armarx::armem::server::robot_state::exteroception
return nullptr;
}
-
template <class ValueT>
ValueT
findBySuffix(const std::string& key, const std::map<std::string, ValueT>& map)
@@ -71,36 +66,37 @@ namespace armarx::armem::server::robot_state::exteroception
return nullptr;
}
-
-
class ConverterTools
{
public:
-
ConverterTools();
public:
+ std::map<std::string, std::function<void(Eigen::Vector3f&, float)>> vector3fSetters;
- std::map<std::string, std::function<void(Eigen::Vector3f&, float)> > vector3fSetters;
+ std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)>>
+ jointGetters;
+ std::map<std::string,
+ std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)>>
+ jointVectorGetters;
- std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
- std::map<std::string, std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)> > jointVectorGetters;
-
- using JointSetter = std::function<void(prop::arondto::Joints& dto, const std::vector<std::string>& split, const ConverterValue& value)>;
+ using JointSetter = std::function<void(prop::arondto::Joints& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)>;
std::map<std::string, JointSetter> jointSetters;
- std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)> > platformPoseGetters;
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)>>
+ platformPoseGetters;
std::set<std::string> platformIgnored;
- std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)> > ftGetters;
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)>>
+ ftGetters;
- std::map<std::string, std::string> sidePrefixMap
- {
- { "R", "Right" },
- { "L", "Left" },
+ std::map<std::string, std::string> sidePrefixMap{
+ {"R", "Right"},
+ {"L", "Left"},
};
-
};
-}
+} // namespace armarx::armem::server::robot_state::exteroception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index 7384f90ed1af0f6237f98c570c4b855a31cef583..67344bde3ad50f15d3c11bd58d91b329dfe4892a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -3,47 +3,46 @@
// STL
#include <iterator>
-#include <manif/SE3.h>
-
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/regression/linear.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
-
#include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
-
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_state/types.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h>
#include <RobotAPI/libraries/armem_robot_state/common/localization/types.h>
-#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
+#include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <manif/SE3.h>
namespace armarx::armem::server::robot_state::localization
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
- Base(memoryToIceAdapter, armem::robot_state::localizationSegmentID.coreSegmentName, arondto::Transform::ToAronType(), 1024)
+ Base(memoryToIceAdapter,
+ armem::robot_state::localizationSegmentID.coreSegmentName,
+ arondto::Transform::ToAronType(),
+ 1024)
{
}
-
Segment::~Segment()
{
}
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
Base::defineProperties(defs, prefix);
@@ -53,30 +52,27 @@ namespace armarx::armem::server::robot_state::localization
" when requested via the PredictingMemoryInterface (in seconds).");
}
- void Segment::init()
+ void
+ Segment::init()
{
Base::init();
segmentPtr->addPredictor(armem::PredictionEngine{.engineID = "Linear"},
- [this](const PredictionRequest& request){ return this->predictLinear(request); });
+ [this](const PredictionRequest& request)
+ { return this->predictLinear(request); });
}
-
- void Segment::onConnect()
+ void
+ Segment::onConnect()
{
}
-
RobotFramePoseMap
Segment::getRobotFramePosesLocking(const armem::Time& timestamp) const
{
- return this->doLocked([this, ×tamp]()
- {
- return getRobotFramePoses(timestamp);
- });
+ return this->doLocked([this, ×tamp]() { return getRobotFramePoses(timestamp); });
}
-
RobotFramePoseMap
Segment::getRobotFramePoses(const armem::Time& timestamp) const
{
@@ -86,15 +82,11 @@ namespace armarx::armem::server::robot_state::localization
RobotFramePoseMap frames;
for (const std::string& robotName : segmentPtr->getProviderSegmentNames())
{
- TransformQuery query
- {
- .header = {
- .parentFrame = armarx::GlobalFrame,
- .frame = ::armarx::armem::robot_state::constants::robotRootNodeName,
- .agent = robotName,
- .timestamp = timestamp
- }
- };
+ TransformQuery query{
+ .header = {.parentFrame = armarx::GlobalFrame,
+ .frame = ::armarx::armem::robot_state::constants::robotRootNodeName,
+ .agent = robotName,
+ .timestamp = timestamp}};
const auto result = TransformHelper::lookupTransformChain(*segmentPtr, query);
if (not result)
@@ -112,17 +104,12 @@ namespace armarx::armem::server::robot_state::localization
return frames;
}
-
RobotPoseMap
Segment::getRobotGlobalPosesLocking(const armem::Time& timestamp) const
{
- return this->doLocked([this, ×tamp]()
- {
- return getRobotGlobalPoses(timestamp);
- });
+ return this->doLocked([this, ×tamp]() { return getRobotGlobalPoses(timestamp); });
}
-
RobotPoseMap
Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
{
@@ -132,16 +119,11 @@ namespace armarx::armem::server::robot_state::localization
RobotPoseMap robotGlobalPoses;
for (const std::string& robotName : segmentPtr->getProviderSegmentNames())
{
- TransformQuery query
- {
- .header =
- {
- .parentFrame = GlobalFrame,
- .frame = armarx::armem::robot_state::constants::robotRootNodeName,
- .agent = robotName,
- .timestamp = timestamp
- }
- };
+ TransformQuery query{
+ .header = {.parentFrame = GlobalFrame,
+ .frame = armarx::armem::robot_state::constants::robotRootNodeName,
+ .agent = robotName,
+ .timestamp = timestamp}};
if (const auto result = TransformHelper::lookupTransform(*segmentPtr, query))
{
@@ -159,8 +141,8 @@ namespace armarx::armem::server::robot_state::localization
return robotGlobalPoses;
}
-
- bool Segment::commitTransform(const armarx::armem::robot_state::localization::Transform& transform)
+ bool
+ Segment::commitTransform(const armarx::armem::robot_state::localization::Transform& transform)
{
Commit commit;
commit.add(makeUpdate(transform));
@@ -169,8 +151,9 @@ namespace armarx::armem::server::robot_state::localization
return result.allSuccess();
}
-
- bool Segment::commitTransformLocking(const armarx::armem::robot_state::localization::Transform& transform)
+ bool
+ Segment::commitTransformLocking(
+ const armarx::armem::robot_state::localization::Transform& transform)
{
Commit commit;
commit.add(makeUpdate(transform));
@@ -179,14 +162,16 @@ namespace armarx::armem::server::robot_state::localization
return result.allSuccess();
}
-
- EntityUpdate Segment::makeUpdate(const armarx::armem::robot_state::localization::Transform& transform) const
+ EntityUpdate
+ Segment::makeUpdate(const armarx::armem::robot_state::localization::Transform& transform) const
{
const armem::Time& timestamp = transform.header.timestamp;
- const MemoryID providerID = segmentPtr->id().withProviderSegmentName(transform.header.agent);
+ const MemoryID providerID =
+ segmentPtr->id().withProviderSegmentName(transform.header.agent);
EntityUpdate update;
- update.entityID = providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
+ update.entityID =
+ providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
update.arrivedTime = update.referencedTime = update.sentTime = timestamp;
arondto::Transform aronTransform;
@@ -196,8 +181,8 @@ namespace armarx::armem::server::robot_state::localization
return update;
}
-
- PredictionResult Segment::predictLinear(const PredictionRequest& request)
+ PredictionResult
+ Segment::predictLinear(const PredictionRequest& request)
{
PredictionResult result;
result.snapshotID = request.snapshotID;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index b974ff31af81496c7d320953f16ae57900a7c897..ea85645fcd6af2a42623535f4f4508be2e8c99ff 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -22,8 +22,8 @@
#pragma once
// STD / STL
-#include <string>
#include <optional>
+#include <string>
#include <unordered_map>
// Eigen
@@ -33,12 +33,10 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedCoreSegment.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
-
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
-
namespace armarx::armem::server::robot_state::localization
{
class Segment : public segment::SpecializedCoreSegment
@@ -46,11 +44,11 @@ namespace armarx::armem::server::robot_state::localization
using Base = segment::SpecializedCoreSegment;
public:
-
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
void init() override;
@@ -68,7 +66,6 @@ namespace armarx::armem::server::robot_state::localization
private:
-
EntityUpdate makeUpdate(const armem::robot_state::localization::Transform& transform) const;
PredictionResult predictLinear(const PredictionRequest& request);
@@ -77,8 +74,8 @@ namespace armarx::armem::server::robot_state::localization
{
double predictionTimeWindow = 2;
};
- Properties properties;
+ Properties properties;
};
-} // namespace armarx::armem::server::robot_state::localization
+} // namespace armarx::armem::server::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index 611257b20771979a763daf62202b3aa96f244ceb..f3348ddfdaa286ac6162f78bd69c9d36d433ad19 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -98,7 +98,8 @@ namespace armarx::armem::server::robot_state::proprioception
endProprioception = std::chrono::high_resolution_clock::now();
// Localization
- for (const armem::robot_state::localization::Transform& transform : update.localization)
+ for (const armem::robot_state::localization::Transform& transform :
+ update.localization)
{
localizationSegment.doLocked(
[&localizationSegment, &transform]()
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
index 917a122df0f8fda9b3cf875f228a2bd63e86e4c2..a7bf44db44378c8816e67245e6aec888d2dd2c4e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
@@ -36,25 +36,26 @@
#include "RobotUnitData.h"
-
namespace armarx::plugins
{
class DebugObserverComponentPlugin;
}
+
namespace armarx::armem::server
{
class MemoryToIceAdapter;
}
+
namespace armarx::armem::server::robot_state::localization
{
class Segment;
}
+
namespace armarx::armem::server::robot_state::proprioception
{
class RobotStateWriter : public armarx::Logging
{
public:
-
void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin);
using Queue = armarx::armem::server::robot_state::proprioception::Queue;
@@ -64,9 +65,7 @@ namespace armarx::armem::server::robot_state::proprioception
void run(float pollFrequency,
Queue& dataBuffer,
MemoryToIceAdapter& memory,
- localization::Segment& localizationSegment
- );
-
+ localization::Segment& localizationSegment);
struct Update
{
@@ -74,23 +73,21 @@ namespace armarx::armem::server::robot_state::proprioception
armem::Commit exteroception;
std::vector<armem::robot_state::localization::Transform> localization;
};
+
Update buildUpdate(const RobotUnitData& dataQueue);
private:
-
armem::robot_state::localization::Transform
- getTransform(
- const aron::data::DictPtr& platformData,
- const Time& timestamp) const;
+ getTransform(const aron::data::DictPtr& platformData, const Time& timestamp) const;
public:
-
struct Properties
{
armem::MemoryID robotUnitProviderID;
};
+
Properties properties;
std::optional<DebugObserverHelper> debugObserver;
@@ -100,10 +97,9 @@ namespace armarx::armem::server::robot_state::proprioception
private:
-
bool noOdometryDataLogged = false;
bool isRunning();
};
-}
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
index 88313402f217eb8f46470aafd796465354f6cc68..f56f576703137c1026289191a5899531e00d9b85 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
@@ -13,24 +13,23 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+#include "../exteroception/converters/ConverterInterface.h"
+#include "../exteroception/converters/ConverterRegistry.h"
#include "RobotUnitData.h"
#include "converters/ConverterInterface.h"
-#include "../exteroception/converters/ConverterInterface.h"
#include "converters/ConverterRegistry.h"
-#include "../exteroception/converters/ConverterRegistry.h"
-
namespace armarx::plugins
{
class RobotUnitComponentPlugin;
class DebugObserverComponentPlugin;
} // namespace armarx::plugins
+
namespace armarx
{
using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
}
-
// TODO move this out of proprioception. it is now for proprioception and exteroception
namespace armarx::armem::server::robot_state::proprioception
{
@@ -63,6 +62,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
std::string sensorPrefix = "sens.*";
};
+
Properties properties;
std::optional<DebugObserverHelper> debugObserver;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index d7136b1ef301a2cdc455fef4fa3f4dbcad5dd1d3..0fb7ddd9c8b8e183a252f68bde6c2c3a1f33c51c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -9,16 +9,15 @@
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/util/prediction_helpers.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include "SensorValues.h"
-#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
-
namespace armarx::armem::server::robot_state::proprioception
{
@@ -43,21 +42,23 @@ namespace armarx::armem::server::robot_state::proprioception
" when requested via the PredictingMemoryInterface (in seconds).");
}
- void Segment::init()
+ void
+ Segment::init()
{
Base::init();
- segmentPtr->addPredictor(
- armem::PredictionEngine{.engineID = "Linear"},
- [this](const PredictionRequest& request) { return this->predictLinear(request); });
+ segmentPtr->addPredictor(armem::PredictionEngine{.engineID = "Linear"},
+ [this](const PredictionRequest& request)
+ { return this->predictLinear(request); });
}
- void Segment::onConnect(RobotUnitInterfacePrx robotUnitPrx)
+ void
+ Segment::onConnect(RobotUnitInterfacePrx robotUnitPrx)
{
this->robotUnit = robotUnitPrx;
std::string providerSegmentName = "Robot";
-
+
KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
if (kinematicUnit)
{
@@ -65,27 +66,23 @@ namespace armarx::armem::server::robot_state::proprioception
}
else
{
- ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
- << "\n Falling back to provider segment name '" << providerSegmentName << "'.";
+ ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name
+ << "' does not have a kinematic unit."
+ << "\n Falling back to provider segment name '" << providerSegmentName
+ << "'.";
}
this->robotUnitProviderID = segmentPtr->id().withProviderSegmentName(providerSegmentName);
}
-
-
- SensorValuesMap Segment::getSensorValuesLocking(
- const armem::Time& timestamp,
- DebugObserverHelper* debugObserver) const
+ SensorValuesMap
+ Segment::getSensorValuesLocking(const armem::Time& timestamp,
+ DebugObserverHelper* debugObserver) const
{
return doLocked([this, ×tamp, &debugObserver]()
- {
- return getSensorValues(timestamp, debugObserver);
- });
+ { return getSensorValues(timestamp, debugObserver); });
}
-
- static
- aron::data::DictPtr
+ static aron::data::DictPtr
getDictElement(const aron::data::Dict& dict, const std::string& key)
{
if (dict.hasElement(key))
@@ -95,11 +92,8 @@ namespace armarx::armem::server::robot_state::proprioception
return nullptr;
}
-
SensorValuesMap
- Segment::getSensorValues(
- const armem::Time& timestamp,
- DebugObserverHelper* debugObserver) const
+ Segment::getSensorValues(const armem::Time& timestamp, DebugObserverHelper* debugObserver) const
{
namespace adn = aron::data;
ARMARX_CHECK_NOT_NULL(segmentPtr);
@@ -107,65 +101,74 @@ namespace armarx::armem::server::robot_state::proprioception
SensorValuesMap sensorValues;
int i = 0;
- Duration tFindData = Duration::MilliSeconds(0), tReadSensorValues = Duration::MilliSeconds(0);
+ Duration tFindData = Duration::MilliSeconds(0),
+ tReadSensorValues = Duration::MilliSeconds(0);
TIMING_START(tProcessEntities)
- segmentPtr->forEachEntity([&](const wm::Entity & entity)
- {
- adn::DictPtr data;
+ segmentPtr->forEachEntity(
+ [&](const wm::Entity& entity)
{
- TIMING_START(_tFindData)
-
- const wm::EntitySnapshot* snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
- if (not snapshot)
+ adn::DictPtr data;
{
- // Got no snapshot <= timestamp. Take latest instead (if present).
- snapshot = entity.findLatestSnapshot();
+ TIMING_START(_tFindData)
+
+ const wm::EntitySnapshot* snapshot =
+ entity.findLatestSnapshotBeforeOrAt(timestamp);
+ if (not snapshot)
+ {
+ // Got no snapshot <= timestamp. Take latest instead (if present).
+ snapshot = entity.findLatestSnapshot();
+ }
+ if (snapshot)
+ {
+ data = snapshot->findInstanceData();
+ }
+
+ TIMING_END_COMMENT_STREAM(
+ _tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG);
+ tFindData += Duration::MicroSeconds(_tFindData.toMicroSeconds());
}
- if (snapshot)
+ if (data)
{
- data = snapshot->findInstanceData();
- }
-
- TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG);
- tFindData += Duration::MicroSeconds(_tFindData.toMicroSeconds());
- }
- if (data)
- {
- TIMING_START(_tReadSensorValues)
+ TIMING_START(_tReadSensorValues)
- sensorValues.emplace(entity.id().providerSegmentName, readSensorValues(*data));
+ sensorValues.emplace(entity.id().providerSegmentName, readSensorValues(*data));
- TIMING_END_COMMENT_STREAM(_tReadSensorValues, "tReadSensorValues " + std::to_string(i), ARMARX_DEBUG)
- tReadSensorValues += Duration::MicroSeconds(_tReadSensorValues.toMicroSeconds());
- }
- ++i;
- });
+ TIMING_END_COMMENT_STREAM(
+ _tReadSensorValues, "tReadSensorValues " + std::to_string(i), ARMARX_DEBUG)
+ tReadSensorValues +=
+ Duration::MicroSeconds(_tReadSensorValues.toMicroSeconds());
+ }
+ ++i;
+ });
TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG)
if (debugObserver)
{
- debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(dp + "t 1.1.1 FindData (ms)", tFindData.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(dp + "t 1.1.2 ReadSensorValues (ms)", tReadSensorValues.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)",
+ tProcessEntities.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1 FindData (ms)",
+ tFindData.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.2 ReadSensorValues (ms)",
+ tReadSensorValues.toMilliSecondsDouble());
}
return sensorValues;
}
-
- const armem::MemoryID& Segment::getRobotUnitProviderID() const
+ const armem::MemoryID&
+ Segment::getRobotUnitProviderID() const
{
return robotUnitProviderID;
}
- Eigen::VectorXd readJointData(const wm::EntityInstanceData& data)
+ Eigen::VectorXd
+ readJointData(const wm::EntityInstanceData& data)
{
namespace adn = aron::data;
std::vector<double> values;
- auto addData =
- [&](adn::DictPtr dict) // NOLINT
+ auto addData = [&](adn::DictPtr dict) // NOLINT
{
for (const auto& [name, value] : dict->getElements())
{
@@ -195,8 +198,7 @@ namespace armarx::armem::server::robot_state::proprioception
}
void
- emplaceJointData(const Eigen::VectorXd& jointData,
- arondto::Proprioception& dataTemplate)
+ emplaceJointData(const Eigen::VectorXd& jointData, arondto::Proprioception& dataTemplate)
{
Eigen::Index row = 0;
for (auto& [joint, value] : dataTemplate.joints.position)
@@ -228,7 +230,8 @@ namespace armarx::armem::server::robot_state::proprioception
}
const DateTime timeOrigin = DateTime::Now();
- const armarx::Duration timeWindow = Duration::SecondsDouble(properties.predictionTimeWindow);
+ const armarx::Duration timeWindow =
+ Duration::SecondsDouble(properties.predictionTimeWindow);
SnapshotRangeInfo<Eigen::VectorXd, aron::data::DictPtr> info;
doLocked(
@@ -236,8 +239,8 @@ namespace armarx::armem::server::robot_state::proprioception
[&, this]()
{
info = getSnapshotsInRange<server::wm::CoreSegment,
- Eigen::VectorXd,
- aron::data::DictPtr>(
+ Eigen::VectorXd,
+ aron::data::DictPtr>(
segmentPtr,
request.snapshotID,
timeOrigin - timeWindow,
@@ -279,7 +282,6 @@ namespace armarx::armem::server::robot_state::proprioception
return result;
}
-
SensorValues
Segment::readSensorValues(const wm::EntityInstanceData& data)
{
@@ -301,8 +303,8 @@ namespace armarx::armem::server::robot_state::proprioception
for (const auto& [name, value] : values->getElements())
{
checkJVM(name);
- sensorValues.jointValueMap[name].position
- = adn::Float::DynamicCastAndCheck(value)->getValue();
+ sensorValues.jointValueMap[name].position =
+ adn::Float::DynamicCastAndCheck(value)->getValue();
}
}
if (adn::DictPtr values = getDictElement(*joints, "velocity"))
@@ -310,8 +312,8 @@ namespace armarx::armem::server::robot_state::proprioception
for (const auto& [name, value] : values->getElements())
{
checkJVM(name);
- sensorValues.jointValueMap[name].velocity
- = adn::Float::DynamicCastAndCheck(value)->getValue();
+ sensorValues.jointValueMap[name].velocity =
+ adn::Float::DynamicCastAndCheck(value)->getValue();
}
}
if (adn::DictPtr values = getDictElement(*joints, "torque"))
@@ -319,8 +321,8 @@ namespace armarx::armem::server::robot_state::proprioception
for (const auto& [name, value] : values->getElements())
{
checkJVM(name);
- sensorValues.jointValueMap[name].velocity
- = adn::Float::DynamicCastAndCheck(value)->getValue();
+ sensorValues.jointValueMap[name].velocity =
+ adn::Float::DynamicCastAndCheck(value)->getValue();
}
}
}
@@ -330,19 +332,18 @@ namespace armarx::armem::server::robot_state::proprioception
{
if (adn::DictPtr forceTorqueValues = aron::data::Dict::DynamicCastAndCheck(value))
{
- const Eigen::Vector3f torque
- = armarx::aron::converter::AronEigenConverter::ConvertToVector3f(
- adn::NDArray::DynamicCastAndCheck(forceTorqueValues->getElement("torque")));
-
- const Eigen::Vector3f force
- = armarx::aron::converter::AronEigenConverter::ConvertToVector3f(
- adn::NDArray::DynamicCastAndCheck(forceTorqueValues->getElement("force")));
-
- sensorValues.forceTorqueValuesMap[name] = ForceTorqueValues
- {
- .force = force,
- .torque = torque
- };
+ const Eigen::Vector3f torque =
+ armarx::aron::converter::AronEigenConverter::ConvertToVector3f(
+ adn::NDArray::DynamicCastAndCheck(
+ forceTorqueValues->getElement("torque")));
+
+ const Eigen::Vector3f force =
+ armarx::aron::converter::AronEigenConverter::ConvertToVector3f(
+ adn::NDArray::DynamicCastAndCheck(
+ forceTorqueValues->getElement("force")));
+
+ sensorValues.forceTorqueValuesMap[name] =
+ ForceTorqueValues{.force = force, .torque = torque};
}
}
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index fbce59a996acb8b17ffb104095eadf570d02d535..2dee6f457709216ca0e32ed1e2744011a827fa97 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -37,11 +37,11 @@
#include <RobotAPI/libraries/armem/server/segment/SpecializedProviderSegment.h>
#include <RobotAPI/libraries/armem_robot_state/server/forward_declarations.h>
-
namespace armarx
{
class DebugObserverHelper;
}
+
namespace armarx::armem::server::robot_state::proprioception
{
class Segment : public segment::SpecializedCoreSegment
@@ -49,21 +49,21 @@ namespace armarx::armem::server::robot_state::proprioception
using Base = segment::SpecializedCoreSegment;
public:
-
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
void init() override;
void onConnect(RobotUnitInterfacePrx robotUnitPrx);
- SensorValuesMap getSensorValues(
- const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
- SensorValuesMap getSensorValuesLocking(
- const armem::Time& timestamp, DebugObserverHelper* debugObserver = nullptr) const;
+ SensorValuesMap getSensorValues(const armem::Time& timestamp,
+ DebugObserverHelper* debugObserver = nullptr) const;
+ SensorValuesMap getSensorValuesLocking(const armem::Time& timestamp,
+ DebugObserverHelper* debugObserver = nullptr) const;
const armem::MemoryID& getRobotUnitProviderID() const;
@@ -71,14 +71,10 @@ namespace armarx::armem::server::robot_state::proprioception
private:
-
- static
- SensorValues
- readSensorValues(const wm::EntityInstanceData& data);
+ static SensorValues readSensorValues(const wm::EntityInstanceData& data);
private:
-
RobotUnitInterfacePrx robotUnit;
armem::MemoryID robotUnitProviderID;
@@ -86,11 +82,11 @@ namespace armarx::armem::server::robot_state::proprioception
{
double predictionTimeWindow = 2;
};
+
Properties properties;
// Debug Observer prefix
const std::string dp = "Proprioception::getRobotJointPositions() | ";
-
};
-} // namespace armarx::armem::server::robot_state::proprioception
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp
index ee25d654eb5ee1e045d67ef5ea504ad4834bc8a5..e0047a28496254c4334f672731c9e6a88f3c2ac4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.cpp
@@ -1,2 +1 @@
#include "SensorValues.h"
-
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h
index 2beb7608afb3a5710b761fce3651fe2dee81b73c..7de781059124c782f135dd7916fd0147ec8c0adf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/SensorValues.h
@@ -26,25 +26,24 @@
namespace armarx::armem::server::robot_state::proprioception
{
-struct JointValues
-{
- double position = 0.0;
- double velocity = 0.0;
- double torque = 0.0;
- //double torqueTicks = 0.0;
-};
-
-struct ForceTorqueValues
-{
- Eigen::Vector3f force = Eigen::Vector3f::Zero();
- Eigen::Vector3f torque = Eigen::Vector3f::Zero();
-};
-
-struct SensorValues
-{
- JointValuesMap jointValueMap;
- ForceTorqueValuesMap forceTorqueValuesMap;
-};
-
-}
-
+ struct JointValues
+ {
+ double position = 0.0;
+ double velocity = 0.0;
+ double torque = 0.0;
+ //double torqueTicks = 0.0;
+ };
+
+ struct ForceTorqueValues
+ {
+ Eigen::Vector3f force = Eigen::Vector3f::Zero();
+ Eigen::Vector3f torque = Eigen::Vector3f::Zero();
+ };
+
+ struct SensorValues
+ {
+ JointValuesMap jointValueMap;
+ ForceTorqueValuesMap forceTorqueValuesMap;
+ };
+
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp
index 888c68c7313b10c775e81512c3fdc414844c4025..9bcdf2a2e3795198bcd2451d27c6481749c21875 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.cpp
@@ -1,22 +1,21 @@
#include "aron_conversions.h"
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
-
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
namespace armarx
{
- aron::data::VariantPtr RobotUnitDataStreaming::toAron(
- const TimeStep& timestep,
- const DataEntry& dataEntry)
+ aron::data::VariantPtr
+ RobotUnitDataStreaming::toAron(const TimeStep& timestep, const DataEntry& dataEntry)
{
switch (dataEntry.type)
{
case RobotUnitDataStreaming::NodeTypeFloat:
{
- float value = RobotUnitDataStreamingReceiver::GetAs<Ice::Float>(timestep, dataEntry);
+ float value =
+ RobotUnitDataStreamingReceiver::GetAs<Ice::Float>(timestep, dataEntry);
return std::make_shared<aron::data::Float>(value);
}
case RobotUnitDataStreaming::NodeTypeBool:
@@ -46,7 +45,8 @@ namespace armarx
}
case RobotUnitDataStreaming::NodeTypeDouble:
{
- double value = RobotUnitDataStreamingReceiver::GetAs<Ice::Double>(timestep, dataEntry);
+ double value =
+ RobotUnitDataStreamingReceiver::GetAs<Ice::Double>(timestep, dataEntry);
return std::make_shared<aron::data::Double>(value);
}
default:
@@ -54,16 +54,15 @@ namespace armarx
}
}
-
- const simox::meta::EnumNames<RobotUnitDataStreaming::DataEntryType> RobotUnitDataStreaming::DataEntryNames =
- {
- { NodeTypeBool, "Bool" },
- { NodeTypeByte, "Byte" },
- { NodeTypeShort, "Short" },
- { NodeTypeInt, "Int" },
- { NodeTypeLong, "Long" },
- { NodeTypeFloat, "Float" },
- { NodeTypeDouble, "Double" },
+ const simox::meta::EnumNames<RobotUnitDataStreaming::DataEntryType>
+ RobotUnitDataStreaming::DataEntryNames = {
+ {NodeTypeBool, "Bool"},
+ {NodeTypeByte, "Byte"},
+ {NodeTypeShort, "Short"},
+ {NodeTypeInt, "Int"},
+ {NodeTypeLong, "Long"},
+ {NodeTypeFloat, "Float"},
+ {NodeTypeDouble, "Double"},
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h
index 6f317faeb8c17a987e3b199a3f935028ae7ef770..3961a11dc4a154f98f076bd90a937799ae34bbfb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h
@@ -4,16 +4,13 @@
#include <SimoxUtility/meta/enum/EnumNames.hpp>
-#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
namespace armarx::RobotUnitDataStreaming
{
- aron::data::VariantPtr toAron(
- const TimeStep& timestep,
- const DataEntry& dataEntry);
+ aron::data::VariantPtr toAron(const TimeStep& timestep, const DataEntry& dataEntry);
extern const simox::meta::EnumNames<DataEntryType> DataEntryNames;
-}
+} // namespace armarx::RobotUnitDataStreaming
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
index dcbe1b7783190849e6a5d8d442a0124ca22c1b4c..88b6c7b3577e113ffdbe6086424b089ecd1fe965 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
@@ -1,36 +1,31 @@
#include "Armar6Converter.h"
+
#include <cstddef>
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/advanced.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include "ConverterTools.h"
-
namespace armarx::armem::server::robot_state::proprioception
{
- Armar6Converter::Armar6Converter() :
- tools(std::make_unique<ConverterTools>())
+ Armar6Converter::Armar6Converter() : tools(std::make_unique<ConverterTools>())
{
}
-
Armar6Converter::~Armar6Converter()
{
}
-
aron::data::DictPtr
- Armar6Converter::convert(
- const RobotUnitDataStreaming::TimeStep& data,
- const RobotUnitDataStreaming::DataStreamingDescription& description)
+ Armar6Converter::convert(const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description)
{
arondto::Proprioception dto;
dto.iterationID = data.iterationId;
@@ -42,18 +37,17 @@ namespace armarx::armem::server::robot_state::proprioception
return dto.toAron();
}
-
- void Armar6Converter::process(
- arondto::Proprioception& dto,
- const std::string& entryName,
- const ConverterValue& value)
+ void
+ Armar6Converter::process(arondto::Proprioception& dto,
+ const std::string& entryName,
+ const ConverterValue& value)
{
const std::vector<std::string> split = simox::alg::split(entryName, ".", false, false);
ARMARX_CHECK_GREATER_EQUAL(split.size(), 3);
const std::set<size_t> acceptedSizes{3, 4, 5};
ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
- << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
- << "\n- split: '" << split << "'";
+ << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
+ << "\n- split: '" << split << "'";
const std::string& category = split.at(0);
const std::string& name = split.at(1);
@@ -77,7 +71,8 @@ namespace armarx::armem::server::robot_state::proprioception
if (not processed)
{
// Fallback: Put in extra.
- const std::vector<std::string> comps{simox::alg::advanced(split.begin(), 1), split.end()};
+ const std::vector<std::string> comps{simox::alg::advanced(split.begin(), 1),
+ split.end()};
const std::string key = simox::alg::join(comps, ".");
switch (value.entry.type)
@@ -101,20 +96,19 @@ namespace armarx::armem::server::robot_state::proprioception
dto.extraBytes[key] = getValueAs<Ice::Byte>(value);
break;
default:
- ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
- << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
+ ARMARX_DEBUG
+ << "Cannot handle extra field '" << key << "' of type "
+ << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
break;
}
}
}
}
-
-
- void Armar6Converter::processPlatformEntry(
- prop::arondto::Platform& dto,
- const std::string& fieldName,
- const ConverterValue& value)
+ void
+ Armar6Converter::processPlatformEntry(prop::arondto::Platform& dto,
+ const std::string& fieldName,
+ const ConverterValue& value)
{
if (findByPrefix(fieldName, tools->platformIgnored))
{
@@ -137,11 +131,10 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
-
- void Armar6Converter::processForceTorqueEntry(
- std::map<std::string, prop::arondto::ForceTorque>& fts,
- const std::vector<std::string>& split,
- const ConverterValue& value)
+ void
+ Armar6Converter::processForceTorqueEntry(std::map<std::string, prop::arondto::ForceTorque>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
{
const std::string& name = split.at(1);
std::vector<std::string> splitName = simox::alg::split(name, " ", false, false);
@@ -155,11 +148,10 @@ namespace armarx::armem::server::robot_state::proprioception
processForceTorqueEntry(fts[side], split, value);
}
-
- void Armar6Converter::processForceTorqueEntry(
- prop::arondto::ForceTorque& dto,
- const std::vector<std::string>& split,
- const ConverterValue& value)
+ void
+ Armar6Converter::processForceTorqueEntry(prop::arondto::ForceTorque& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
{
const std::string& fieldName = split.at(2);
if (auto getter = findByPrefix(fieldName, tools->ftGetters))
@@ -175,9 +167,7 @@ namespace armarx::armem::server::robot_state::proprioception
else
{
// No setter for this field. Put in extra.
- std::string key = split.size() == 4
- ? (fieldName + "." + split.at(3))
- : fieldName;
+ std::string key = split.size() == 4 ? (fieldName + "." + split.at(3)) : fieldName;
switch (value.entry.type)
{
@@ -192,17 +182,17 @@ namespace armarx::armem::server::robot_state::proprioception
break;
default:
ARMARX_DEBUG << "Cannot handle extra field '" << key << "' of type "
- << RobotUnitDataStreaming::DataEntryNames.to_name(value.entry.type);
+ << RobotUnitDataStreaming::DataEntryNames.to_name(
+ value.entry.type);
break;
}
}
}
-
- bool Armar6Converter::processJointEntry(
- prop::arondto::Joints& dto,
- const std::vector<std::string>& split,
- const ConverterValue& value)
+ bool
+ Armar6Converter::processJointEntry(prop::arondto::Joints& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)
{
const std::string& jointName = split.at(1);
const std::string& fieldName = split.at(2);
@@ -239,4 +229,4 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
-}
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h
index 2ae2f7ebcfa93fa2fcced476aee2bb1a8ec46569..1cfc4c6083d9b787233db93e849f1eb0fde847b4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h
@@ -9,61 +9,48 @@
#include "ConverterInterface.h"
-
-
namespace armarx::armem::server::robot_state::proprioception
{
struct ConverterValue;
class ConverterTools;
-
class Armar6Converter : public ConverterInterface
{
public:
-
Armar6Converter();
virtual ~Armar6Converter() override;
aron::data::DictPtr
- convert(
- const RobotUnitDataStreaming::TimeStep& data,
- const RobotUnitDataStreaming::DataStreamingDescription& description) override;
+ convert(const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) override;
protected:
-
- void process(arondto::Proprioception& dto, const std::string& entryName, const ConverterValue& value);
-
+ void process(arondto::Proprioception& dto,
+ const std::string& entryName,
+ const ConverterValue& value);
private:
+ void processPlatformEntry(prop::arondto::Platform& dto,
+ const std::string& fieldName,
+ const ConverterValue& value);
- void processPlatformEntry(
- prop::arondto::Platform& dto,
- const std::string& fieldName,
- const ConverterValue& value);
-
- void processForceTorqueEntry(
- std::map<std::string, prop::arondto::ForceTorque>& fts,
- const std::vector<std::string>& split,
- const ConverterValue& value);
+ void processForceTorqueEntry(std::map<std::string, prop::arondto::ForceTorque>& fts,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
- void processForceTorqueEntry(
- prop::arondto::ForceTorque& ft,
- const std::vector<std::string>& split,
- const ConverterValue& value);
+ void processForceTorqueEntry(prop::arondto::ForceTorque& ft,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
- bool processJointEntry(
- prop::arondto::Joints& dto,
- const std::vector<std::string>& split,
- const ConverterValue& value);
+ bool processJointEntry(prop::arondto::Joints& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value);
private:
-
std::unique_ptr<ConverterTools> tools;
-
};
-}
-
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.cpp
index 8ddf012be951db403448f855966b026efe33ba8f..cd70051834b7d588de1ff729c8f43f3fc4462061 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.cpp
@@ -1,6 +1,5 @@
#include "ConverterInterface.h"
-
namespace armarx::armem::server::robot_state::proprioception
{
@@ -8,4 +7,4 @@ namespace armarx::armem::server::robot_state::proprioception
{
}
-}
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h
index d7f7855245dd373185bd2d34a8817e21b0a33f01..ae3c25ca1c50e3f01843fe4f01ae5df3d6e9b1d1 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterInterface.h
@@ -9,7 +9,8 @@ namespace armarx::RobotUnitDataStreaming
struct TimeStep;
struct DataStreamingDescription;
struct DataEntry;
-}
+} // namespace armarx::RobotUnitDataStreaming
+
namespace armarx::armem::arondto
{
class Proprioception;
@@ -20,14 +21,10 @@ namespace armarx::armem::server::robot_state::proprioception
class ConverterInterface
{
public:
-
virtual ~ConverterInterface();
- virtual
- aron::data::DictPtr convert(
- const RobotUnitDataStreaming::TimeStep& data,
- const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
-
+ virtual aron::data::DictPtr
+ convert(const RobotUnitDataStreaming::TimeStep& data,
+ const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
};
-}
-
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
index 4c2d920267eb05f799f66e08f26d3ae48b1ee811..61569e798df789ef09e3d7f76c890f954234b376 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.cpp
@@ -1,9 +1,8 @@
#include "ConverterRegistry.h"
-#include "Armar6Converter.h"
-
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include "Armar6Converter.h"
namespace armarx::armem::server::robot_state::proprioception
{
@@ -16,7 +15,6 @@ namespace armarx::armem::server::robot_state::proprioception
add<Armar6Converter>("Frankie");
}
-
ConverterInterface*
ConverterRegistry::get(const std::string& key) const
{
@@ -24,10 +22,10 @@ namespace armarx::armem::server::robot_state::proprioception
return it != converters.end() ? it->second.get() : nullptr;
}
-
- std::vector<std::string> ConverterRegistry::getKeys() const
+ std::vector<std::string>
+ ConverterRegistry::getKeys() const
{
return simox::alg::get_keys(converters);
}
-}
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.h
index f62f808f6ee305f67aba8ba0bff24e414baaa802..17cf7abceb64b3eadbcf760e8f5b5238a278023d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterRegistry.h
@@ -7,31 +7,25 @@
#include "ConverterInterface.h"
-
namespace armarx::armem::server::robot_state::proprioception
{
class ConverterRegistry
{
public:
-
ConverterRegistry();
-
- template <class ConverterT, class ...Args>
- void add(const std::string& key, Args... args)
+ template <class ConverterT, class... Args>
+ void
+ add(const std::string& key, Args... args)
{
converters[key].reset(new ConverterT(args...));
}
-
ConverterInterface* get(const std::string& key) const;
std::vector<std::string> getKeys() const;
private:
-
std::map<std::string, std::unique_ptr<ConverterInterface>> converters;
-
};
-}
-
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp
index d8483d22fbf836afec34991064d9f6070496b7de..fd5a938045ab5ac10ef352a31dbed9d3a46a33a3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.cpp
@@ -1,12 +1,11 @@
#include "ConverterTools.h"
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/advanced.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
namespace armarx::armem::server::robot_state
{
@@ -22,104 +21,57 @@ namespace armarx::armem::server::robot_state
}
return std::nullopt;
}
-}
-
+} // namespace armarx::armem::server::robot_state
namespace armarx::armem::server::robot_state::proprioception
{
ConverterTools::ConverterTools()
{
{
- vector3fSetters["X"] = [](Eigen::Vector3f & v, float f)
- {
- v.x() = f;
- };
- vector3fSetters["Y"] = [](Eigen::Vector3f & v, float f)
- {
- v.y() = f;
- };
- vector3fSetters["Z"] = [](Eigen::Vector3f & v, float f)
- {
- v.z() = f;
- };
+ vector3fSetters["X"] = [](Eigen::Vector3f& v, float f) { v.x() = f; };
+ vector3fSetters["Y"] = [](Eigen::Vector3f& v, float f) { v.y() = f; };
+ vector3fSetters["Z"] = [](Eigen::Vector3f& v, float f) { v.z() = f; };
vector3fSetters["x"] = vector3fSetters["X"];
vector3fSetters["y"] = vector3fSetters["Y"];
vector3fSetters["z"] = vector3fSetters["Z"];
vector3fSetters["Rotation"] = vector3fSetters["Z"];
}
{
- platformPoseGetters["acceleration"] = [](prop::arondto::Platform & p)
- {
- return &p.acceleration;
- };
- platformPoseGetters["relativePosition"] = [](prop::arondto::Platform & p)
- {
- return &p.relativePosition;
- };
- platformPoseGetters["velocity"] = [](prop::arondto::Platform & p)
- {
- return &p.velocity;
- };
+ platformPoseGetters["acceleration"] = [](prop::arondto::Platform& p)
+ { return &p.acceleration; };
+ platformPoseGetters["relativePosition"] = [](prop::arondto::Platform& p)
+ { return &p.relativePosition; };
+ platformPoseGetters["velocity"] = [](prop::arondto::Platform& p)
+ { return &p.velocity; };
platformIgnored.insert("absolutePosition");
}
{
- ftGetters["gravCompF"] = [](prop::arondto::ForceTorque & ft)
- {
- return &ft.gravityCompensationForce;
- };
- ftGetters["gravCompT"] = [](prop::arondto::ForceTorque & ft)
- {
- return &ft.gravityCompensationTorque;
- };
- ftGetters["f"] = [](prop::arondto::ForceTorque & ft)
- {
- return &ft.force;
- };
- ftGetters["t"] = [](prop::arondto::ForceTorque & ft)
- {
- return &ft.torque;
- };
+ ftGetters["gravCompF"] = [](prop::arondto::ForceTorque& ft)
+ { return &ft.gravityCompensationForce; };
+ ftGetters["gravCompT"] = [](prop::arondto::ForceTorque& ft)
+ { return &ft.gravityCompensationTorque; };
+ ftGetters["f"] = [](prop::arondto::ForceTorque& ft) { return &ft.force; };
+ ftGetters["t"] = [](prop::arondto::ForceTorque& ft) { return &ft.torque; };
}
{
- jointGetters["acceleration"] = [](prop::arondto::Joints & j)
- {
- return &j.acceleration;
- };
- jointGetters["gravityTorque"] = [](prop::arondto::Joints & j)
- {
- return &j.gravityTorque;
- };
- jointGetters["inertiaTorque"] = [](prop::arondto::Joints & j)
- {
- return &j.inertiaTorque;
- };
- jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints & j)
- {
- return &j.inverseDynamicsTorque;
- };
- jointGetters["position"] = [](prop::arondto::Joints & j)
- {
- return &j.position;
- };
- jointGetters["torque"] = [](prop::arondto::Joints & j)
- {
- return &j.torque;
- };
- jointGetters["velocity"] = [](prop::arondto::Joints & j)
- {
- return &j.velocity;
- };
+ jointGetters["acceleration"] = [](prop::arondto::Joints& j) { return &j.acceleration; };
+ jointGetters["gravityTorque"] = [](prop::arondto::Joints& j)
+ { return &j.gravityTorque; };
+ jointGetters["inertiaTorque"] = [](prop::arondto::Joints& j)
+ { return &j.inertiaTorque; };
+ jointGetters["inverseDynamicsTorque"] = [](prop::arondto::Joints& j)
+ { return &j.inverseDynamicsTorque; };
+ jointGetters["position"] = [](prop::arondto::Joints& j) { return &j.position; };
+ jointGetters["torque"] = [](prop::arondto::Joints& j) { return &j.torque; };
+ jointGetters["velocity"] = [](prop::arondto::Joints& j) { return &j.velocity; };
}
{
-#define ADD_SCALAR_SETTER(container, name, type) \
- container [ #name ] = []( \
- prop::arondto::Joints & dto, \
- const std::vector<std::string>& split, \
- const ConverterValue & value) \
- { \
- dto. name [split.at(1)] = getValueAs< type >(value); \
- }
+#define ADD_SCALAR_SETTER(container, name, type) \
+ container[#name] = [](prop::arondto::Joints& dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue& value) \
+ { dto.name[split.at(1)] = getValueAs<type>(value); }
ADD_SCALAR_SETTER(jointSetters, position, float);
ADD_SCALAR_SETTER(jointSetters, velocity, float);
@@ -158,15 +110,14 @@ namespace armarx::armem::server::robot_state::proprioception
ADD_SCALAR_SETTER(jointSetters, JointStatusOperation, int);
-#define ADD_VECTOR3_SETTER(container, name, type) \
- container [ #name ] = [this]( \
- prop::arondto::Joints & dto, \
- const std::vector<std::string>& split, \
- const ConverterValue & value) \
- { \
- auto& vec = dto. name [split.at(1)]; \
- auto& setter = this->vector3fSetters.at(split.at(3)); \
- setter(vec, getValueAs< type >(value)); \
+#define ADD_VECTOR3_SETTER(container, name, type) \
+ container[#name] = [this](prop::arondto::Joints& dto, \
+ const std::vector<std::string>& split, \
+ const ConverterValue& value) \
+ { \
+ auto& vec = dto.name[split.at(1)]; \
+ auto& setter = this -> vector3fSetters.at(split.at(3)); \
+ setter(vec, getValueAs<type>(value)); \
}
ADD_VECTOR3_SETTER(jointSetters, angularVelocity, float);
@@ -176,4 +127,4 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
-}
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.h
index ff667a1b578c074126e2a2bfb5db9100dafa964d..73ad6aa9710c8367f2c9e19ac2643bf0952fabec 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/ConverterTools.h
@@ -8,12 +8,11 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include "ConverterInterface.h"
-
namespace armarx::armem::server::robot_state::proprioception
{
@@ -23,7 +22,6 @@ namespace armarx::armem::server::robot_state::proprioception
const RobotUnitDataStreaming::DataEntry& entry;
};
-
template <class T>
T
getValueAs(const ConverterValue& value)
@@ -31,16 +29,14 @@ namespace armarx::armem::server::robot_state::proprioception
return RobotUnitDataStreamingReceiver::GetAs<T>(value.data, value.entry);
}
-
/**
* @brief Search
* @param key
* @param prefixes
* @return
*/
- std::optional<std::string>
- findByPrefix(const std::string& key, const std::set<std::string>& prefixes);
-
+ std::optional<std::string> findByPrefix(const std::string& key,
+ const std::set<std::string>& prefixes);
template <class ValueT>
ValueT
@@ -56,7 +52,6 @@ namespace armarx::armem::server::robot_state::proprioception
return nullptr;
}
-
template <class ValueT>
ValueT
findBySuffix(const std::string& key, const std::map<std::string, ValueT>& map)
@@ -71,37 +66,37 @@ namespace armarx::armem::server::robot_state::proprioception
return nullptr;
}
-
-
class ConverterTools
{
public:
-
ConverterTools();
public:
+ std::map<std::string, std::function<void(Eigen::Vector3f&, float)>> vector3fSetters;
- std::map<std::string, std::function<void(Eigen::Vector3f&, float)> > vector3fSetters;
-
- std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)> > jointGetters;
- std::map<std::string, std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)> > jointVectorGetters;
+ std::map<std::string, std::function<std::map<std::string, float>*(prop::arondto::Joints&)>>
+ jointGetters;
+ std::map<std::string,
+ std::function<std::map<std::string, Eigen::Vector3f>*(prop::arondto::Joints&)>>
+ jointVectorGetters;
- using JointSetter = std::function<void(prop::arondto::Joints& dto, const std::vector<std::string>& split, const ConverterValue& value)>;
+ using JointSetter = std::function<void(prop::arondto::Joints& dto,
+ const std::vector<std::string>& split,
+ const ConverterValue& value)>;
std::map<std::string, JointSetter> jointSetters;
- std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)> > platformPoseGetters;
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::Platform&)>>
+ platformPoseGetters;
std::set<std::string> platformIgnored;
- std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)> > ftGetters;
+ std::map<std::string, std::function<Eigen::Vector3f*(prop::arondto::ForceTorque&)>>
+ ftGetters;
- std::map<std::string, std::string> sidePrefixMap
- {
- { "R", "Right" },
- { "L", "Left" },
+ std::map<std::string, std::string> sidePrefixMap{
+ {"R", "Right"},
+ {"L", "Left"},
};
-
};
-}
-
+} // namespace armarx::armem::server::robot_state::proprioception
diff --git a/source/RobotAPI/libraries/armem_robot_state/utils.cpp b/source/RobotAPI/libraries/armem_robot_state/utils.cpp
index 5e35c27c75eb84e0493008290879dcecec22efd1..2ae8fabe667382d619514a21930b03f1686ea2ba 100644
--- a/source/RobotAPI/libraries/armem_robot_state/utils.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/utils.cpp
@@ -2,10 +2,11 @@
namespace armarx::armem::robot_state
{
- armarx::armem::MemoryID makeMemoryID(const description::RobotDescription& desc)
+ armarx::armem::MemoryID
+ makeMemoryID(const description::RobotDescription& desc)
{
return MemoryID("RobotState/Description")
- .withProviderSegmentName(desc.name)
- .withEntityName("description");
+ .withProviderSegmentName(desc.name)
+ .withEntityName("description");
}
-}
+} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.cpp b/source/RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.cpp
index 76fe90120e850e15260b65edcd868f756f5aa9b9..917e65f13b88d3f63995761fa20cd61183342a03 100644
--- a/source/RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.cpp
@@ -4,16 +4,22 @@
namespace armarx::plugins
{
- void StatechartListenerComponentPlugin::preOnInitComponent()
- {}
-
- void StatechartListenerComponentPlugin::preOnConnectComponent()
- {}
+ void
+ StatechartListenerComponentPlugin::preOnInitComponent()
+ {
+ }
- void StatechartListenerComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
- {}
-}
+ void
+ StatechartListenerComponentPlugin::preOnConnectComponent()
+ {
+ }
+ void
+ StatechartListenerComponentPlugin::postCreatePropertyDefinitions(
+ PropertyDefinitionsPtr& properties)
+ {
+ }
+} // namespace armarx::plugins
namespace armarx
{
@@ -21,4 +27,4 @@ namespace armarx
{
addPlugin(plugin);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.h b/source/RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.h
index 4f62bfdfe806ea69ccd9b758974b288511152bad..af4db1b2c032fbd7d869e5aff299fb0224dee3de 100644
--- a/source/RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.h
+++ b/source/RobotAPI/libraries/armem_skills/server/StatechartListenerComponentPlugin.h
@@ -17,37 +17,107 @@ namespace armarx::plugins
void preOnConnectComponent() override;
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
-
};
-}
+} // namespace armarx::plugins
namespace armarx
{
class StatechartListenerComponentPluginUser :
- virtual public ManagedIceObject,
- virtual public dti::StatechartListenerInterface
+ virtual public ManagedIceObject,
+ virtual public dti::StatechartListenerInterface
{
public:
StatechartListenerComponentPluginUser();
- void reportNetworkTraffic(const std::string&, const std::string&, Ice::Int, Ice::Int, const Ice::Current&) override {}
- void reportEvent(const ProfilerEvent&, const Ice::Current&) override {}
- void reportStatechartTransition(const ProfilerStatechartTransition& event, const Ice::Current&) override {}
- void reportStatechartInputParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
- void reportStatechartLocalParameters(const ProfilerStatechartParameters& event, const Ice::Current&) override {}
- void reportStatechartOutputParameters(const ProfilerStatechartParameters&, const Ice::Current&) override {}
- void reportProcessCpuUsage(const ProfilerProcessCpuUsage&, const Ice::Current&) override {}
- void reportProcessMemoryUsage(const ProfilerProcessMemoryUsage&, const Ice::Current&) override {}
-
- void reportEventList(const ProfilerEventList& events, const Ice::Current&) override {}
- void reportStatechartTransitionList(const ProfilerStatechartTransitionList&, const Ice::Current&) override {}
- void reportStatechartInputParametersList(const ProfilerStatechartParametersList& data, const Ice::Current&) override {}
- void reportStatechartLocalParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
- void reportStatechartOutputParametersList(const ProfilerStatechartParametersList&, const Ice::Current&) override {}
- void reportProcessCpuUsageList(const ProfilerProcessCpuUsageList&, const Ice::Current&) override {}
- void reportProcessMemoryUsageList(const ProfilerProcessMemoryUsageList&, const Ice::Current&) override {}
+ void
+ reportNetworkTraffic(const std::string&,
+ const std::string&,
+ Ice::Int,
+ Ice::Int,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportEvent(const ProfilerEvent&, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartTransition(const ProfilerStatechartTransition& event,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartInputParameters(const ProfilerStatechartParameters& event,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartLocalParameters(const ProfilerStatechartParameters& event,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartOutputParameters(const ProfilerStatechartParameters&,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportProcessCpuUsage(const ProfilerProcessCpuUsage&, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportProcessMemoryUsage(const ProfilerProcessMemoryUsage&, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportEventList(const ProfilerEventList& events, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartTransitionList(const ProfilerStatechartTransitionList&,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartInputParametersList(const ProfilerStatechartParametersList& data,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartLocalParametersList(const ProfilerStatechartParametersList&,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportStatechartOutputParametersList(const ProfilerStatechartParametersList&,
+ const Ice::Current&) override
+ {
+ }
+
+ void
+ reportProcessCpuUsageList(const ProfilerProcessCpuUsageList&, const Ice::Current&) override
+ {
+ }
+
+ void
+ reportProcessMemoryUsageList(const ProfilerProcessMemoryUsageList&,
+ const Ice::Current&) override
+ {
+ }
private:
armarx::plugins::StatechartListenerComponentPlugin* plugin = nullptr;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
index 057c7573743497ff8f518bd0ab2d1d0d90153b8a..ec05d57bacf2eae517ce9c173e78190f1eb8343f 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/SkillEventSegment.cpp
@@ -60,23 +60,25 @@ namespace armarx::skills::segment
auto coreSegment = this->segmentPtr;
ARMARX_CHECK(coreSegment);
- coreSegment->doLocked([&](){
- coreSegment->forEachInstance(
- [&](const armem::wm::EntityInstance& i)
+ coreSegment->doLocked(
+ [&]()
{
- auto event = i.dataAs<armarx::skills::arondto::SkillStatusUpdate>();
- skills::SkillStatusUpdate up;
- armem::fromAron(event, up);
-
- if (auto it = ret.find(up.executionId); it != ret.end() && up < it->second)
- {
- return;
- }
-
- // set or replace
- ret[up.executionId] = up;
+ coreSegment->forEachInstance(
+ [&](const armem::wm::EntityInstance& i)
+ {
+ auto event = i.dataAs<armarx::skills::arondto::SkillStatusUpdate>();
+ skills::SkillStatusUpdate up;
+ armem::fromAron(event, up);
+
+ if (auto it = ret.find(up.executionId); it != ret.end() && up < it->second)
+ {
+ return;
+ }
+
+ // set or replace
+ ret[up.executionId] = up;
+ });
});
- });
// for (const auto& [k, v] : ret)
// {
@@ -93,23 +95,25 @@ namespace armarx::skills::segment
auto coreSegment = this->segmentPtr;
ARMARX_CHECK(coreSegment);
- coreSegment->doLocked([&](){
- coreSegment->forEachInstance(
- [&](const armem::wm::EntityInstance& i)
+ coreSegment->doLocked(
+ [&]()
{
- auto event = i.dataAs<armarx::skills::arondto::SkillStatusUpdate>();
- skills::SkillStatusUpdate up;
- armem::fromAron(event, up);
-
- if (up.executionId == id)
- {
- if (!ret || (ret.has_value() && *ret < up))
+ coreSegment->forEachInstance(
+ [&](const armem::wm::EntityInstance& i)
{
- ret = up;
- }
- }
+ auto event = i.dataAs<armarx::skills::arondto::SkillStatusUpdate>();
+ skills::SkillStatusUpdate up;
+ armem::fromAron(event, up);
+
+ if (up.executionId == id)
+ {
+ if (!ret || (ret.has_value() && *ret < up))
+ {
+ ret = up;
+ }
+ }
+ });
});
- });
return ret;
}
} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.cpp b/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.cpp
index e8315cab7af3f8d5d8993823b992df7e80a0a3cf..e137b541b6f9fa341e64547b95782145d0312d99 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.cpp
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.cpp
@@ -5,29 +5,44 @@
namespace armarx::skills::segment
{
- StatechartListenerProviderSegment::StatechartListenerProviderSegment(armem::server::MemoryToIceAdapter& iceMemory):
- Base(iceMemory, "StatechartListener", "Transitions", arondto::Statechart::Transition::ToAronType())
+ StatechartListenerProviderSegment::StatechartListenerProviderSegment(
+ armem::server::MemoryToIceAdapter& iceMemory) :
+ Base(iceMemory,
+ "StatechartListener",
+ "Transitions",
+ arondto::Statechart::Transition::ToAronType())
{
}
- void StatechartListenerProviderSegment::defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix)
+ void
+ StatechartListenerProviderSegment::defineProperties(PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
// Statechart Logging
- defs->optional(p.statechartCoreSegmentName, "StatechartCoreSegmentName", "Name of the core segment for statecharts.");
- defs->optional(p.statechartTransitionsProviderSegmentName, "TransitionsProviderSegmentName", "Name of the provider segment for statechart transitions.");
- defs->optional(p.statechartTransitionsTopicName, "tpc.sub.ProfilerListener", "Name of the ProfilerListenerInterface topics to subscribe.");
+ defs->optional(p.statechartCoreSegmentName,
+ "StatechartCoreSegmentName",
+ "Name of the core segment for statecharts.");
+ defs->optional(p.statechartTransitionsProviderSegmentName,
+ "TransitionsProviderSegmentName",
+ "Name of the provider segment for statechart transitions.");
+ defs->optional(p.statechartTransitionsTopicName,
+ "tpc.sub.ProfilerListener",
+ "Name of the ProfilerListenerInterface topics to subscribe.");
this->setDefaultMaxHistorySize(10);
coreSegment.setDefaultMaxHistorySize(10);
Base::defineProperties(defs, prefix);
}
- void StatechartListenerProviderSegment::init()
+ void
+ StatechartListenerProviderSegment::init()
{
Base::init();
}
- void StatechartListenerProviderSegment::reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters& t)
+ void
+ StatechartListenerProviderSegment::reportStatechartTransitionWithParameters(
+ const ProfilerStatechartTransitionWithParameters& t)
{
const std::string& entityName = getStatechartName(t.targetStateIdentifier);
armem::Time transitionTime = armem::Time(armem::Duration::MicroSeconds(t.timestamp));
@@ -50,7 +65,9 @@ namespace armarx::skills::segment
}
}
- void StatechartListenerProviderSegment::reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList& transitions)
+ void
+ StatechartListenerProviderSegment::reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList& transitions)
{
for (const auto& t : transitions)
{
@@ -58,7 +75,8 @@ namespace armarx::skills::segment
}
}
- std::string StatechartListenerProviderSegment::getStatechartName(std::string stateName)
+ std::string
+ StatechartListenerProviderSegment::getStatechartName(std::string stateName)
{
const std::string delimiter = "->";
const int maxLevels = 2;
@@ -79,4 +97,4 @@ namespace armarx::skills::segment
return statechartName;
}
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.h b/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.h
index ed3b92876555e5fe68890e4a48685324c3587406..63fed2c74947c44896edb29e614e34871f6f3215 100644
--- a/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.h
+++ b/source/RobotAPI/libraries/armem_skills/server/segment/StatechartListenerSegment.h
@@ -4,10 +4,9 @@
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
// ArmarX
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/interface/core/Profiler.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
namespace armarx::skills::segment
{
@@ -19,11 +18,13 @@ namespace armarx::skills::segment
public:
StatechartListenerProviderSegment(armem::server::MemoryToIceAdapter& iceMemory);
- void defineProperties(PropertyDefinitionsPtr defs, const std::string &prefix);
+ void defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix);
void init();
- void reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&);
- void reportStatechartTransitionWithParametersList(const ProfilerStatechartTransitionWithParametersList&);
+ void
+ reportStatechartTransitionWithParameters(const ProfilerStatechartTransitionWithParameters&);
+ void reportStatechartTransitionWithParametersList(
+ const ProfilerStatechartTransitionWithParametersList&);
private:
std::string getStatechartName(std::string stateName);
@@ -36,6 +37,7 @@ namespace armarx::skills::segment
std::string statechartTransitionsProviderSegmentName = "Transitions";
std::string statechartTransitionsTopicName = "StateReportingTopic";
};
+
Properties p;
};
-}
+} // namespace armarx::skills::segment
diff --git a/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.cpp b/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.cpp
index 9886fdbe2b73326ca8b614ab0ed039d52e4e2b6e..74dce2a3fe1ae3225af051ef128a85007dea7335 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.cpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.cpp
@@ -2,29 +2,31 @@
#include "CPUSegment.h"
// ArmarX
-#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
+#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
namespace armarx::armem::server::systemstate::segment
{
- LightweightCpuMonitorProviderSegment::LightweightCpuMonitorProviderSegment(armem::server::MemoryToIceAdapter& iceMemory) :
+ LightweightCpuMonitorProviderSegment::LightweightCpuMonitorProviderSegment(
+ armem::server::MemoryToIceAdapter& iceMemory) :
Base(iceMemory, "LightweightSystemMonitor", "CPUUsage", nullptr, nullptr, 1000)
{
-
}
- void LightweightCpuMonitorProviderSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ LightweightCpuMonitorProviderSegment::defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
defs->optional(pollFrequencyHz, prefix + "pollFrequencyHz", "The poll frequency in Hz");
}
- void LightweightCpuMonitorProviderSegment::init()
+ void
+ LightweightCpuMonitorProviderSegment::init()
{
Base::init();
@@ -33,11 +35,13 @@ namespace armarx::armem::server::systemstate::segment
float sleepTime = (1000.f / pollFrequencyHz);
- runningTask = new PeriodicTask<LightweightCpuMonitorProviderSegment>(this, &LightweightCpuMonitorProviderSegment::loop, sleepTime);
+ runningTask = new PeriodicTask<LightweightCpuMonitorProviderSegment>(
+ this, &LightweightCpuMonitorProviderSegment::loop, sleepTime);
runningTask->start();
}
- void LightweightCpuMonitorProviderSegment::loop()
+ void
+ LightweightCpuMonitorProviderSegment::loop()
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
@@ -48,7 +52,8 @@ namespace armarx::armem::server::systemstate::segment
std::vector<double> cores = cpuMonitoring->getCurrentMultiCoreUsage();
auto data = std::make_shared<aron::data::Dict>();
- data->addElement("model", std::make_shared<aron::data::String>(model)); // TODO in seperate segment?
+ data->addElement("model",
+ std::make_shared<aron::data::String>(model)); // TODO in seperate segment?
data->addElement("load", std::make_shared<aron::data::Double>(usage));
auto list = std::make_shared<aron::data::List>();
@@ -64,8 +69,8 @@ namespace armarx::armem::server::systemstate::segment
update.entityID = providerId.withEntityName("CurrentCpuLoad");
update.confidence = 1.0;
update.referencedTime = armem::Time::Now();
- update.instancesData = { data };
+ update.instancesData = {data};
segmentPtr->update(update);
}
-}
+} // namespace armarx::armem::server::systemstate::segment
diff --git a/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.h b/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.h
index 01b46e8ef4afb9c5bae29458f225bcb061e873fa..97732c5ee574f8758a484da25137c9f7f5974de2 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.h
+++ b/source/RobotAPI/libraries/armem_system_state/server/CPUSegment.h
@@ -1,17 +1,17 @@
#pragma once
// STD/STL
-#include <iostream>
+#include <atomic>
#include <csignal>
+#include <iostream>
#include <memory>
-#include <atomic>
#include <thread>
// System Monitor
-#include "LightweightSystemMonitor/linux_memoryload.hpp"
#include "LightweightSystemMonitor/linux_cpuload.hpp"
-#include "LightweightSystemMonitor/linux_process_load.hpp"
+#include "LightweightSystemMonitor/linux_memoryload.hpp"
#include "LightweightSystemMonitor/linux_networkload.hpp"
+#include "LightweightSystemMonitor/linux_process_load.hpp"
#include "LightweightSystemMonitor/linux_systemutil.hpp"
#include "LightweightSystemMonitor/util/record_value.hpp"
#include "LightweightSystemMonitor/util/timer.hpp"
@@ -24,14 +24,16 @@
namespace armarx::armem::server::systemstate::segment
{
- class LightweightCpuMonitorProviderSegment : public armem::server::segment::SpecializedProviderSegment
+ class LightweightCpuMonitorProviderSegment :
+ public armem::server::segment::SpecializedProviderSegment
{
using Base = armem::server::segment::SpecializedProviderSegment;
public:
LightweightCpuMonitorProviderSegment(armem::server::MemoryToIceAdapter& iceMemory);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
void init() override;
private:
@@ -43,4 +45,4 @@ namespace armarx::armem::server::systemstate::segment
armarx::PeriodicTask<LightweightCpuMonitorProviderSegment>::pointer_type runningTask;
};
-}
+} // namespace armarx::armem::server::systemstate::segment
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_cpuload.cpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_cpuload.cpp
index f418cb67019616eb3fedf13c0acc3ab0d981c69d..1ea73ea48f02b6283598d80734a9ed375605369d 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_cpuload.cpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_cpuload.cpp
@@ -7,26 +7,29 @@
*
*/
#include "linux_cpuload.hpp"
-#include <stdexcept>
+
+#include <chrono>
+#include <cmath>
#include <fstream>
#include <iostream>
-#include <cmath>
#include <sstream>
-#include <chrono>
+#include <stdexcept>
#include <thread>
const std::vector<std::string> cpuIdentifiers{"user",
- "nice",
- "system",
- "idle",
- "iowait",
- "irq",
- "softirq",
- "steal",
- "guest",
- "guest_nice"};
-
-void cpuLoad::initCpuUsage() {
+ "nice",
+ "system",
+ "idle",
+ "iowait",
+ "irq",
+ "softirq",
+ "steal",
+ "guest",
+ "guest_nice"};
+
+void
+cpuLoad::initCpuUsage()
+{
this->cpuLoadMap = this->parseStatFile(this->procFile);
this->currentTime = std::chrono::system_clock::now();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
@@ -34,8 +37,11 @@ void cpuLoad::initCpuUsage() {
this->upDateCPUUsage();
}
-void cpuLoad::upDateCPUUsage() {
- if (!((this->currentTime + std::chrono::milliseconds(10)) > std::chrono::system_clock::now())) {
+void
+cpuLoad::upDateCPUUsage()
+{
+ if (!((this->currentTime + std::chrono::milliseconds(10)) > std::chrono::system_clock::now()))
+ {
this->oldCpuLoadMap = this->cpuLoadMap;
this->currentTime = std::chrono::system_clock::now();
this->cpuLoadMap = this->parseStatFile(this->procFile);
@@ -43,18 +49,23 @@ void cpuLoad::upDateCPUUsage() {
}
}
-
-double cpuLoad::getCurrentCpuUsage() {
+double
+cpuLoad::getCurrentCpuUsage()
+{
this->upDateCPUUsage();
return this->cpuUsage.at("cpu");
}
-std::vector<double> cpuLoad::getCurrentMultiCoreUsage() {
+std::vector<double>
+cpuLoad::getCurrentMultiCoreUsage()
+{
this->upDateCPUUsage();
std::vector<double> percents;
- for (const auto &elem: this->cpuUsage) {
- if (elem.first == "cpu") {
+ for (const auto& elem : this->cpuUsage)
+ {
+ if (elem.first == "cpu")
+ {
continue;
}
percents.push_back(elem.second);
@@ -62,89 +73,122 @@ std::vector<double> cpuLoad::getCurrentMultiCoreUsage() {
return percents;
}
-void cpuLoad::calculateCpuUsage() {
- for (const auto &elem: this->cpuLoadMap) {
- if (this->cpuLoadMap.at(elem.first).at("user") < this->oldCpuLoadMap.at(elem.first).at("user") ||
- this->cpuLoadMap.at(elem.first).at("nice") < this->oldCpuLoadMap.at(elem.first).at("nice") ||
- this->cpuLoadMap.at(elem.first).at("system") < this->oldCpuLoadMap.at(elem.first).at("system") ||
- this->cpuLoadMap.at(elem.first).at("idle") < this->oldCpuLoadMap.at(elem.first).at("idle")) {
- } else {
- auto total = (this->cpuLoadMap.at(elem.first).at("user") - this->oldCpuLoadMap.at(elem.first).at("user")) +
- (this->cpuLoadMap.at(elem.first).at("nice") - this->oldCpuLoadMap.at(elem.first).at("nice")) +
- (this->cpuLoadMap.at(elem.first).at("system") - this->oldCpuLoadMap.at(elem.first).at("system"));
+void
+cpuLoad::calculateCpuUsage()
+{
+ for (const auto& elem : this->cpuLoadMap)
+ {
+ if (this->cpuLoadMap.at(elem.first).at("user") <
+ this->oldCpuLoadMap.at(elem.first).at("user") ||
+ this->cpuLoadMap.at(elem.first).at("nice") <
+ this->oldCpuLoadMap.at(elem.first).at("nice") ||
+ this->cpuLoadMap.at(elem.first).at("system") <
+ this->oldCpuLoadMap.at(elem.first).at("system") ||
+ this->cpuLoadMap.at(elem.first).at("idle") <
+ this->oldCpuLoadMap.at(elem.first).at("idle"))
+ {
+ }
+ else
+ {
+ auto total = (this->cpuLoadMap.at(elem.first).at("user") -
+ this->oldCpuLoadMap.at(elem.first).at("user")) +
+ (this->cpuLoadMap.at(elem.first).at("nice") -
+ this->oldCpuLoadMap.at(elem.first).at("nice")) +
+ (this->cpuLoadMap.at(elem.first).at("system") -
+ this->oldCpuLoadMap.at(elem.first).at("system"));
double percent = total;
- total += (this->cpuLoadMap.at(elem.first).at("idle") - this->oldCpuLoadMap.at(elem.first).at("idle"));
+ total += (this->cpuLoadMap.at(elem.first).at("idle") -
+ this->oldCpuLoadMap.at(elem.first).at("idle"));
percent /= total;
percent *= 100.0;
this->cpuUsage[elem.first] = percent;
-
}
-
}
}
-std::map<std::string, std::unordered_map<std::string, uint64_t>> cpuLoad::parseStatFile(const std::string &fileName) {
+std::map<std::string, std::unordered_map<std::string, uint64_t>>
+cpuLoad::parseStatFile(const std::string& fileName)
+{
std::map<std::string, std::unordered_map<std::string, uint64_t>> cpuLoad_;
- try {
+ try
+ {
std::ifstream cpuFile(fileName);
uint32_t lineCnt = 0;
bool infoValid = true;
- for (std::string line; std::getline(cpuFile, line) && infoValid; lineCnt++) {
+ for (std::string line; std::getline(cpuFile, line) && infoValid; lineCnt++)
+ {
std::stringstream strStream(line);
std::string strPart;
std::string cpuNum;
auto it = cpuIdentifiers.begin();
std::unordered_map<std::string, uint64_t> cpuValues;
- while (strStream >> strPart) {
- if (cpuNum.empty()) {
- if (strPart.find("cpu") != std::string::npos) {
+ while (strStream >> strPart)
+ {
+ if (cpuNum.empty())
+ {
+ if (strPart.find("cpu") != std::string::npos)
+ {
cpuNum = strPart;
continue;
- } else {
+ }
+ else
+ {
infoValid = false;
break;
}
}
- if (it != cpuIdentifiers.end()) {
+ if (it != cpuIdentifiers.end())
+ {
cpuValues[it->data()] = std::stoull(strPart);
}
- if (it->data() == cpuIdentifiers.at(4)) {
+ if (it->data() == cpuIdentifiers.at(4))
+ {
break;
}
it++;
}
- if (!cpuNum.empty()) {
+ if (!cpuNum.empty())
+ {
cpuLoad_[cpuNum] = cpuValues;
}
}
- } catch (std::ifstream::failure &e) {
+ }
+ catch (std::ifstream::failure& e)
+ {
throw std::runtime_error("Exception: " + fileName + std::string(e.what()));
}
return cpuLoad_;
}
-std::string cpuLoad::getCPUName(const std::string &cpuNameFile) {
+std::string
+cpuLoad::getCPUName(const std::string& cpuNameFile)
+{
- if (!this->cpuName.empty()) {
+ if (!this->cpuName.empty())
+ {
return this->cpuName;
}
std::ifstream file;
file.open(cpuNameFile);
- if (!file.is_open()) {
+ if (!file.is_open())
+ {
throw std::runtime_error("unable to open " + cpuNameFile);
}
std::string line;
- while (std::getline(file, line)) {
- if (line.find("model name") != std::string::npos) {
+ while (std::getline(file, line))
+ {
+ if (line.find("model name") != std::string::npos)
+ {
size_t pos = line.find(':');
- if (pos != std::string::npos) {
+ if (pos != std::string::npos)
+ {
this->cpuName = line.substr(pos, line.size());
return this->cpuName;
}
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_cpuload.hpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_cpuload.hpp
index c6075c0b60d851abdf13faf71b95b91aaadd202c..1877898171de2194688f505b055ca224df14d566 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_cpuload.hpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_cpuload.hpp
@@ -8,14 +8,15 @@
*/
#pragma once
-#include <vector>
+#include <chrono>
+#include <map>
#include <string>
#include <tuple>
-#include <map>
#include <unordered_map>
-#include <chrono>
+#include <vector>
-class cpuLoad {
+class cpuLoad
+{
public:
cpuLoad() = delete;
@@ -23,8 +24,8 @@ public:
* @brief constructor
* @param procFileName
*/
- explicit cpuLoad(std::string procFileName = "/proc/stat"):
- procFile(std::move(procFileName)), cpuName("") {};
+ explicit cpuLoad(std::string procFileName = "/proc/stat") :
+ procFile(std::move(procFileName)), cpuName(""){};
/**
* @brief initialize the parsing algo
@@ -40,7 +41,9 @@ public:
* @brief get Cpu user / nice / system /idle time. used for cpu usage per process
* @return tuple<user,nice,system,idle>
*/
- std::tuple<uint64_t , uint64_t, uint64_t, uint64_t> getCpuTimes() {
+ std::tuple<uint64_t, uint64_t, uint64_t, uint64_t>
+ getCpuTimes()
+ {
auto cpuLoad_ = this->parseStatFile(this->procFile);
return std::make_tuple(cpuLoad_.at("cpu").at("user"),
cpuLoad_.at("cpu").at("nice"),
@@ -62,7 +65,8 @@ public:
private:
void calculateCpuUsage();
- std::map<std::string, std::unordered_map<std::string, uint64_t>> parseStatFile(const std::string& fileName);
+ std::map<std::string, std::unordered_map<std::string, uint64_t>>
+ parseStatFile(const std::string& fileName);
void upDateCPUUsage();
std::chrono::system_clock::time_point currentTime;
std::string procFile;
@@ -71,5 +75,3 @@ private:
std::map<std::string, std::unordered_map<std::string, uint64_t>> cpuLoadMap;
std::map<std::string, std::unordered_map<std::string, uint64_t>> oldCpuLoadMap;
};
-
-
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_memoryload.cpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_memoryload.cpp
index 8fa6cf3dcf91026cbf82c4c0cf88815b9ad73e64..616298deebe9eea0c1e9e0d84cb5685b373f63a6 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_memoryload.cpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_memoryload.cpp
@@ -7,23 +7,30 @@
*
*/
#include "linux_memoryload.hpp"
+
+#include <cinttypes>
#include <cmath>
#include <fstream>
#include <iostream>
-#include <cinttypes>
-bool memoryLoad::parseMemoryFile() {
- if(timeStamp + std::chrono::milliseconds(10) > std::chrono::steady_clock::now()) {
+
+bool
+memoryLoad::parseMemoryFile()
+{
+ if (timeStamp + std::chrono::milliseconds(10) > std::chrono::steady_clock::now())
+ {
return true;
}
std::ifstream memoryFile;
memoryFile.open(this->memInfoFile);
this->timeStamp = std::chrono::steady_clock::now();
- if (!memoryFile.is_open()) {
+ if (!memoryFile.is_open())
+ {
return false;
}
std::string line;
- while (std::getline(memoryFile, line)) {
+ while (std::getline(memoryFile, line))
+ {
sscanf(line.c_str(), "MemTotal: %" PRIu64, &this->totalMemoryInKB);
sscanf(line.c_str(), "MemAvailable: %" PRIu64, &this->currentMemoryUsageInKB);
}
@@ -31,36 +38,49 @@ bool memoryLoad::parseMemoryFile() {
return true;
}
-uint64_t memoryLoad::getTotalMemoryInKB() {
+uint64_t
+memoryLoad::getTotalMemoryInKB()
+{
this->parseMemoryFile();
return this->totalMemoryInKB;
}
-uint64_t memoryLoad::getCurrentMemUsageInKB() {
+uint64_t
+memoryLoad::getCurrentMemUsageInKB()
+{
this->parseMemoryFile();
return this->getTotalMemoryInKB() - this->currentMemoryUsageInKB;
}
-float memoryLoad::getCurrentMemUsageInPercent() {
+float
+memoryLoad::getCurrentMemUsageInPercent()
+{
this->parseMemoryFile();
uint64_t memavail = this->getCurrentMemUsageInKB();
return round((((memavail * 100.0 / this->getTotalMemoryInKB()))) * 100.0) / 100.0;
}
-uint64_t memoryLoad::getMemoryUsageByThisProcess() {
+uint64_t
+memoryLoad::getMemoryUsageByThisProcess()
+{
return this->parseProcessMemoryFile(this->memInfoOfProcessFile);
}
-uint64_t memoryLoad::getMemoryUsedByProcess(int pid) {
+uint64_t
+memoryLoad::getMemoryUsedByProcess(int pid)
+{
return memoryLoad::parseProcessMemoryFile("/proc/" + std::to_string(pid) + "/status");
}
-uint64_t memoryLoad::parseProcessMemoryFile(std::string fileToParse) {
+uint64_t
+memoryLoad::parseProcessMemoryFile(std::string fileToParse)
+{
uint64_t MemFree = 0;
std::ifstream memoryFile;
memoryFile.open(fileToParse);
std::string line;
- while (std::getline(memoryFile, line)) {
+ while (std::getline(memoryFile, line))
+ {
sscanf(line.c_str(), "VmSize: %" PRIu64, &MemFree);
}
return MemFree;
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_memoryload.hpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_memoryload.hpp
index 5b40f9baf50242127b2c96fcc28da1008c011249..b5ef50cb16edbd2e01ad77b9411ab0d38f8e17ec 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_memoryload.hpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_memoryload.hpp
@@ -8,20 +8,20 @@
*/
#pragma once
-#include <string>
#include <chrono>
+#include <string>
-class memoryLoad {
+class memoryLoad
+{
public:
explicit memoryLoad(std::string memInfo = "/proc/meminfo",
std::string memInfoOfProcess = "/proc/self/status",
- std::string memInfoOfProcessPrefix = "/proc/self/"):
- totalMemoryInKB(0),
- currentMemoryUsageInKB(0),
- memInfoFile(std::move(memInfo)),
- memInfoOfProcessFile(std::move(memInfoOfProcess)),
- memInfoOfProcessPrefixFile(std::move(memInfoOfProcessPrefix))
- {};
+ std::string memInfoOfProcessPrefix = "/proc/self/") :
+ totalMemoryInKB(0),
+ currentMemoryUsageInKB(0),
+ memInfoFile(std::move(memInfo)),
+ memInfoOfProcessFile(std::move(memInfoOfProcess)),
+ memInfoOfProcessPrefixFile(std::move(memInfoOfProcessPrefix)){};
/**
* @brief get total memory of the system in KB
* @return total memory in KB
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_networkload.cpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_networkload.cpp
index c55a1809b491a7d7fc8f59be2ba3273a8581cba3..387f6aed0f309e1da1a4108509be19b3fad4084a 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_networkload.cpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_networkload.cpp
@@ -7,71 +7,84 @@
*
*/
#include "linux_networkload.hpp"
-#include <string>
+
+#include <algorithm>
+#include <cstring>
#include <exception>
#include <fstream>
#include <iostream>
#include <list>
#include <sstream>
#include <stdexcept>
-#include <algorithm>
-#include <cstring>
+#include <string>
#include <utility>
const std::list<std::string> identifiers{"RXbytes",
- "RXpackets",
- "RXerrs",
- "RXdrop",
- "RXfifo",
- "RXframe",
- "RXcompressed",
- "RXmulticast",
- "TXbytes",
- "TXpackets",
- "TXerrs",
- "TXdrop",
- "TXfifo",
- "TXcolls",
- "TXcarrier",
- "TXcompressed"
-};
-
-std::string networkLoad::mapEnumToString(networkLoad::networkParam param) {
+ "RXpackets",
+ "RXerrs",
+ "RXdrop",
+ "RXfifo",
+ "RXframe",
+ "RXcompressed",
+ "RXmulticast",
+ "TXbytes",
+ "TXpackets",
+ "TXerrs",
+ "TXdrop",
+ "TXfifo",
+ "TXcolls",
+ "TXcarrier",
+ "TXcompressed"};
+
+std::string
+networkLoad::mapEnumToString(networkLoad::networkParam param)
+{
auto it = identifiers.begin();
- std::advance(it,static_cast<uint32_t>(param));
+ std::advance(it, static_cast<uint32_t>(param));
return it->data();
}
-
std::shared_ptr<networkLoad::networkParser> networkLoad::networkParser::inst = nullptr;
-networkLoad::networkLoad(std::string ethernetDataFileName, std::string ethName) : ethernetDataFile(std::move(ethernetDataFileName)), ethDev(std::move(ethName)) {
+networkLoad::networkLoad(std::string ethernetDataFileName, std::string ethName) :
+ ethernetDataFile(std::move(ethernetDataFileName)), ethDev(std::move(ethName))
+{
networkParser::getNetworkParser()->parse(ethernetDataFileName);
}
-void networkLoad::networkParser::parse(const std::string& netFile) {
+void
+networkLoad::networkParser::parse(const std::string& netFile)
+{
- if((this->currentTime + std::chrono::milliseconds(10)) > std::chrono::system_clock::now()) {
+ if ((this->currentTime + std::chrono::milliseconds(10)) > std::chrono::system_clock::now())
+ {
return;
- } else {
+ }
+ else
+ {
this->timeBefore = this->currentTime;
this->currentTime = std::chrono::system_clock::now();
}
std::ifstream ethFile;
- try {
+ try
+ {
ethFile.open(netFile);
- } catch (std::ifstream::failure &e) {
- throw std::runtime_error("Exception: "+ netFile + std::string(e.what()));
+ }
+ catch (std::ifstream::failure& e)
+ {
+ throw std::runtime_error("Exception: " + netFile + std::string(e.what()));
}
this->timeBefore = std::chrono::system_clock::now();
this->ethObjOld = this->ethObj;
uint32_t lineCnt = 0;
- for(std::string line; std::getline(ethFile, line); lineCnt++) {
- if(lineCnt < 2) {
+ for (std::string line; std::getline(ethFile, line); lineCnt++)
+ {
+ if (lineCnt < 2)
+ {
continue;
}
std::stringstream strStream(line);
@@ -79,14 +92,20 @@ void networkLoad::networkParser::parse(const std::string& netFile) {
std::string ifName = "";
std::unordered_map<std::string, uint64_t> ifValues;
auto it = identifiers.begin();
- while(strStream >> strPart) {
- if(ifName.empty()) {
- strPart.erase(std::remove_if(strPart.begin(),strPart.end(),[](auto c) {
- return !std::isalnum(c);
- }), strPart.end());
+ while (strStream >> strPart)
+ {
+ if (ifName.empty())
+ {
+ strPart.erase(std::remove_if(strPart.begin(),
+ strPart.end(),
+ [](auto c) { return !std::isalnum(c); }),
+ strPart.end());
ifName = strPart;
- } else {
- if(it != identifiers.end()) {
+ }
+ else
+ {
+ if (it != identifiers.end())
+ {
ifValues[it->data()] = std::stoull(strPart);
}
it++;
@@ -94,96 +113,121 @@ void networkLoad::networkParser::parse(const std::string& netFile) {
}
this->ethObj[ifName] = ifValues;
}
- if(this->ethObjOld.empty()) {
+ if (this->ethObjOld.empty())
+ {
this->ethObjOld = this->ethObj;
}
}
-const std::unordered_map<std::string, uint64_t>
-&networkLoad::networkParser::getEthObj(const std::string& ethDevice) const {
+const std::unordered_map<std::string, uint64_t>&
+networkLoad::networkParser::getEthObj(const std::string& ethDevice) const
+{
return this->ethObj.at(ethDevice);
}
-const std::unordered_map<std::string, uint64_t> &
-networkLoad::networkParser::getEthObjOld(const std::string& ethDevice) const {
+const std::unordered_map<std::string, uint64_t>&
+networkLoad::networkParser::getEthObjOld(const std::string& ethDevice) const
+{
return this->ethObjOld.at(ethDevice);
}
-
-std::list<std::string> networkLoad::networkParser::getNetworkDevices(std::string netFile) {
+std::list<std::string>
+networkLoad::networkParser::getNetworkDevices(std::string netFile)
+{
std::list<std::string> ifList;
this->parse(netFile);
- for(const auto& elem: this->ethObj) {
+ for (const auto& elem : this->ethObj)
+ {
ifList.push_back(elem.first);
}
return ifList;
}
-networkLoad::networkParser::networkParser() {
- this->currentTime = std::chrono::system_clock::now() - std::chrono::milliseconds (100);
+networkLoad::networkParser::networkParser()
+{
+ this->currentTime = std::chrono::system_clock::now() - std::chrono::milliseconds(100);
this->timeBefore = std::chrono::system_clock::now();
}
-std::shared_ptr<networkLoad::networkParser> networkLoad::networkParser::getNetworkParser() {
- if(networkLoad::networkParser::inst == nullptr) {
+std::shared_ptr<networkLoad::networkParser>
+networkLoad::networkParser::getNetworkParser()
+{
+ if (networkLoad::networkParser::inst == nullptr)
+ {
networkLoad::networkParser::inst = std::make_unique<networkLoad::networkParser>();
}
return networkLoad::networkParser::inst;
}
-const std::chrono::system_clock::time_point networkLoad::networkParser::getTimeStamp() const {
+const std::chrono::system_clock::time_point
+networkLoad::networkParser::getTimeStamp() const
+{
return this->currentTime;
}
-
-
-const std::chrono::system_clock::time_point networkLoad::networkParser::getTimeBefore() const {
+const std::chrono::system_clock::time_point
+networkLoad::networkParser::getTimeBefore() const
+{
return this->timeBefore;
}
-
-bool networkLoad::isDeviceUp() const {
+bool
+networkLoad::isDeviceUp() const
+{
return this->isDeviceAvailable;
}
-
-std::string networkLoad::getDeviceName() {
+std::string
+networkLoad::getDeviceName()
+{
return this->ethDev;
}
-std::list<std::string> networkLoad::scanNetworkDevices(const std::string& ethernetDataFile) {
+std::list<std::string>
+networkLoad::scanNetworkDevices(const std::string& ethernetDataFile)
+{
auto parser = networkParser::getNetworkParser();
return parser->getNetworkDevices(ethernetDataFile);
}
-std::string networkLoad::getBytesPerSeceondString(uint64_t bytesPerSecond) {
+std::string
+networkLoad::getBytesPerSeceondString(uint64_t bytesPerSecond)
+{
return getBytesString(bytesPerSecond) + "/s";
}
-std::string networkLoad::getBitsPerSeceondString(uint64_t bytesPerSecond) {
+std::string
+networkLoad::getBitsPerSeceondString(uint64_t bytesPerSecond)
+{
return getBitsString(bytesPerSecond) + "/s";
}
-std::string networkLoad::getBytesString(uint64_t totalBytes) {
+std::string
+networkLoad::getBytesString(uint64_t totalBytes)
+{
uint64_t Bytes = totalBytes;
uint64_t kByte = 0;
uint64_t mByte = 0;
uint64_t gByte = 0;
- if (Bytes > 1024) {
+ if (Bytes > 1024)
+ {
kByte = Bytes / 1024;
Bytes %= 1024;
}
- if (kByte > 1024) {
+ if (kByte > 1024)
+ {
mByte = kByte / 1024;
kByte %= 1024;
}
- if(mByte > 1024) {
+ if (mByte > 1024)
+ {
gByte = mByte / 1024;
- mByte %=1024;
+ mByte %= 1024;
}
- if (gByte > 0) {
+ if (gByte > 0)
+ {
std::string net;
net += std::to_string(gByte);
net += ".";
@@ -193,7 +237,8 @@ std::string networkLoad::getBytesString(uint64_t totalBytes) {
}
- if (mByte > 0) {
+ if (mByte > 0)
+ {
std::string net;
net += std::to_string(mByte);
net += ".";
@@ -202,7 +247,8 @@ std::string networkLoad::getBytesString(uint64_t totalBytes) {
return net;
}
- if (kByte > 0) {
+ if (kByte > 0)
+ {
std::string net;
net += std::to_string(kByte);
net += ".";
@@ -217,25 +263,31 @@ std::string networkLoad::getBytesString(uint64_t totalBytes) {
return net;
}
-std::string networkLoad::getBitsString(uint64_t totalBytes) {
+std::string
+networkLoad::getBitsString(uint64_t totalBytes)
+{
uint64_t Bytes = totalBytes * 8;
uint64_t kByte = 0;
uint64_t mByte = 0;
uint64_t gByte = 0;
- if (Bytes > 1024) {
+ if (Bytes > 1024)
+ {
kByte = Bytes / 1024;
Bytes %= 1024;
}
- if (kByte > 1024) {
+ if (kByte > 1024)
+ {
mByte = kByte / 1024;
kByte %= 1024;
}
- if(mByte > 1024) {
+ if (mByte > 1024)
+ {
gByte = mByte / 1024;
- mByte %=1024;
+ mByte %= 1024;
}
- if (gByte > 0) {
+ if (gByte > 0)
+ {
std::string net;
net += std::to_string(gByte);
net += ".";
@@ -245,7 +297,8 @@ std::string networkLoad::getBitsString(uint64_t totalBytes) {
}
- if (mByte > 0) {
+ if (mByte > 0)
+ {
std::string net;
net += std::to_string(mByte);
net += ".";
@@ -254,7 +307,8 @@ std::string networkLoad::getBitsString(uint64_t totalBytes) {
return net;
}
- if (kByte > 0) {
+ if (kByte > 0)
+ {
std::string net;
net += std::to_string(kByte);
net += ".";
@@ -270,37 +324,44 @@ std::string networkLoad::getBitsString(uint64_t totalBytes) {
return net;
}
-uint64_t networkLoad::getParamPerSecond(std::string designator) {
+uint64_t
+networkLoad::getParamPerSecond(std::string designator)
+{
networkParser::getNetworkParser()->parse(this->ethernetDataFile);
- if(!std::count_if(identifiers.begin(),identifiers.end(),[designator](auto elem) {
- return elem == designator;
- })) {
+ if (!std::count_if(identifiers.begin(),
+ identifiers.end(),
+ [designator](auto elem) { return elem == designator; }))
+ {
throw std::runtime_error("invalid designator: " + designator);
}
auto before = networkParser::getNetworkParser()->getTimeBefore();
auto current = networkParser::getNetworkParser()->getTimeStamp();
- auto msec = std::chrono::duration_cast<std::chrono::milliseconds> (
- before - current);
+ auto msec = std::chrono::duration_cast<std::chrono::milliseconds>(before - current);
uint64_t Bytes = (networkParser::getNetworkParser()->getEthObj(this->ethDev).at(designator) -
- networkParser::getNetworkParser()->getEthObjOld(this->ethDev).at(designator));
- if (static_cast<unsigned long>(msec.count()) <= 0) {
+ networkParser::getNetworkParser()->getEthObjOld(this->ethDev).at(designator));
+ if (static_cast<unsigned long>(msec.count()) <= 0)
+ {
Bytes /= 1;
- } else {
+ }
+ else
+ {
Bytes /= static_cast<unsigned long>(msec.count());
}
return Bytes;
}
-uint64_t networkLoad::getParamSinceStartup(std::string designator) {
+uint64_t
+networkLoad::getParamSinceStartup(std::string designator)
+{
networkParser::getNetworkParser()->parse(this->ethernetDataFile);
auto ifObj = networkParser::getNetworkParser()->getEthObj(this->ethDev);
- if(!std::count_if(identifiers.begin(),identifiers.end(),[designator](auto elem) {
- return elem == designator;
- })) {
+ if (!std::count_if(identifiers.begin(),
+ identifiers.end(),
+ [designator](auto elem) { return elem == designator; }))
+ {
throw std::runtime_error("invalid designator: " + designator);
}
return ifObj[designator];
}
-
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_networkload.hpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_networkload.hpp
index 95dcbde9b85a08cb5e4dac638a3eaf7606e0baaf..c4ed753c571223f8b97d921e704698abcb030aaa 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_networkload.hpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_networkload.hpp
@@ -9,28 +9,34 @@
#pragma once
+#include <chrono>
+#include <list>
#include <map>
#include <memory>
#include <string>
-#include <list>
-#include <vector>
-#include <chrono>
#include <unordered_map>
+#include <vector>
-
-class networkLoad {
+class networkLoad
+{
public:
- static std::list<std::string> scanNetworkDevices(const std::string& ethernetDataFile= "/proc/net/dev");
- static std::vector<std::shared_ptr<networkLoad>> createLinuxEthernetScanList(const std::string& ethernetDataFileName = "/proc/net/dev") {
+ static std::list<std::string>
+ scanNetworkDevices(const std::string& ethernetDataFile = "/proc/net/dev");
+
+ static std::vector<std::shared_ptr<networkLoad>>
+ createLinuxEthernetScanList(const std::string& ethernetDataFileName = "/proc/net/dev")
+ {
std::vector<std::shared_ptr<networkLoad>> v;
- for (const auto& elem: networkLoad::scanNetworkDevices(ethernetDataFileName)) {
- v.push_back(std::make_shared<networkLoad>(ethernetDataFileName,elem));
+ for (const auto& elem : networkLoad::scanNetworkDevices(ethernetDataFileName))
+ {
+ v.push_back(std::make_shared<networkLoad>(ethernetDataFileName, elem));
}
return v;
}
- explicit networkLoad(std::string ethernetDataFileName = "/proc/net/dev", std::string ethName = "eth0");
+ explicit networkLoad(std::string ethernetDataFileName = "/proc/net/dev",
+ std::string ethName = "eth0");
uint64_t getParamPerSecond(std::string designator);
uint64_t getParamSinceStartup(std::string designator);
@@ -42,9 +48,8 @@ public:
bool isDeviceUp() const;
std::string getDeviceName();
-
-
- enum networkParam {
+ enum networkParam
+ {
RXbytes = 0,
RXpackets,
RXerrs,
@@ -62,15 +67,16 @@ public:
TXcarrier,
TXcompressed
};
+
static std::string mapEnumToString(networkLoad::networkParam param);
-private:
+private:
std::string ethernetDataFile;
std::string ethDev;
bool isDeviceAvailable = false;
-
- class networkParser {
+ class networkParser
+ {
private:
std::chrono::system_clock::time_point currentTime;
std::chrono::system_clock::time_point timeBefore;
@@ -82,15 +88,14 @@ private:
static std::shared_ptr<networkParser> getNetworkParser();
networkParser();
void parse(const std::string& netFile = "/proc/net/dev");
- const std::unordered_map<std::string, uint64_t> &getEthObj(const std::string& ethDevice) const;
+ const std::unordered_map<std::string, uint64_t>&
+ getEthObj(const std::string& ethDevice) const;
std::list<std::string> getNetworkDevices(std::string netFile = "/proc/net/dev");
const std::chrono::system_clock::time_point getTimeStamp() const;
- const std::unordered_map<std::string, uint64_t> &getEthObjOld(const std::string& ethDevice) const;
+ const std::unordered_map<std::string, uint64_t>&
+ getEthObjOld(const std::string& ethDevice) const;
const std::chrono::system_clock::time_point getTimeBefore() const;
static std::shared_ptr<networkParser> inst;
};
-
-
};
-
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.cpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.cpp
index b460e18e3a9649d6e1048ed98491b678f3034cb3..ba8badab57f2d6c5fe872300c03566803f881491 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.cpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.cpp
@@ -8,85 +8,51 @@
*/
#include "linux_process_load.hpp"
-#include <list>
-#include <memory>
-#include <iostream>
+
#include <algorithm>
#include <filesystem>
#include <fstream>
+#include <iostream>
+#include <list>
+#include <memory>
+
#include "linux_cpuload.hpp"
#include "linux_memoryload.hpp"
-static const std::list<std::string> stats {"pid",
- "comm",
- "state",
- "ppid",
- "pgrp",
- "session",
- "tty_nr",
- "tpgid",
- "flags",
- "minflt",
- "cminflt",
- "majflt",
- "cmajflt",
- "utime",
- "stime",
- "cutime",
- "cstime",
- "priority",
- "nice",
- "num_threads",
- "itrealvalue",
- "starttime",
- "vsize",
- "rss",
- "rsslim",
- "startcode",
- "endcode",
- "startstack",
- "kstkesp",
- "kstkeip",
- "signal",
- "blocked",
- "sigignore",
- "sigcatch",
- "wchan",
- "nswap",
- "cnswap",
- "exit_signal",
- "processor",
- "rt_priority",
- "policy",
- "delaycct_blkio_ticks",
- "guest_time",
- "cguest_time",
- "start_data",
- "end_data",
- "start_brk",
- "arg_start",
- "arg_end",
- "env_start",
- "env_end",
- "exit_code"};
-
-std::map<std::string, double> linuxProcessLoad::getProcessCpuLoad() {
+static const std::list<std::string> stats{
+ "pid", "comm", "state", "ppid", "pgrp", "session",
+ "tty_nr", "tpgid", "flags", "minflt", "cminflt", "majflt",
+ "cmajflt", "utime", "stime", "cutime", "cstime", "priority",
+ "nice", "num_threads", "itrealvalue", "starttime", "vsize", "rss",
+ "rsslim", "startcode", "endcode", "startstack", "kstkesp", "kstkeip",
+ "signal", "blocked", "sigignore", "sigcatch", "wchan", "nswap",
+ "cnswap", "exit_signal", "processor", "rt_priority", "policy", "delaycct_blkio_ticks",
+ "guest_time", "cguest_time", "start_data", "end_data", "start_brk", "arg_start",
+ "arg_end", "env_start", "env_end", "exit_code"};
+
+std::map<std::string, double>
+linuxProcessLoad::getProcessCpuLoad()
+{
this->findProcesses();
return this->procCPUUsage;
}
+void
+linuxProcessLoad::calculateProcessLoad()
+{
+ auto [cpuTotalUserTime, cpuTotalUserLowTime, cpuTotalSysTime, cpuTotalIdleTime] = CpuTimes;
-void linuxProcessLoad::calculateProcessLoad() {
- auto [ cpuTotalUserTime, cpuTotalUserLowTime, cpuTotalSysTime, cpuTotalIdleTime] = CpuTimes;
-
- auto [ oldCpuTotalUserTime, oldCpuTotalUserLowTime, oldCpuTotalSysTime, oldCpuTotalIdleTime] = oldCpuTimes;
+ auto [oldCpuTotalUserTime, oldCpuTotalUserLowTime, oldCpuTotalSysTime, oldCpuTotalIdleTime] =
+ oldCpuTimes;
auto TotalUserTime = cpuTotalUserTime - oldCpuTotalUserTime;
auto TotalSysTime = cpuTotalSysTime - oldCpuTotalSysTime;
this->procCPUUsage.clear();
- for(const auto& elem: processStat) {
+ for (const auto& elem : processStat)
+ {
auto pid = elem.first;
- try {
+ try
+ {
auto oldProc = oldProcessStat.at(pid);
auto proc = elem.second;
auto procName = proc.at("comm");
@@ -95,20 +61,24 @@ void linuxProcessLoad::calculateProcessLoad() {
cpuTime += (std::stoull(proc.at("stime")) - std::stoull(oldProc.at("stime")));
- double percentage = ((static_cast<double>(cpuTime) *100.0) / static_cast<double>((TotalUserTime + TotalSysTime)) );
+ double percentage = ((static_cast<double>(cpuTime) * 100.0) /
+ static_cast<double>((TotalUserTime + TotalSysTime)));
- if(percentage > 0.1) {
+ if (percentage > 0.1)
+ {
this->procCPUUsage[procName] = percentage;
}
-
- } catch(...) {
- std::cerr << "process: " << pid << " disappeared in meantime" << std::endl;
+ }
+ catch (...)
+ {
+ std::cerr << "process: " << pid << " disappeared in meantime" << std::endl;
}
}
}
-
-void linuxProcessLoad::findProcesses() {
+void
+linuxProcessLoad::findProcesses()
+{
auto cpuMonitoring = std::make_unique<cpuLoad>("/proc/stat");
@@ -117,14 +87,17 @@ void linuxProcessLoad::findProcesses() {
std::string path{"/proc/"};
std::vector<std::string> processes;
- for(auto& elem: std::filesystem::directory_iterator(path)) {
+ for (auto& elem : std::filesystem::directory_iterator(path))
+ {
auto procPath = elem.path().string();
- if(elem.exists()) {
- procPath.erase(procPath.begin(), procPath.begin() + static_cast<int32_t >(path.size()));
- if (std::isdigit(procPath.at(0))) {
- if (!std::count_if(procPath.begin(), procPath.end(), [](auto c) {
- return std::isalpha(c);
- })) {
+ if (elem.exists())
+ {
+ procPath.erase(procPath.begin(), procPath.begin() + static_cast<int32_t>(path.size()));
+ if (std::isdigit(procPath.at(0)))
+ {
+ if (!std::count_if(
+ procPath.begin(), procPath.end(), [](auto c) { return std::isalpha(c); }))
+ {
parseProcess(procPath);
}
}
@@ -136,48 +109,58 @@ void linuxProcessLoad::findProcesses() {
this->oldCpuTimes = this->CpuTimes;
}
-void linuxProcessLoad::parseProcess(const std::string& pid) {
- std::string path {"/proc/" + pid + "/stat"};
+void
+linuxProcessLoad::parseProcess(const std::string& pid)
+{
+ std::string path{"/proc/" + pid + "/stat"};
std::ifstream ethFile(path);
std::string strPart;
std::unordered_map<std::string, std::string> procStat;
auto identifierStart = stats.begin();
- enum state {
+
+ enum state
+ {
normal,
isProcessParse
};
+
bool isProcessFound = false;
- while (ethFile >> strPart) {
+ while (ethFile >> strPart)
+ {
- if(identifierStart != stats.end()) {
- if(isProcessFound) {
+ if (identifierStart != stats.end())
+ {
+ if (isProcessFound)
+ {
procStat[identifierStart->data()] += " " + strPart;
}
- if(std::count_if(strPart.begin(),strPart.end(),[](auto c) { return c == '('; })) {
+ if (std::count_if(strPart.begin(), strPart.end(), [](auto c) { return c == '('; }))
+ {
isProcessFound = true;
procStat[identifierStart->data()] = strPart;
}
- if(!isProcessFound) {
+ if (!isProcessFound)
+ {
procStat[identifierStart->data()] = strPart;
}
- if(std::count_if(strPart.begin(), strPart.end(), [] (auto c) { return c == ')';})) {
+ if (std::count_if(strPart.begin(), strPart.end(), [](auto c) { return c == ')'; }))
+ {
isProcessFound = false;
}
-
}
- if(!isProcessFound) {
+ if (!isProcessFound)
+ {
identifierStart++;
continue;
}
-
}
- procStat["comm"].erase(std::remove_if(procStat["comm"].begin(),procStat["comm"].end(), [](auto c) {
- return c == '(' || c == ')';
- }), procStat["comm"].end());
+ procStat["comm"].erase(std::remove_if(procStat["comm"].begin(),
+ procStat["comm"].end(),
+ [](auto c) { return c == '(' || c == ')'; }),
+ procStat["comm"].end());
processStat[pid] = procStat;
}
-
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp
index ab76f86ff23c9b47c402bc7797c7a6d72b99a640..9a0cd6ebfc3eb72963fd790c0d9930c7001237b7 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp
@@ -15,7 +15,8 @@
#include <tuple>
#include <unordered_map>
-class linuxProcessLoad {
+class linuxProcessLoad
+{
public:
/**
@@ -26,7 +27,6 @@ public:
std::map<std::string, double> getProcessCpuLoad();
-
private:
void parseProcess(const std::string& pid);
void findProcesses();
@@ -34,8 +34,6 @@ private:
std::map<std::string, double> procCPUUsage;
std::tuple<uint64_t, uint64_t, uint64_t, uint64_t> oldCpuTimes;
std::tuple<uint64_t, uint64_t, uint64_t, uint64_t> CpuTimes;
- std::map<std::string,std::unordered_map<std::string, std::string>> processStat;
- std::map<std::string,std::unordered_map<std::string, std::string>> oldProcessStat;
+ std::map<std::string, std::unordered_map<std::string, std::string>> processStat;
+ std::map<std::string, std::unordered_map<std::string, std::string>> oldProcessStat;
};
-
-
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_systemutil.cpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_systemutil.cpp
index 8164ea09ed81eb4239da0c86a7e6222864e84f1c..2ae035cbfca71380793db88e27a5a8648b90ff17 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_systemutil.cpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_systemutil.cpp
@@ -6,60 +6,71 @@
* Copyright (c) Daniel Fuchs
*
*/
-#include <cstdlib>
-#include <unistd.h>
-#include <cstdio>
-#include <dirent.h>
-#include <fstream>
-#include <sys/types.h>
+#include "linux_systemutil.hpp"
+
+#include <cinttypes>
#include <csignal>
+#include <cstdio>
+#include <cstdlib>
#include <cstring>
-#include <netinet/in.h>
-#include <sys/statvfs.h>
-#include <sys/stat.h>
-#include <pwd.h>
+#include <fstream>
#include <thread>
-#include "linux_systemutil.hpp"
-#include <cinttypes>
+#include <dirent.h>
+#include <netinet/in.h>
+#include <pwd.h>
+#include <sys/stat.h>
+#include <sys/statvfs.h>
+#include <sys/types.h>
+#include <unistd.h>
-int64_t linuxUtil::getTemperature(const std::string &thermalZone) {
+int64_t
+linuxUtil::getTemperature(const std::string& thermalZone)
+{
std::ifstream temperatureFile;
const std::string parsePath = "/sys/class/thermal/" + thermalZone + "/temp";
temperatureFile.open(parsePath);
int64_t temperature;
std::string line;
- while (std::getline(temperatureFile, line)) {
+ while (std::getline(temperatureFile, line))
+ {
auto unused = scanf(line.c_str(), "%ld", &temperature);
- (void) unused;
+ (void)unused;
}
temperatureFile.close();
return temperature;
}
-int linuxUtil::getProcIdByName(const std::string &procName) {
+int
+linuxUtil::getProcIdByName(const std::string& procName)
+{
int pid = -1;
auto dp = opendir("/proc");
- if (dp != nullptr) {
- struct dirent *dirp;
- while (pid < 0 && (dirp = readdir(dp))) {
+ if (dp != nullptr)
+ {
+ struct dirent* dirp;
+ while (pid < 0 && (dirp = readdir(dp)))
+ {
int id = atoi(dirp->d_name);
- if (id > 0) {
+ if (id > 0)
+ {
std::string cmdPath{"/proc/"};
cmdPath.append(dirp->d_name);
cmdPath.append("/cmdline");
std::ifstream cmdFile(cmdPath.c_str());
std::string cmdLine;
getline(cmdFile, cmdLine);
- if (!cmdLine.empty()) {
+ if (!cmdLine.empty())
+ {
size_t pos = cmdLine.find('\0');
if (pos != std::string::npos)
cmdLine = cmdLine.substr(0, pos);
pos = cmdLine.rfind('/');
if (pos != std::string::npos)
cmdLine = cmdLine.substr(pos + 1);
- if (strcmp(procName.c_str(), cmdLine.c_str()) == 0) {
+ if (strcmp(procName.c_str(), cmdLine.c_str()) == 0)
+ {
pid = id;
}
}
@@ -70,40 +81,50 @@ int linuxUtil::getProcIdByName(const std::string &procName) {
return pid;
}
-int linuxUtil::killProcessById(int pid, const std::string &procName) {
- if (pid == -1) {
- throw std::runtime_error(
- "Nothing to Kill, no Process " + procName + " PID " + std::to_string(pid));
+int
+linuxUtil::killProcessById(int pid, const std::string& procName)
+{
+ if (pid == -1)
+ {
+ throw std::runtime_error("Nothing to Kill, no Process " + procName + " PID " +
+ std::to_string(pid));
}
int ret = kill(pid, 9);
- if (ret == -1) {
+ if (ret == -1)
+ {
throw std::runtime_error("killing " + procName + " was not successful!");
}
return ret;
}
-uint64_t linuxUtil::getSysUpTime() {
+uint64_t
+linuxUtil::getSysUpTime()
+{
std::ifstream upTimeFile;
upTimeFile.open("/proc/uptime");
- if (!upTimeFile.is_open()) {
+ if (!upTimeFile.is_open())
+ {
return 0;
}
uint64_t beforeBootTime;
uint64_t sysUptime = 0;
std::string line;
- while (std::getline(upTimeFile, line)) {
+ while (std::getline(upTimeFile, line))
+ {
sscanf(line.c_str(), "%" PRIu64 "%" PRIu64, &sysUptime, &beforeBootTime);
}
upTimeFile.close();
return sysUptime;
}
-
-bool linuxUtil::startAppAsDaemon() {
+bool
+linuxUtil::startAppAsDaemon()
+{
pid_t pid = fork();
- if (pid < 0) {
+ if (pid < 0)
+ {
return false;
}
@@ -126,26 +147,31 @@ bool linuxUtil::startAppAsDaemon() {
umask(0);
auto retval = chdir("/test");
- if (retval < 0) {
+ if (retval < 0)
+ {
return false;
}
- for (int x = sysconf(_SC_OPEN_MAX); x >= 0; x--) {
+ for (int x = sysconf(_SC_OPEN_MAX); x >= 0; x--)
+ {
close(x);
}
return true;
}
-uint64_t linuxUtil::getFreeDiskSpace(std::string absoluteFilePath) {
+uint64_t
+linuxUtil::getFreeDiskSpace(std::string absoluteFilePath)
+{
struct statvfs buf;
- if (!statvfs(absoluteFilePath.c_str(), &buf)) {
+ if (!statvfs(absoluteFilePath.c_str(), &buf))
+ {
uint64_t blksize, blocks, freeblks, disk_size, used, free;
- std::cout << "blksize :" << buf.f_bsize << std::endl;
- std::cout << "blocks : " << buf.f_blocks;
+ std::cout << "blksize :" << buf.f_bsize << std::endl;
+ std::cout << "blocks : " << buf.f_blocks;
std::cout << "bfree : " << buf.f_bfree;
std::cout << "bavail: " << buf.f_bavail;
- std::cout << "f_frsize: " << buf.f_frsize;
+ std::cout << "f_frsize: " << buf.f_frsize;
blksize = buf.f_bsize;
blocks = buf.f_blocks;
freeblks = buf.f_bfree;
@@ -153,34 +179,39 @@ uint64_t linuxUtil::getFreeDiskSpace(std::string absoluteFilePath) {
free = freeblks * blksize;
used = disk_size - free;
- std::cout << "disk " << absoluteFilePath
- << " disksize: " << disk_size
- << " free: " << free
- << " used: " << used
- << std::endl;
+ std::cout << "disk " << absoluteFilePath << " disksize: " << disk_size << " free: " << free
+ << " used: " << used << std::endl;
return free;
- } else {
+ }
+ else
+ {
return 0;
}
return 0;
}
-uint64_t linuxUtil::userAvailableFreeSpace() {
+uint64_t
+linuxUtil::userAvailableFreeSpace()
+{
struct statvfs stat;
- struct passwd *pw = getpwuid(getuid());
- if (nullptr != pw && 0 == statvfs(pw->pw_dir, &stat)) {
- std::cout << "path " << pw->pw_dir << std::endl;
+ struct passwd* pw = getpwuid(getuid());
+ if (nullptr != pw && 0 == statvfs(pw->pw_dir, &stat))
+ {
+ std::cout << "path " << pw->pw_dir << std::endl;
uint64_t freeBytes = stat.f_bavail * stat.f_frsize;
return freeBytes;
}
return 0ULL;
}
-std::string linuxUtil::getOSVersion_Signature(void) {
+std::string
+linuxUtil::getOSVersion_Signature(void)
+{
std::ifstream versionFile;
versionFile.open("/proc/version_signature");
- if (!versionFile.is_open()) {
+ if (!versionFile.is_open())
+ {
return std::string();
}
std::string line;
@@ -190,11 +221,14 @@ std::string linuxUtil::getOSVersion_Signature(void) {
return line;
}
-std::string linuxUtil::getOsVersionString(void) {
+std::string
+linuxUtil::getOsVersionString(void)
+{
std::ifstream versionFile;
versionFile.open("/proc/version");
- if (!versionFile.is_open()) {
+ if (!versionFile.is_open())
+ {
return std::string();
}
std::string line;
@@ -204,24 +238,30 @@ std::string linuxUtil::getOsVersionString(void) {
return line;
}
-bool linuxUtil::isDeviceOnline(std::string address) {
+bool
+linuxUtil::isDeviceOnline(std::string address)
+{
const std::string processPrefix = {"ping -c 1 -w 1 "};
const std::string processPostfix = {" 2>&1"};
auto fd = popen((processPrefix + address + processPostfix).c_str(), "r");
std::this_thread::sleep_for(std::chrono::seconds(2));
- if (fd == nullptr) {
+ if (fd == nullptr)
+ {
return false;
}
char buff[1000];
- char *ptr = buff;
+ char* ptr = buff;
size_t sz = sizeof(buff);
- while (getline(&ptr, &sz, fd) != -1) {
+ while (getline(&ptr, &sz, fd) != -1)
+ {
std::string line(buff);
- if (line.find(" 1 received") != std::string::npos) {
+ if (line.find(" 1 received") != std::string::npos)
+ {
pclose(fd);
return true;
}
- if (line.find("100% packet loss") != std::string::npos) {
+ if (line.find("100% packet loss") != std::string::npos)
+ {
pclose(fd);
return false;
}
@@ -229,29 +269,37 @@ bool linuxUtil::isDeviceOnline(std::string address) {
return false;
}
-uint32_t linuxUtil::getNumOfThreadsByThisProcess() {
+uint32_t
+linuxUtil::getNumOfThreadsByThisProcess()
+{
uint32_t Threads = 0;
std::ifstream memoryFile;
memoryFile.open("/proc/self/status");
std::string line;
- while (std::getline(memoryFile, line)) {
+ while (std::getline(memoryFile, line))
+ {
sscanf(line.c_str(), "Threads: %u", &Threads);
}
return Threads;
}
-uint32_t linuxUtil::getNumOfThreadsByPID(int Pid) {
+uint32_t
+linuxUtil::getNumOfThreadsByPID(int Pid)
+{
uint32_t Threads = 0;
std::ifstream memoryFile;
memoryFile.open("/proc/self/" + std::to_string(Pid));
std::string line;
- while (std::getline(memoryFile, line)) {
+ while (std::getline(memoryFile, line))
+ {
sscanf(line.c_str(), "Threads: %u", &Threads);
}
return Threads;
}
-std::string linuxUtil::getIFaceMacAddress(std::string deviceName) {
+std::string
+linuxUtil::getIFaceMacAddress(std::string deviceName)
+{
const std::string sysClassPath = "/sys/class/net/";
std::ifstream IFaceFile;
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_systemutil.hpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_systemutil.hpp
index fe2c1a2e710134735c057b0e69ba24c7927f102b..e70cc7c679e7a92ef5ab8dcac132b851c3706c57 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_systemutil.hpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_systemutil.hpp
@@ -11,12 +11,10 @@
#include <iostream>
#include <string>
-
-
-class linuxUtil {
+class linuxUtil
+{
public:
-
static bool isDeviceOnline(std::string address);
static std::string getOSVersion_Signature(void);
static std::string getOsVersionString(void);
@@ -25,10 +23,9 @@ public:
static int getProcIdByName(const std::string& procName);
static bool startAppAsDaemon();
static uint64_t userAvailableFreeSpace();
- static int64_t getTemperature(const std::string& thermalZone= "thermal_zone0");
+ static int64_t getTemperature(const std::string& thermalZone = "thermal_zone0");
static uint64_t getFreeDiskSpace(std::string absoluteFilePath);
static uint64_t getSysUpTime();
static uint32_t getNumOfThreadsByThisProcess();
static uint32_t getNumOfThreadsByPID(int Pid);
};
-
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/ip_iterator.hpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/ip_iterator.hpp
index b095e561e680961ae1c98f6ef2e7cf0d54203e0a..fc1fb6180bbdfebbb3806e037df50577841caca6 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/ip_iterator.hpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/ip_iterator.hpp
@@ -10,50 +10,59 @@
#include <algorithm>
-
-class ipv4_Range {
+class ipv4_Range
+{
public:
- class ipv4_Address {
+ class ipv4_Address
+ {
public:
-
- ipv4_Address() {
-
+ ipv4_Address()
+ {
}
ipv4_Address(uint8_t p1, uint8_t p2, uint8_t p3, uint8_t p4) :
- ipv4_part1(p1), ipv4_part2(p2), ipv4_part3(p3), ipv4_part4(p4) {
+ ipv4_part1(p1), ipv4_part2(p2), ipv4_part3(p3), ipv4_part4(p4)
+ {
convertU8toU32();
}
- std::string toString() const {
+ std::string
+ toString() const
+ {
return std::to_string(static_cast<int>(ipv4_part1)) + "." +
std::to_string(static_cast<int>(ipv4_part2)) + "." +
std::to_string(static_cast<int>(ipv4_part3)) + "." +
std::to_string(static_cast<int>(ipv4_part4));
}
- bool operator==(const ipv4_Address &rhs) const {
+ bool
+ operator==(const ipv4_Address& rhs) const
+ {
return ipv4 == rhs.ipv4;
}
- bool operator>(const ipv4_Address &rhs) const {
+ bool
+ operator>(const ipv4_Address& rhs) const
+ {
return ipv4 > rhs.ipv4;
}
- bool operator<(const ipv4_Address &rhs) const {
+ bool
+ operator<(const ipv4_Address& rhs) const
+ {
return ipv4 < rhs.ipv4;
}
- ipv4_Address operator=(ipv4_Address &adr) {
- ipv4_Address newAdr(adr.ipv4_part1,
- adr.ipv4_part2,
- adr.ipv4_part3,
- adr.ipv4_part4
- );
+ ipv4_Address
+ operator=(ipv4_Address& adr)
+ {
+ ipv4_Address newAdr(adr.ipv4_part1, adr.ipv4_part2, adr.ipv4_part3, adr.ipv4_part4);
return newAdr;
}
- ipv4_Address operator++(int) {
+ ipv4_Address
+ operator++(int)
+ {
ipv4_Address i = *this;
ipv4 += 1;
convertU32toU8();
@@ -67,72 +76,96 @@ public:
uint8_t ipv4_part4;
uint32_t ipv4;
- void convertU32toU8() {
+ void
+ convertU32toU8()
+ {
ipv4_part1 = static_cast<uint8_t>(ipv4 >> 24);
ipv4_part2 = static_cast<uint8_t>(ipv4 >> 16);
ipv4_part3 = static_cast<uint8_t>(ipv4 >> 8);
ipv4_part4 = static_cast<uint8_t>(ipv4);
-
}
- void convertU8toU32() {
+ void
+ convertU8toU32()
+ {
ipv4 = (static_cast<uint32_t>(ipv4_part1) << 24) +
(static_cast<uint32_t>(ipv4_part2) << 16) +
- (static_cast<uint32_t>(ipv4_part3) << 8) +
- static_cast<uint32_t>(ipv4_part4);
+ (static_cast<uint32_t>(ipv4_part3) << 8) + static_cast<uint32_t>(ipv4_part4);
}
-
};
-
- class iterator : public std::iterator<
- std::input_iterator_tag,
- ipv4_Address,
- ipv4_Address,
- const ipv4_Address *,
- ipv4_Address> {
+ class iterator :
+ public std::iterator<std::input_iterator_tag,
+ ipv4_Address,
+ ipv4_Address,
+ const ipv4_Address*,
+ ipv4_Address>
+ {
ipv4_Address start;
+
public:
- explicit iterator(ipv4_Address startAdr) : start(startAdr) {}
+ explicit iterator(ipv4_Address startAdr) : start(startAdr)
+ {
+ }
- iterator &operator++() {
+ iterator&
+ operator++()
+ {
start++;
return *this;
}
- bool operator!=(iterator other) const {
+ bool
+ operator!=(iterator other) const
+ {
return !(start == other.start);
}
- bool operator==(iterator other) const {
+ bool
+ operator==(iterator other) const
+ {
return start == other.start;
}
- reference operator*() const { return start; }
+ reference
+ operator*() const
+ {
+ return start;
+ }
};
-
- ipv4_Range(ipv4_Address start, ipv4_Address end) : startAddress(start), endAddress(end) {
+ ipv4_Range(ipv4_Address start, ipv4_Address end) : startAddress(start), endAddress(end)
+ {
validate();
}
- iterator begin() {
+ iterator
+ begin()
+ {
return iterator(startAddress);
}
- iterator end() {
+ iterator
+ end()
+ {
return iterator(endAddress++);
}
private:
- void validate() {
- if (startAddress == endAddress) {
+ void
+ validate()
+ {
+ if (startAddress == endAddress)
+ {
throw std::invalid_argument("start and end are equal");
}
- if (startAddress > endAddress) {
+ if (startAddress > endAddress)
+ {
// ok
- } else {
+ }
+ else
+ {
// switch
ipv4_Address temp = startAddress;
startAddress = endAddress;
@@ -140,9 +173,6 @@ private:
}
}
-
ipv4_Address startAddress;
ipv4_Address endAddress;
-
-
};
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/record_value.hpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/record_value.hpp
index 6612be090af1f479eaa102ebbb3c4843f90875c6..128a5a691afb4104e86f781f59c64853679c935e 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/record_value.hpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/record_value.hpp
@@ -8,31 +8,34 @@
*/
#pragma once
-#include <chrono>
-#include <vector>
#include <algorithm>
-#include <numeric>
+#include <chrono>
#include <iostream>
+#include <numeric>
+#include <vector>
-
-template<typename T>
-class recordValue {
+template <typename T>
+class recordValue
+{
public:
- recordValue(std::chrono::system_clock::duration observTime, std::chrono::system_clock::duration upDateTime)
- : stepSize(
- static_cast<uint64_t>(observTime / upDateTime)), firstTime(true) {
- if(stepSize == 0) {
- throw std::runtime_error("stepsize is 0 -- not allowed: observTime: "
- + std::to_string(observTime.count())
- + " upDateTime: "
- + std::to_string(upDateTime.count()));
+ recordValue(std::chrono::system_clock::duration observTime,
+ std::chrono::system_clock::duration upDateTime) :
+ stepSize(static_cast<uint64_t>(observTime / upDateTime)), firstTime(true)
+ {
+ if (stepSize == 0)
+ {
+ throw std::runtime_error(
+ "stepsize is 0 -- not allowed: observTime: " + std::to_string(observTime.count()) +
+ " upDateTime: " + std::to_string(upDateTime.count()));
}
this->recordContainer.resize(stepSize);
};
- explicit recordValue(uint64_t stepSize_) : stepSize(stepSize_), firstTime(true) {
- if(stepSize_ == 0) {
+ explicit recordValue(uint64_t stepSize_) : stepSize(stepSize_), firstTime(true)
+ {
+ if (stepSize_ == 0)
+ {
throw std::runtime_error("stepsize is 0 -- not allowed!");
}
this->recordContainer.resize(stepSize);
@@ -40,41 +43,53 @@ public:
recordValue() = delete;
- void addRecord(const T &rec) {
+ void
+ addRecord(const T& rec)
+ {
this->recordContainer.push_back(rec);
- if (this->firstTime) {
+ if (this->firstTime)
+ {
std::fill(this->recordContainer.begin(), this->recordContainer.end(), rec);
this->firstTime = false;
}
- if (this->recordContainer.size() >= this->stepSize) {
+ if (this->recordContainer.size() >= this->stepSize)
+ {
this->recordContainer.erase(this->recordContainer.begin());
}
}
- T getMinRecord() const {
- return static_cast<T>(*std::min_element(this->recordContainer.begin(), this->recordContainer.end()));
+ T
+ getMinRecord() const
+ {
+ return static_cast<T>(
+ *std::min_element(this->recordContainer.begin(), this->recordContainer.end()));
}
- T getMaxRecord() const {
- return static_cast<T>(*std::max_element(this->recordContainer.begin(), this->recordContainer.end()));
+ T
+ getMaxRecord() const
+ {
+ return static_cast<T>(
+ *std::max_element(this->recordContainer.begin(), this->recordContainer.end()));
}
- T getAverageRecord() const {
- return static_cast<T>(std::accumulate(this->recordContainer.begin(), this->recordContainer.end(), 0.0) /
- this->stepSize);
+ T
+ getAverageRecord() const
+ {
+ return static_cast<T>(
+ std::accumulate(this->recordContainer.begin(), this->recordContainer.end(), 0.0) /
+ this->stepSize);
}
- std::vector<T> getRecordContainer() const {
+ std::vector<T>
+ getRecordContainer() const
+ {
return recordContainer;
}
~recordValue() = default;
private:
-
std::vector<T> recordContainer;
uint64_t stepSize;
bool firstTime;
};
-
-
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/timer.hpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/timer.hpp
index a7a276f0c01def6d62ce7c3f5af2911ed672c142..f3a1ec3310996bd49a8c7a84fa07f71c693e582a 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/timer.hpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/util/timer.hpp
@@ -8,24 +8,25 @@
*/
#pragma once
-#include <iostream>
+#include <atomic>
#include <chrono>
-#include <thread>
+#include <condition_variable>
#include <functional>
+#include <future>
+#include <iostream>
#include <mutex>
-#include <condition_variable>
-#include <atomic>
+#include <thread>
#include <tuple>
-#include <future>
#include <utility>
-enum timer_mode {
+enum timer_mode
+{
singleshot,
continuous
};
-
-class ITimerObserver {
+class ITimerObserver
+{
public:
virtual ~ITimerObserver() = default;
@@ -34,114 +35,141 @@ public:
using slot = std::function<void()>;
-class ITimerSubject {
+class ITimerSubject
+{
public:
virtual ~ITimerSubject() = default;
- virtual void Attach(ITimerObserver *observer, timer_mode mode, std::chrono::milliseconds ms) = 0;
+ virtual void
+ Attach(ITimerObserver* observer, timer_mode mode, std::chrono::milliseconds ms) = 0;
virtual void Attach(slot functionPtr, timer_mode mode, std::chrono::milliseconds ms) = 0;
- virtual void Detach(ITimerObserver *observer) = 0;
+ virtual void Detach(ITimerObserver* observer) = 0;
};
-class IObservable {
+class IObservable
+{
public:
- virtual ~IObservable() = default;
+ virtual ~IObservable() = default;
virtual void notify() = 0;
virtual bool toDestroy() = 0;
-
};
+class Timer : ITimerSubject
+{
-class Timer : ITimerSubject {
-
- class TimerObservable : public IObservable {
+ class TimerObservable : public IObservable
+ {
public:
- ~TimerObservable() override {
- };
-
- TimerObservable(ITimerObserver *observer, timer_mode mode, std::chrono::milliseconds ms) :
- timerMode(mode),
- m_observable(observer),
- interval(ms),
- isFinish(false) {
+ ~TimerObservable() override{};
+
+ TimerObservable(ITimerObserver* observer, timer_mode mode, std::chrono::milliseconds ms) :
+ timerMode(mode), m_observable(observer), interval(ms), isFinish(false)
+ {
this->nextExecution = std::chrono::system_clock::now() + ms;
}
-
- void notify() override {
- if (std::chrono::system_clock::now() >= this->nextExecution) {
- this->isFinished = std::async(std::launch::async, [this] {
- if (this->m_observable != nullptr) {
- this->m_observable->Update();
- }
- });
- if (this->timerMode == continuous) {
+ void
+ notify() override
+ {
+ if (std::chrono::system_clock::now() >= this->nextExecution)
+ {
+ this->isFinished = std::async(std::launch::async,
+ [this]
+ {
+ if (this->m_observable != nullptr)
+ {
+ this->m_observable->Update();
+ }
+ });
+ if (this->timerMode == continuous)
+ {
this->nextExecution = std::chrono::system_clock::now() + interval;
- } else {
+ }
+ else
+ {
// destroy object
this->isFinish = true;
}
}
}
- bool toDestroy() override {
+ bool
+ toDestroy() override
+ {
return isFinish && this->timerMode == singleshot;
}
private:
timer_mode timerMode;
- ITimerObserver *m_observable;
+ ITimerObserver* m_observable;
std::chrono::milliseconds interval;
std::chrono::time_point<std::chrono::system_clock> nextExecution;
std::future<void> isFinished;
bool isFinish;
};
- class SlotObservable : public IObservable {
+ class SlotObservable : public IObservable
+ {
public:
- ~SlotObservable() override {
+ ~SlotObservable() override
+ {
}
SlotObservable(slot fPointer, timer_mode mode, std::chrono::milliseconds ms) :
- timerMode(mode),
- interval(ms),
- functionPointer(std::move(fPointer)),
- isFinish(false) {
+ timerMode(mode), interval(ms), functionPointer(std::move(fPointer)), isFinish(false)
+ {
this->nextExecution = std::chrono::system_clock::now() + ms;
}
- void notify() override {
- if (std::chrono::system_clock::now() >= this->nextExecution) {
+ void
+ notify() override
+ {
+ if (std::chrono::system_clock::now() >= this->nextExecution)
+ {
//this->isFinished = this->promise.get_future();
- auto p = std::async(std::launch::async, [this] {
- try {
- if (this->functionPointer != nullptr) {
- this->functionPointer();
- this->promise.set_value(true);
- }
- this->promise.set_value(false);
- } catch(std::future_error &e) {
- std::cout << "future already satisfied" << e.what() << std::endl;
- }
- });
- if (this->timerMode == continuous) {
+ auto p = std::async(std::launch::async,
+ [this]
+ {
+ try
+ {
+ if (this->functionPointer != nullptr)
+ {
+ this->functionPointer();
+ this->promise.set_value(true);
+ }
+ this->promise.set_value(false);
+ }
+ catch (std::future_error& e)
+ {
+ std::cout << "future already satisfied" << e.what()
+ << std::endl;
+ }
+ });
+ if (this->timerMode == continuous)
+ {
this->nextExecution = std::chrono::system_clock::now() + interval;
- } else {
+ }
+ else
+ {
// destroy object
this->isFinish = true;
}
}
}
- bool toDestroy() override {
+ bool
+ toDestroy() override
+ {
return isFinish && this->timerMode == singleshot;
}
- std::future<bool>& getFut() {
+ std::future<bool>&
+ getFut()
+ {
return this->isFinished;
}
@@ -153,41 +181,52 @@ class Timer : ITimerSubject {
std::promise<bool> promise;
slot functionPointer;
bool isFinish;
-
};
- class FutureObservable : public IObservable {
+ class FutureObservable : public IObservable
+ {
public:
- ~FutureObservable() override {
+ ~FutureObservable() override
+ {
}
- FutureObservable(slot fPointer, std::shared_future<bool> sfuture) : sfut(std::move(sfuture)),
- functionPointer(std::move(fPointer)),
- isFinish(false) {
+ FutureObservable(slot fPointer, std::shared_future<bool> sfuture) :
+ sfut(std::move(sfuture)), functionPointer(std::move(fPointer)), isFinish(false)
+ {
}
- void notify() override {
- if (this->sfut.valid()) {
- if(this->sfut.get()) {
+ void
+ notify() override
+ {
+ if (this->sfut.valid())
+ {
+ if (this->sfut.get())
+ {
this->isFinished = this->isFinishedPromise.get_future();
- auto promise = std::async(std::launch::async, [this] {
- if (this->functionPointer != nullptr) {
- this->functionPointer();
- this->isFinish = true;
- this->isFinishedPromise.set_value(true);
- }
- this->isFinishedPromise.set_value(false);
- });
+ auto promise = std::async(std::launch::async,
+ [this]
+ {
+ if (this->functionPointer != nullptr)
+ {
+ this->functionPointer();
+ this->isFinish = true;
+ this->isFinishedPromise.set_value(true);
+ }
+ this->isFinishedPromise.set_value(false);
+ });
}
}
-
}
- bool toDestroy() override {
+ bool
+ toDestroy() override
+ {
return this->isFinish;
}
- std::future<bool>& getFut() {
+ std::future<bool>&
+ getFut()
+ {
return this->isFinished;
}
@@ -201,104 +240,139 @@ class Timer : ITimerSubject {
public:
-
-
- static std::shared_ptr<Timer> createTimer() {
- if (instance == nullptr) {
+ static std::shared_ptr<Timer>
+ createTimer()
+ {
+ if (instance == nullptr)
+ {
instance = std::make_shared<Timer>();
}
return instance;
}
- static std::future<bool>& singleShot(const slot &functionPointer, std::chrono::milliseconds ms) {
+ static std::future<bool>&
+ singleShot(const slot& functionPointer, std::chrono::milliseconds ms)
+ {
return Timer::createTimer()->AttachSingleshot(functionPointer, ms);
}
- static void periodicShot(slot functionPointer, std::chrono::milliseconds ms) {
+ static void
+ periodicShot(slot functionPointer, std::chrono::milliseconds ms)
+ {
Timer::createTimer()->Attach(std::move(functionPointer), continuous, ms);
}
- static std::future<bool>& futureWatch(const slot &functionPointer, const std::shared_future<bool>& fut) {
+ static std::future<bool>&
+ futureWatch(const slot& functionPointer, const std::shared_future<bool>& fut)
+ {
return Timer::createTimer()->AttachFutureWatcher(functionPointer, fut);
}
- ~Timer() override {
- for (auto it = this->v_observables.begin(); it != this->v_observables.end(); it++) {
+ ~Timer() override
+ {
+ for (auto it = this->v_observables.begin(); it != this->v_observables.end(); it++)
+ {
this->v_observables.erase(it--);
}
this->b_isRunning = false;
}
- static void stop() {
+ static void
+ stop()
+ {
Timer::instance->b_isRunning = false;
}
- static bool isRunning() {
+ static bool
+ isRunning()
+ {
return Timer::instance->b_isRunning;
}
- void start() {
- if (!b_isRunning) {
+ void
+ start()
+ {
+ if (!b_isRunning)
+ {
b_isRunning = true;
this->wait_thread->detach();
}
}
- void Attach(ITimerObserver *observer, timer_mode mode, std::chrono::milliseconds ms) override {
+ void
+ Attach(ITimerObserver* observer, timer_mode mode, std::chrono::milliseconds ms) override
+ {
this->start();
- if (this->time > ms) {
+ if (this->time > ms)
+ {
this->time = ms;
}
this->v_observables.push_back(std::make_unique<TimerObservable>(observer, mode, ms));
}
-
- void Detach(ITimerObserver *observer) override {
- (void) observer;
+ void
+ Detach(ITimerObserver* observer) override
+ {
+ (void)observer;
}
- explicit Timer() {
+ explicit Timer()
+ {
this->wait_thread = std::make_unique<std::thread>(&Timer::dispatcher, this);
this->time = std::chrono::milliseconds(1000);
}
private:
- std::future<bool>& AttachFutureWatcher(const slot &functionPointer, const std::shared_future<bool>& sfut) {
+ std::future<bool>&
+ AttachFutureWatcher(const slot& functionPointer, const std::shared_future<bool>& sfut)
+ {
this->start();
auto o = std::make_unique<FutureObservable>(functionPointer, sfut);
- std::future<bool> &fut{o->getFut()};
+ std::future<bool>& fut{o->getFut()};
this->v_observables.push_back(std::move(o));
return fut;
}
- std::future<bool>& AttachSingleshot(const slot &functionPointer, std::chrono::milliseconds ms) {
+ std::future<bool>&
+ AttachSingleshot(const slot& functionPointer, std::chrono::milliseconds ms)
+ {
this->start();
- if (this->time > ms) {
+ if (this->time > ms)
+ {
this->time = ms;
}
auto o = std::make_unique<SlotObservable>(functionPointer, singleshot, ms);
- auto &fut = o->getFut();
+ auto& fut = o->getFut();
this->v_observables.push_back(std::move(o));
return fut;
}
- void Attach(slot functionPointer, timer_mode mode, std::chrono::milliseconds ms) override {
+ void
+ Attach(slot functionPointer, timer_mode mode, std::chrono::milliseconds ms) override
+ {
this->start();
- if (this->time > ms) {
+ if (this->time > ms)
+ {
this->time = ms;
}
this->v_observables.push_back(std::make_unique<SlotObservable>(functionPointer, mode, ms));
}
- void dispatcher() {
- do {
+ void
+ dispatcher()
+ {
+ do
+ {
std::unique_lock<std::mutex> lck{mtx};
- for (int i{10}; i > 0 && b_isRunning; --i) {
+ for (int i{10}; i > 0 && b_isRunning; --i)
+ {
cv.wait_for(lck, time / 10);
- for (auto it = this->v_observables.begin(); it != this->v_observables.end(); it++) {
+ for (auto it = this->v_observables.begin(); it != this->v_observables.end(); it++)
+ {
(*it)->notify();
- if ((*it)->toDestroy()) {
+ if ((*it)->toDestroy())
+ {
(*it).reset(nullptr);
this->v_observables.erase(it--);
}
@@ -307,10 +381,10 @@ private:
} while (this->b_isRunning);
- for (auto it = this->v_observables.begin(); it != this->v_observables.end(); it++) {
+ for (auto it = this->v_observables.begin(); it != this->v_observables.end(); it++)
+ {
this->v_observables.erase(it--);
}
-
}
std::mutex mtx;
@@ -322,5 +396,4 @@ private:
std::chrono::milliseconds time{};
static std::shared_ptr<Timer> instance;
-
};
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.cpp b/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.cpp
index a201c4731c14a211cfb670608f8839acf19482d1..97de9f294364e20d8dcec8293c410b2e94793a95 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.cpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.cpp
@@ -2,29 +2,31 @@
#include "RAMSegment.h"
// ArmarX
-#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/application/properties/ProxyPropertyDefinition.h>
+#include <RobotAPI/libraries/PriorKnowledge/motions/MotionFinder.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
namespace armarx::armem::server::systemstate::segment
{
- LightweightRamMonitorProviderSegment::LightweightRamMonitorProviderSegment(armem::server::MemoryToIceAdapter& iceMemory) :
+ LightweightRamMonitorProviderSegment::LightweightRamMonitorProviderSegment(
+ armem::server::MemoryToIceAdapter& iceMemory) :
Base(iceMemory, "LightweightSystemMonitor", "MemoryUsage", nullptr, nullptr, 1000)
{
-
}
- void LightweightRamMonitorProviderSegment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ LightweightRamMonitorProviderSegment::defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
defs->optional(pollFrequencyHz, prefix + "pollFrequencyHz", "The poll frequency in Hz");
}
- void LightweightRamMonitorProviderSegment::init()
+ void
+ LightweightRamMonitorProviderSegment::init()
{
Base::init();
@@ -32,11 +34,13 @@ namespace armarx::armem::server::systemstate::segment
usleep(200 * 1000.f); // sleep for 100ms to ensure the monitor is ready
- runningTask = new PeriodicTask<LightweightRamMonitorProviderSegment>(this, &LightweightRamMonitorProviderSegment::loop, (1000.f / pollFrequencyHz));
+ runningTask = new PeriodicTask<LightweightRamMonitorProviderSegment>(
+ this, &LightweightRamMonitorProviderSegment::loop, (1000.f / pollFrequencyHz));
runningTask->start();
}
- void LightweightRamMonitorProviderSegment::loop()
+ void
+ LightweightRamMonitorProviderSegment::loop()
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
@@ -46,7 +50,8 @@ namespace armarx::armem::server::systemstate::segment
double usage = memoryMonitoring->getCurrentMemUsageInPercent();
auto data = std::make_shared<aron::data::Dict>();
- data->addElement("total", std::make_shared<aron::data::Long>(total)); // TODO in seperate segment?
+ data->addElement("total",
+ std::make_shared<aron::data::Long>(total)); // TODO in seperate segment?
data->addElement("load", std::make_shared<aron::data::Double>(usage));
ARMARX_DEBUG << "RAM Usage is: " << usage << "% (of " << total << "KB max)";
@@ -55,8 +60,8 @@ namespace armarx::armem::server::systemstate::segment
update.entityID = providerId.withEntityName("CurrentMemoryLoad");
update.confidence = 1.0;
update.referencedTime = armem::Time::Now();
- update.instancesData = { data };
+ update.instancesData = {data};
segmentPtr->update(update);
}
-}
+} // namespace armarx::armem::server::systemstate::segment
diff --git a/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.h b/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.h
index 28629e356937ee61415c2c926b7b9433c96af35c..78f78a06005b81370fa616b0d2b4a2c93dc23275 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.h
+++ b/source/RobotAPI/libraries/armem_system_state/server/RAMSegment.h
@@ -1,17 +1,17 @@
#pragma once
// STD/STL
-#include <iostream>
+#include <atomic>
#include <csignal>
+#include <iostream>
#include <memory>
-#include <atomic>
#include <thread>
// System Monitor
-#include "LightweightSystemMonitor/linux_memoryload.hpp"
#include "LightweightSystemMonitor/linux_cpuload.hpp"
-#include "LightweightSystemMonitor/linux_process_load.hpp"
+#include "LightweightSystemMonitor/linux_memoryload.hpp"
#include "LightweightSystemMonitor/linux_networkload.hpp"
+#include "LightweightSystemMonitor/linux_process_load.hpp"
#include "LightweightSystemMonitor/linux_systemutil.hpp"
#include "LightweightSystemMonitor/util/record_value.hpp"
#include "LightweightSystemMonitor/util/timer.hpp"
@@ -24,14 +24,16 @@
namespace armarx::armem::server::systemstate::segment
{
- class LightweightRamMonitorProviderSegment : public armem::server::segment::SpecializedProviderSegment
+ class LightweightRamMonitorProviderSegment :
+ public armem::server::segment::SpecializedProviderSegment
{
using Base = armem::server::segment::SpecializedProviderSegment;
public:
LightweightRamMonitorProviderSegment(armem::server::MemoryToIceAdapter& iceMemory);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
void init() override;
private:
@@ -43,4 +45,4 @@ namespace armarx::armem::server::systemstate::segment
armarx::PeriodicTask<LightweightRamMonitorProviderSegment>::pointer_type runningTask;
};
-}
+} // namespace armarx::armem::server::systemstate::segment
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
index f88cc75454fd6f83545c3207c1e58642bedcc225..57968b54c2d16f764f216453ed57202e85729b9b 100644
--- a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
@@ -5,17 +5,19 @@
namespace armarx
{
OccupancyGridHelper::OccupancyGridHelper(const OccupancyGrid& occupancyGrid,
- const Params& params) :
+ const Params& params) :
occupancyGrid(occupancyGrid), params(params)
{
}
- OccupancyGridHelper::BinaryArray OccupancyGridHelper::knownCells() const
+ OccupancyGridHelper::BinaryArray
+ OccupancyGridHelper::knownCells() const
{
return (occupancyGrid.grid > 0.F).cast<bool>();
}
- OccupancyGridHelper::BinaryArray OccupancyGridHelper::freespace() const
+ OccupancyGridHelper::BinaryArray
+ OccupancyGridHelper::freespace() const
{
// matrix1 = matrix1 .unaryExpr(std::ptr_fun(ReplaceNanWithValue<1>));
// return (occupancyGrid.grid ).cast<bool>();
@@ -28,7 +30,8 @@ namespace armarx
return occupancyGrid.grid.unaryViewExpr(isFree).cast<bool>();
}
- OccupancyGridHelper::BinaryArray OccupancyGridHelper::obstacles() const
+ OccupancyGridHelper::BinaryArray
+ OccupancyGridHelper::obstacles() const
{
const auto isOccupied = [&](OccupancyGrid::CellType p) -> float
{ return static_cast<float>(p > params.occupiedThreshold); };
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
index 89ee1de1132faa3298765bd1c28fda208ca164a7..7dcbf6fb9b5da0fa6243335463879336d9173a5d 100644
--- a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
@@ -18,7 +18,7 @@ namespace armarx
float freespaceThreshold = 0.45F;
float occupiedThreshold = 0.55F;
};
- }
+ } // namespace detail
class OccupancyGridHelper
{
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
index eaf9b2fdfa2021db7f8e1215baedc6aba907dec3..1312fea051bda7ba1c64c404bea956815a09bbaa 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
@@ -23,10 +23,10 @@
#include <mutex>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_vision/types.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
namespace armarx::armem::vision::occupancy_grid::client
{
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/CodeWriter.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/CodeWriter.h
index 0e17ccc1ee4938bd81ae462e8a2b069669c25826..22b5fdaf2a5549ac3f8fb677188c7e42e5b55367 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/CodeWriter.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/CodeWriter.h
@@ -24,22 +24,21 @@
#pragma once
// STD/STL
-#include <memory>
#include <map>
-#include <vector>
-#include <typeinfo>
+#include <memory>
#include <typeindex>
+#include <typeinfo>
+#include <vector>
// ArmarX
-#include <ArmarXCore/libraries/cppgen/CppEnum.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <ArmarXCore/libraries/cppgen/CppEnum.h>
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
-
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/helper/WriterInfo.h>
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/helper/ReaderInfo.h>
-#include <RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateTypeInfo.h>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/helper/WriterInfo.h>
#include <RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateIntEnumInfo.h>
+#include <RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateTypeInfo.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
namespace armarx::aron::codegenerator
{
@@ -47,22 +46,26 @@ namespace armarx::aron::codegenerator
{
public:
CodeWriter() = delete;
- CodeWriter(const std::string& producerName, const std::vector<std::string>& additionalIncludesFromXMLFile):
- producerName(producerName),
- additionalIncludes(additionalIncludesFromXMLFile)
- {}
+
+ CodeWriter(const std::string& producerName,
+ const std::vector<std::string>& additionalIncludesFromXMLFile) :
+ producerName(producerName), additionalIncludes(additionalIncludesFromXMLFile)
+ {
+ }
virtual ~CodeWriter() = default;
virtual void generateTypeObjects(const std::vector<typereader::GenerateObjectInfo>&) = 0;
virtual void generateTypeIntEnums(const std::vector<typereader::GenerateIntEnumInfo>&) = 0;
- std::vector<MetaClassPtr> getTypeClasses() const
+ std::vector<MetaClassPtr>
+ getTypeClasses() const
{
return typeClasses;
}
- std::vector<MetaEnumPtr> getTypeEnums() const // TODO: Create Meta Enum class to abstract away cpp details
+ std::vector<MetaEnumPtr>
+ getTypeEnums() const // TODO: Create Meta Enum class to abstract away cpp details
{
return typeEnums;
}
@@ -90,4 +93,4 @@ namespace armarx::aron::codegenerator
std::vector<std::string> additionalIncludes;
};
-}
+} // namespace armarx::aron::codegenerator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp
index c65402434f631f9d26f34c15b6f12d818e1b4a43..668537ed21e8ff738059e33a3d20d25ecf5da294 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp
@@ -35,7 +35,8 @@ namespace armarx::aron::codegenerator::cpp
// Function to convert camel case
// string to snake case string
- std::string camelToSnake(const std::string& str)
+ std::string
+ camelToSnake(const std::string& str)
{
// Empty String
@@ -44,25 +45,28 @@ namespace armarx::aron::codegenerator::cpp
// Append first character(in lower case)
// to result string
char c = std::tolower(str[0]);
- result+=(char(c));
+ result += (char(c));
// Traverse the string from
// ist index to last index
- for (unsigned int i = 1; i < str.length(); i++) {
+ for (unsigned int i = 1; i < str.length(); i++)
+ {
char ch = str[i];
// Check if the character is upper case
// then append '_' and such character
// (in lower case) to result string
- if (std::isupper(ch)) {
+ if (std::isupper(ch))
+ {
result = result + '_';
- result+=char(std::tolower(ch));
+ result += char(std::tolower(ch));
}
// If the character is lower case then
// add such character into result string
- else {
+ else
+ {
result = result + ch;
}
}
@@ -71,14 +75,16 @@ namespace armarx::aron::codegenerator::cpp
return result;
}
- Writer::Writer(const std::string& producerName, const std::vector<std::string>& additionalIncludesFromXMLFile) :
+ Writer::Writer(const std::string& producerName,
+ const std::vector<std::string>& additionalIncludesFromXMLFile) :
CodeWriter(producerName, additionalIncludesFromXMLFile)
{
addSpecificWriterMethods();
addSpecificReaderMethods();
}
- void Writer::addSpecificWriterMethods()
+ void
+ Writer::addSpecificWriterMethods()
{
// DictData stuff
{
@@ -174,14 +180,16 @@ namespace armarx::aron::codegenerator::cpp
ToAronTypeDTO.writerClassType = "armarx::aron::type::writer::VariantWriter";
ToAronTypeDTO.include =
"<RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h>";
- ToAronTypeDTO.enforceConversion = "armarx::aron::type::IntEnum::DynamicCastAndCheck";
+ ToAronTypeDTO.enforceConversion =
+ "armarx::aron::type::IntEnum::DynamicCastAndCheck";
ToAronTypeDTO.enforceMemberAccess = "->toIntEnumDTO()";
initialIntEnumTypeWriters.push_back(ToAronTypeDTO);
}
}
}
- void Writer::addSpecificReaderMethods()
+ void
+ Writer::addSpecificReaderMethods()
{
// Dict stuff
{
@@ -204,7 +212,8 @@ namespace armarx::aron::codegenerator::cpp
fromAron.methodName = "fromAron";
fromAron.argumentType = "armarx::aron::data::DictPtr";
fromAron.readerClassType = "armarx::aron::data::reader::VariantReader";
- fromAron.include = "<RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>";
+ fromAron.include =
+ "<RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>";
fromAron.override = true;
dictDataReaders.push_back(fromAron);
}
@@ -213,7 +222,8 @@ namespace armarx::aron::codegenerator::cpp
fromAronDTO.methodName = "fromAron";
fromAronDTO.argumentType = "armarx::aron::data::dto::DictPtr";
fromAronDTO.readerClassType = "armarx::aron::data::reader::VariantReader";
- fromAronDTO.include = "<RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>";
+ fromAronDTO.include =
+ "<RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>";
fromAronDTO.enforceConversion = "std::make_shared<armarx::aron::data::Dict>";
fromAronDTO.override = true;
dictDataReaders.push_back(fromAronDTO);
@@ -241,7 +251,8 @@ namespace armarx::aron::codegenerator::cpp
fromAron.methodName = "fromAron";
fromAron.argumentType = "armarx::aron::data::IntPtr";
fromAron.readerClassType = "armarx::aron::data::reader::VariantReader";
- fromAron.include = "<RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>";
+ fromAron.include =
+ "<RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>";
fromAron.override = true;
intEnumDataReaders.push_back(fromAron);
}
@@ -259,8 +270,8 @@ namespace armarx::aron::codegenerator::cpp
}
}
-
- void Writer::generateTypeObjects(const std::vector<typereader::GenerateObjectInfo>& generateObjects)
+ void
+ Writer::generateTypeObjects(const std::vector<typereader::GenerateObjectInfo>& generateObjects)
{
for (const auto& publicGenerateObjectType : generateObjects)
{
@@ -298,7 +309,8 @@ namespace armarx::aron::codegenerator::cpp
}
else
{
- c->addInherit("public armarx::aron::cpp::AronGeneratedObject<" + generator.getFullClassCppTypename() + ">");
+ c->addInherit("public armarx::aron::cpp::AronGeneratedObject<" +
+ generator.getFullClassCppTypename() + ">");
}
// Writermethods
@@ -372,7 +384,9 @@ namespace armarx::aron::codegenerator::cpp
}
}
- void Writer::generateTypeIntEnums(const std::vector<typereader::GenerateIntEnumInfo>& generateIntEnums)
+ void
+ Writer::generateTypeIntEnums(
+ const std::vector<typereader::GenerateIntEnumInfo>& generateIntEnums)
{
for (const auto& publicGenerateIntEnumType : generateIntEnums)
{
@@ -391,7 +405,8 @@ namespace armarx::aron::codegenerator::cpp
setupMemberFields(c, publicGenerateIntEnumType.doc_values, generator);
- c->addInherit("public armarx::aron::cpp::AronGeneratedIntEnum<" + generator.getFullClassCppTypename() + ">");
+ c->addInherit("public armarx::aron::cpp::AronGeneratedIntEnum<" +
+ generator.getFullClassCppTypename() + ">");
// ctor
c->addCtor(generator.toEnumCtor(c->getName()));
@@ -484,13 +499,21 @@ namespace armarx::aron::codegenerator::cpp
}
}
- CppEnumPtr Writer::setupEnumPtr(const typereader::GenerateInfo& info, const generator::IntEnumClass& gen) const
+ CppEnumPtr
+ Writer::setupEnumPtr(const typereader::GenerateInfo& info,
+ const generator::IntEnumClass& gen) const
{
const auto& type = gen.getType();
if (type.getMaybe() != type::Maybe::NONE)
{
// Should not happen since we check the maybe flag on creation of the generator. However, better to double check
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Somehow the maybe flag of a top level object declaration is set. This is not valid!", std::to_string((int) type.getMaybe()) + " aka " + type::defaultconversion::string::Maybe2String.at(type.getMaybe()), type.getPath());
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__,
+ "Somehow the maybe flag of a top level object declaration is set. This is not "
+ "valid!",
+ std::to_string((int)type.getMaybe()) + " aka " +
+ type::defaultconversion::string::Maybe2String.at(type.getMaybe()),
+ type.getPath());
}
const std::string classCppTypename = gen.getClassCppTypename();
@@ -499,7 +522,8 @@ namespace armarx::aron::codegenerator::cpp
std::string rawObjectName = info.getNameWithoutNamespace();
namespaces.push_back(simox::alg::to_lower(camelToSnake(rawObjectName)) + "_details");
- std::string enumdoc = "The enum definition of the enum of the auogenerated class '" + gen.getFullClassCppTypename() + "'.";
+ std::string enumdoc = "The enum definition of the enum of the auogenerated class '" +
+ gen.getFullClassCppTypename() + "'.";
CppEnumPtr e = std::make_shared<CppEnum>(namespaces, "Enum");
auto enumFields = gen.toEnumFields();
@@ -513,13 +537,20 @@ namespace armarx::aron::codegenerator::cpp
return e;
}
- CppClassPtr Writer::setupBasicCppClass(const typereader::GenerateInfo& info, const Generator& gen) const
+ CppClassPtr
+ Writer::setupBasicCppClass(const typereader::GenerateInfo& info, const Generator& gen) const
{
const auto& type = gen.getType();
if (type.getMaybe() != type::Maybe::NONE)
{
// Should not happen since we check the maybe flag on creation of the generator. However, better to double check
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Somehow the maybe flag of a top level object declaration is set. This is not valid!", std::to_string((int) type.getMaybe()) + " aka " + type::defaultconversion::string::Maybe2String.at(type.getMaybe()), type.getPath());
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__,
+ "Somehow the maybe flag of a top level object declaration is set. This is not "
+ "valid!",
+ std::to_string((int)type.getMaybe()) + " aka " +
+ type::defaultconversion::string::Maybe2String.at(type.getMaybe()),
+ type.getPath());
}
const std::string classCppTypename = gen.getClassCppTypename();
@@ -555,7 +586,8 @@ namespace armarx::aron::codegenerator::cpp
{
if (!s.empty())
{
- c->addInclude(simox::alg::replace_last(s, ".aron.generated.codesuffix", ".aron.generated.h"));
+ c->addInclude(
+ simox::alg::replace_last(s, ".aron.generated.codesuffix", ".aron.generated.h"));
}
}
@@ -600,7 +632,10 @@ namespace armarx::aron::codegenerator::cpp
return c;
}
- void Writer::setupMemberFields(CppClassPtr& c, const std::map<std::string, std::string>& doc_members, const generator::ObjectClass& o) const
+ void
+ Writer::setupMemberFields(CppClassPtr& c,
+ const std::map<std::string, std::string>& doc_members,
+ const generator::ObjectClass& o) const
{
auto publicFields = o.getPublicVariableDeclarations(c->getName());
for (const auto& f : publicFields)
@@ -613,7 +648,10 @@ namespace armarx::aron::codegenerator::cpp
}
}
- void Writer::setupMemberFields(CppClassPtr& c, const std::map<std::string, std::string>& doc_members, const generator::IntEnumClass& o) const
+ void
+ Writer::setupMemberFields(CppClassPtr& c,
+ const std::map<std::string, std::string>& doc_members,
+ const generator::IntEnumClass& o) const
{
auto publicFields = o.getPublicVariableDeclarations(camelToSnake(c->getName()));
for (const auto& f : publicFields)
@@ -625,9 +663,4 @@ namespace armarx::aron::codegenerator::cpp
c->addPublicField(f);
}
}
-}
-
-
-
-
-
+} // namespace armarx::aron::codegenerator::cpp
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.h
index 3aa93f1775c7fe460935552c9eb078f8ae45ad0c..557fe7914e135511965becea148c3e47f409ff7f 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.h
@@ -24,26 +24,25 @@
#pragma once
// STD/STL
+#include <map>
#include <memory>
#include <set>
-#include <map>
#include <vector>
// Parent class
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/CodeWriter.h>
// ArmarX
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h>
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/IntEnumClass.h>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/ObjectClass.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
namespace armarx::aron::codegenerator::cpp
{
- class Writer :
- virtual public codegenerator::CodeWriter
+ class Writer : virtual public codegenerator::CodeWriter
{
public:
Writer() = delete;
@@ -51,21 +50,28 @@ namespace armarx::aron::codegenerator::cpp
virtual ~Writer() = default;
- virtual void generateTypeObjects(const std::vector<typereader::GenerateObjectInfo>&) override;
- virtual void generateTypeIntEnums(const std::vector<typereader::GenerateIntEnumInfo>&) override;
+ virtual void
+ generateTypeObjects(const std::vector<typereader::GenerateObjectInfo>&) override;
+ virtual void
+ generateTypeIntEnums(const std::vector<typereader::GenerateIntEnumInfo>&) override;
protected:
virtual void addSpecificWriterMethods() override;
virtual void addSpecificReaderMethods() override;
- CppClassPtr setupBasicCppClass(const typereader::GenerateInfo& info, const Generator& gen) const;
- CppEnumPtr setupEnumPtr(const typereader::GenerateInfo& info, const generator::IntEnumClass& gen) const;
+ CppClassPtr setupBasicCppClass(const typereader::GenerateInfo& info,
+ const Generator& gen) const;
+ CppEnumPtr setupEnumPtr(const typereader::GenerateInfo& info,
+ const generator::IntEnumClass& gen) const;
- void setupMemberFields(CppClassPtr&, const std::map<std::string, std::string>& doc_members, const generator::ObjectClass&) const;
- void setupMemberFields(CppClassPtr&, const std::map<std::string, std::string>& doc_members, const generator::IntEnumClass&) const;
+ void setupMemberFields(CppClassPtr&,
+ const std::map<std::string, std::string>& doc_members,
+ const generator::ObjectClass&) const;
+ void setupMemberFields(CppClassPtr&,
+ const std::map<std::string, std::string>& doc_members,
+ const generator::IntEnumClass&) const;
private:
static const constexpr char* OWN_TYPE_NAME = "OWN_TYPE_NAME";
-
};
-}
+} // namespace armarx::aron::codegenerator::cpp
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/All.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/All.h
index 36169d4a2c00a6f21e7ac13801ac6919814d6c3f..1271d07ef5e8342f69094f3e4c0a5f9237612fde 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/All.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/All.h
@@ -1,12 +1,13 @@
#pragma once
-#include "toplevel/All.h"
+#include "any/All.h"
#include "container/All.h"
-#include "ndarray/All.h"
#include "enum/All.h"
+#include "ndarray/All.h"
#include "primitive/All.h"
-#include "any/All.h"
+#include "toplevel/All.h"
-namespace {
+namespace
+{
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Factory.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Factory.cpp
index c83144cb7090da617e3abcd1c04f81a302eebfae..7b04ec0d005485289497d95dbc248388478fa057 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Factory.cpp
@@ -33,32 +33,55 @@
namespace armarx::aron::codegenerator::cpp
{
// Access method
- std::unique_ptr<Generator> GeneratorFactory::create(const type::Variant& n, const Path& path) const
+ std::unique_ptr<Generator>
+ GeneratorFactory::create(const type::Variant& n, const Path& path) const
{
auto desc = n.getDescriptor();
- switch(desc)
+ switch (desc)
{
- case type::Descriptor::LIST: return std::make_unique<generator::List>(dynamic_cast<const type::List&>(n));
- case type::Descriptor::DICT: return std::make_unique<generator::Dict>(dynamic_cast<const type::Dict&>(n));
- case type::Descriptor::OBJECT: return std::make_unique<generator::Object>(dynamic_cast<const type::Object&>(n));
- case type::Descriptor::TUPLE: return std::make_unique<generator::Tuple>(dynamic_cast<const type::Tuple&>(n));
- case type::Descriptor::PAIR: return std::make_unique<generator::Pair>(dynamic_cast<const type::Pair&>(n));
- case type::Descriptor::NDARRAY: return std::make_unique<generator::NDArray>(dynamic_cast<const type::NDArray&>(n));
- case type::Descriptor::MATRIX: return std::make_unique<generator::Matrix>(dynamic_cast<const type::Matrix&>(n));
- case type::Descriptor::QUATERNION: return std::make_unique<generator::Quaternion>(dynamic_cast<const type::Quaternion&>(n));
- case type::Descriptor::IMAGE: return std::make_unique<generator::Image>(dynamic_cast<const type::Image&>(n));
- case type::Descriptor::POINTCLOUD: return std::make_unique<generator::PointCloud>(dynamic_cast<const type::PointCloud&>(n));
- case type::Descriptor::INT_ENUM: return std::make_unique<generator::IntEnum>(dynamic_cast<const type::IntEnum&>(n));
- case type::Descriptor::INT: return std::make_unique<generator::Int>(dynamic_cast<const type::Int&>(n));
- case type::Descriptor::LONG: return std::make_unique<generator::Long>(dynamic_cast<const type::Long&>(n));
- case type::Descriptor::FLOAT: return std::make_unique<generator::Float>(dynamic_cast<const type::Float&>(n));
- case type::Descriptor::DOUBLE: return std::make_unique<generator::Double>(dynamic_cast<const type::Double&>(n));
- case type::Descriptor::STRING: return std::make_unique<generator::String>(dynamic_cast<const type::String&>(n));
- case type::Descriptor::BOOL: return std::make_unique<generator::Bool>(dynamic_cast<const type::Bool&>(n));
- case type::Descriptor::ANY_OBJECT: return std::make_unique<generator::AnyObject>(dynamic_cast<const type::AnyObject&>(n));
- case type::Descriptor::UNKNOWN: break;
+ case type::Descriptor::LIST:
+ return std::make_unique<generator::List>(dynamic_cast<const type::List&>(n));
+ case type::Descriptor::DICT:
+ return std::make_unique<generator::Dict>(dynamic_cast<const type::Dict&>(n));
+ case type::Descriptor::OBJECT:
+ return std::make_unique<generator::Object>(dynamic_cast<const type::Object&>(n));
+ case type::Descriptor::TUPLE:
+ return std::make_unique<generator::Tuple>(dynamic_cast<const type::Tuple&>(n));
+ case type::Descriptor::PAIR:
+ return std::make_unique<generator::Pair>(dynamic_cast<const type::Pair&>(n));
+ case type::Descriptor::NDARRAY:
+ return std::make_unique<generator::NDArray>(dynamic_cast<const type::NDArray&>(n));
+ case type::Descriptor::MATRIX:
+ return std::make_unique<generator::Matrix>(dynamic_cast<const type::Matrix&>(n));
+ case type::Descriptor::QUATERNION:
+ return std::make_unique<generator::Quaternion>(
+ dynamic_cast<const type::Quaternion&>(n));
+ case type::Descriptor::IMAGE:
+ return std::make_unique<generator::Image>(dynamic_cast<const type::Image&>(n));
+ case type::Descriptor::POINTCLOUD:
+ return std::make_unique<generator::PointCloud>(
+ dynamic_cast<const type::PointCloud&>(n));
+ case type::Descriptor::INT_ENUM:
+ return std::make_unique<generator::IntEnum>(dynamic_cast<const type::IntEnum&>(n));
+ case type::Descriptor::INT:
+ return std::make_unique<generator::Int>(dynamic_cast<const type::Int&>(n));
+ case type::Descriptor::LONG:
+ return std::make_unique<generator::Long>(dynamic_cast<const type::Long&>(n));
+ case type::Descriptor::FLOAT:
+ return std::make_unique<generator::Float>(dynamic_cast<const type::Float&>(n));
+ case type::Descriptor::DOUBLE:
+ return std::make_unique<generator::Double>(dynamic_cast<const type::Double&>(n));
+ case type::Descriptor::STRING:
+ return std::make_unique<generator::String>(dynamic_cast<const type::String&>(n));
+ case type::Descriptor::BOOL:
+ return std::make_unique<generator::Bool>(dynamic_cast<const type::Bool&>(n));
+ case type::Descriptor::ANY_OBJECT:
+ return std::make_unique<generator::AnyObject>(
+ dynamic_cast<const type::AnyObject&>(n));
+ case type::Descriptor::UNKNOWN:
+ break;
}
throw error::AronEOFException(__PRETTY_FUNCTION__);
}
-}
+} // namespace armarx::aron::codegenerator::cpp
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Factory.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Factory.h
index cc75cbe6b6d754bf36cbbe72bd3f1342a7c840a6..b13c7d87924582e5653728722c625688d3b55eba 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Factory.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Factory.h
@@ -28,9 +28,9 @@
#include <unordered_map>
// ArmarX
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/All.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
namespace armarx::aron::codegenerator::cpp
{
@@ -41,6 +41,5 @@ namespace armarx::aron::codegenerator::cpp
std::unique_ptr<Generator> create(const type::Variant&, const Path&) const;
virtual ~GeneratorFactory() = default;
-
};
-}
+} // namespace armarx::aron::codegenerator::cpp
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
index 076d5aa61a6d0c79e4e3a56453675308e09f7bde..94c739fd74895be6d4c5ee4e12490818bd8b115a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.cpp
@@ -737,22 +737,27 @@ namespace armarx::aron::codegenerator::cpp
const std::string& otherInstanceAccessor) const
{
const auto& type = getType();
- if (type.getMaybe() == type::Maybe::RAW_PTR || type.getMaybe() == type::Maybe::SHARED_PTR || type.getMaybe() == type::Maybe::UNIQUE_PTR)
+ if (type.getMaybe() == type::Maybe::RAW_PTR || type.getMaybe() == type::Maybe::SHARED_PTR ||
+ type.getMaybe() == type::Maybe::UNIQUE_PTR)
{
CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("if (not (static_cast<bool>(" + accessor + ") == static_cast<bool>(" + otherInstanceAccessor + "))) // check if both contain data");
+ b->addLine("if (not (static_cast<bool>(" + accessor + ") == static_cast<bool>(" +
+ otherInstanceAccessor + "))) // check if both contain data");
b->addLineAsBlock("return false;");
- b->addLine("if (static_cast<bool>(" + accessor + ") && static_cast<bool>(" + otherInstanceAccessor + "))");
+ b->addLine("if (static_cast<bool>(" + accessor + ") && static_cast<bool>(" +
+ otherInstanceAccessor + "))");
b->addBlock(block_if_data);
return b;
}
-
+
if (type.getMaybe() == type::Maybe::OPTIONAL)
{
CppBlockPtr b = std::make_shared<CppBlock>();
- b->addLine("if (not ( " + accessor + ".has_value() == " + otherInstanceAccessor + ".has_value())) // check if both contain data");
+ b->addLine("if (not ( " + accessor + ".has_value() == " + otherInstanceAccessor +
+ ".has_value())) // check if both contain data");
b->addLineAsBlock("return false;");
- b->addLine("if ( " + accessor + ".has_value() && " + otherInstanceAccessor + ".has_value())");
+ b->addLine("if ( " + accessor + ".has_value() && " + otherInstanceAccessor +
+ ".has_value())");
b->addBlock(block_if_data);
return b;
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.h
index 18bec1e225a6fbae97c71c10a7bd82a21ba5604b..5394b0f9a23184dac9cad4a7ae819d8273c4de61 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.h
@@ -23,37 +23,34 @@
#pragma once
-#include <RobotAPI/interface/aron.h>
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/helper/ReaderInfo.h>
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/helper/WriterInfo.h>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
#include <ArmarXCore/libraries/cppgen/CppBlock.h>
-#include <ArmarXCore/libraries/cppgen/CppField.h>
+#include <ArmarXCore/libraries/cppgen/CppClass.h>
#include <ArmarXCore/libraries/cppgen/CppCtor.h>
+#include <ArmarXCore/libraries/cppgen/CppField.h>
#include <ArmarXCore/libraries/cppgen/CppMethod.h>
-#include <ArmarXCore/libraries/cppgen/CppClass.h>
-
-#include <memory>
-#include <map>
-#include <string>
-#include <vector>
+#include <RobotAPI/interface/aron.h>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/helper/ReaderInfo.h>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/helper/WriterInfo.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
namespace armarx::aron::codegenerator::cpp
{
namespace conversion
{
- const std::map<type::Maybe, std::string> Maybe2CppString =
- {
+ const std::map<type::Maybe, std::string> Maybe2CppString = {
{type::Maybe::NONE, "::armarx::aron::type::Maybe::NONE"},
{type::Maybe::OPTIONAL, "::armarx::aron::type::Maybe::OPTIONAL"},
{type::Maybe::RAW_PTR, "::armarx::aron::type::Maybe::RAW_PTR"},
{type::Maybe::SHARED_PTR, "::armarx::aron::type::Maybe::SHARED_PTR"},
- {type::Maybe::UNIQUE_PTR, "::armarx::aron::type::Maybe::UNIQUE_PTR"}
- };
+ {type::Maybe::UNIQUE_PTR, "::armarx::aron::type::Maybe::UNIQUE_PTR"}};
}
class GeneratorFactory;
@@ -70,7 +67,11 @@ namespace armarx::aron::codegenerator::cpp
public:
// constructors
Generator() = delete;
- Generator(const std::string& instantiatedCppTypename /* used for templates, e.g. vector<string> */, const std::string& classCppTypename /* the raw typename, e.g. vector<T> */, const std::string& aronDataTypename, const std::string& aronTypeTypename);
+ Generator(const std::string&
+ instantiatedCppTypename /* used for templates, e.g. vector<string> */,
+ const std::string& classCppTypename /* the raw typename, e.g. vector<T> */,
+ const std::string& aronDataTypename,
+ const std::string& aronTypeTypename);
virtual ~Generator() = default;
// public member methods
@@ -91,11 +92,13 @@ namespace armarx::aron::codegenerator::cpp
virtual std::vector<CppFieldPtr> getPublicVariableDeclarations(const std::string&) const;
CppCtorPtr toCtor(const std::string&) const;
- virtual std::pair<std::vector<std::pair<std::string, std::string>>, bool> getCtorInitializers(const std::string&) const;
+ virtual std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCtorInitializers(const std::string&) const;
virtual CppBlockPtr getCtorBlock(const std::string&) const;
CppCtorPtr toCopyCtor(const std::string&) const;
- virtual std::pair<std::vector<std::pair<std::string, std::string>>, bool> getCopyCtorInitializers(const std::string&) const;
+ virtual std::pair<std::vector<std::pair<std::string, std::string>>, bool>
+ getCopyCtorInitializers(const std::string&) const;
virtual CppBlockPtr getCopyCtorBlock(const std::string&) const;
CppMethodPtr toDtor(const std::string&) const;
@@ -108,16 +111,23 @@ namespace armarx::aron::codegenerator::cpp
virtual CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const;
CppMethodPtr toWriteTypeMethod() const;
- virtual CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const = 0;
+ virtual CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const = 0;
CppMethodPtr toWriteMethod() const;
- virtual CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const = 0;
+ virtual CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const = 0;
CppMethodPtr toReadMethod() const;
- virtual CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const = 0;
+ virtual CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const = 0;
CppMethodPtr toEqualsMethod() const;
- virtual CppBlockPtr getEqualsBlock(const std::string& cppAccessorThis, const std::string& cppAccessorOther) const;
+ virtual CppBlockPtr getEqualsBlock(const std::string& cppAccessorThis,
+ const std::string& cppAccessorOther) const;
virtual const type::Variant& getType() const = 0;
@@ -135,13 +145,16 @@ namespace armarx::aron::codegenerator::cpp
std::string resolveMaybeAccessor(const std::string&) const;
std::string resolveMaybeGenerator(const std::string& args = "") const;
- std::string resolveMaybeGeneratorWithSetter(const std::string&, const std::string& args = "") const;
+ std::string resolveMaybeGeneratorWithSetter(const std::string&,
+ const std::string& args = "") const;
CppBlockPtr resolveMaybeResetHardBlock(const CppBlockPtr&, const std::string&) const;
CppBlockPtr resolveMaybeResetSoftBlock(const CppBlockPtr&, const std::string&) const;
CppBlockPtr resolveMaybeWriteBlock(const CppBlockPtr&, const std::string&) const;
- CppBlockPtr resolveMaybeReadBlock(const CppBlockPtr&, const std::string&, const std::string&) const;
- CppBlockPtr resolveMaybeEqualsBlock(const CppBlockPtr&, const std::string&, const std::string&) const;
+ CppBlockPtr
+ resolveMaybeReadBlock(const CppBlockPtr&, const std::string&, const std::string&) const;
+ CppBlockPtr
+ resolveMaybeEqualsBlock(const CppBlockPtr&, const std::string&, const std::string&) const;
protected:
static const std::string ARON_VARIABLE_PREFIX;
@@ -162,4 +175,4 @@ namespace armarx::aron::codegenerator::cpp
std::string aronDataTypename;
std::string aronTypeTypename;
};
-}
+} // namespace armarx::aron::codegenerator::cpp
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/All.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/All.h
index 9a11aa6b9192a4f2f151910f4b809c80540c1103..17946bd12ce6ca53bde6142198e378821e3dbab0 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/All.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/All.h
@@ -2,6 +2,7 @@
#include "AnyObject.h"
-namespace {
+namespace
+{
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp
index e3c2913dd52b5924ea7d02f9c5a402626788f7e2..ee0912a52c8f8512c60cd19356b1b3164413195e 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.cpp
@@ -25,7 +25,6 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
/* constructors */
@@ -40,48 +39,74 @@ namespace armarx::aron::codegenerator::cpp::generator
// if the type is not known, we only accept shared ptr for unspecified aron dicts
if (type.getMaybe() != type::Maybe::SHARED_PTR)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "At the moment the unknown any object type must be a shared_ptr! ", std::to_string((int) type.getMaybe()) + " aka " + type::defaultconversion::string::Maybe2String.at(type.getMaybe()), type.getPath());
+ throw error::ValueNotValidException(
+ __PRETTY_FUNCTION__,
+ "At the moment the unknown any object type must be a shared_ptr! ",
+ std::to_string((int)type.getMaybe()) + " aka " +
+ type::defaultconversion::string::Maybe2String.at(type.getMaybe()),
+ type.getPath());
}
-
}
/* virtual implementations */
- std::vector<std::string> AnyObject::getRequiredIncludes() const
+ std::vector<std::string>
+ AnyObject::getRequiredIncludes() const
{
- std::vector<std::string> required_includes = {"<RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h>"};
+ std::vector<std::string> required_includes = {
+ "<RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h>"};
return required_includes;
}
- CppBlockPtr AnyObject::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ AnyObject::getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
auto block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = this->EscapeAccessor(cppAccessor);
variantAccessor = Generator::ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- block_if_data->addLine(variantAccessor + " = ::armarx::aron::data::readAndWrite<::armarx::aron::data::FromVariantConverter<WriterT>>(" + cppAccessor + "); // of " + cppAccessor);
+ block_if_data->addLine(variantAccessor +
+ " = "
+ "::armarx::aron::data::readAndWrite<::armarx::aron::data::"
+ "FromVariantConverter<WriterT>>(" +
+ cppAccessor + "); // of " + cppAccessor);
return block_if_data;
}
- CppBlockPtr AnyObject::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ AnyObject::getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const
{
auto block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = this->EscapeAccessor(cppAccessor);
- block_if_data->addLine(cppAccessor + " = ::armarx::aron::data::Dict::DynamicCastAndCheck(::armarx::aron::data::readAndWrite<::armarx::aron::data::ToVariantConverter<ReaderT>>(" + variantAccessor + ")); // of " + cppAccessor);
+ block_if_data->addLine(
+ cppAccessor +
+ " = "
+ "::armarx::aron::data::Dict::DynamicCastAndCheck(::armarx::aron::data::readAndWrite<::"
+ "armarx::aron::data::ToVariantConverter<ReaderT>>(" +
+ variantAccessor + ")); // of " + cppAccessor);
return block_if_data;
}
- CppBlockPtr AnyObject::getWriteTypeBlock(const std::string& typeAccessor, const std::string& accessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ AnyObject::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& accessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(accessor);
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeAnyObject(" + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "::armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + typeAccessor);
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeAnyObject(" +
+ conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "::armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + typeAccessor);
return b;
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.h
index bfa7692ffd78cca516cb69e8a12e09d684ef9105..f3146db3e68693311ef753a35ebfe03bf8ed37be 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/any/AnyObject.h
@@ -26,11 +26,9 @@
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/AnyGenerator.h>
#include <RobotAPI/libraries/aron/core/type/variant/any/AnyObject.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
- class AnyObject :
- public detail::AnyGenerator<type::AnyObject, AnyObject>
+ class AnyObject : public detail::AnyGenerator<type::AnyObject, AnyObject>
{
public:
/* constructors */
@@ -40,10 +38,16 @@ namespace armarx::aron::codegenerator::cpp::generator
/* virtual implementations */
std::vector<std::string> getRequiredIncludes() const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/All.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/All.h
index 444268f1da13d6701494a0e24b68419b64e47212..05192c93466613dee2fb94bac520f6a0844b59ca 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/All.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/All.h
@@ -1,11 +1,12 @@
#pragma once
-#include "Object.h"
-#include "List.h"
#include "Dict.h"
-#include "Tuple.h"
+#include "List.h"
+#include "Object.h"
#include "Pair.h"
+#include "Tuple.h"
-namespace {
+namespace
+{
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Dict.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Dict.cpp
index 946f160ba9b063fc4099c29e8f5a3d9914fa3f91..45dbb9de4e7478ba9a66c5378ecb63d1d29daf67 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Dict.cpp
@@ -26,106 +26,135 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
// constructors
Dict::Dict(const type::Dict& e) :
detail::ContainerGenerator<type::Dict, Dict>(
- "std::map<std::string, " + FromAronType(*e.getAcceptedType())->getFullInstantiatedCppTypename() + ">",
- "std::map<std::string, " + FromAronType(*e.getAcceptedType())->getFullInstantiatedCppTypename() + ">",
+ "std::map<std::string, " +
+ FromAronType(*e.getAcceptedType())->getFullInstantiatedCppTypename() + ">",
+ "std::map<std::string, " +
+ FromAronType(*e.getAcceptedType())->getFullInstantiatedCppTypename() + ">",
simox::meta::get_type_name<data::dto::Dict>(),
- simox::meta::get_type_name<type::dto::Dict>(), e)
+ simox::meta::get_type_name<type::dto::Dict>(),
+ e)
{
}
// virtual implementations
- std::vector<std::string> Dict::getRequiredIncludes() const
+ std::vector<std::string>
+ Dict::getRequiredIncludes() const
{
auto c = FromAronType(*type.getAcceptedType());
return c->getRequiredIncludes();
}
- CppBlockPtr Dict::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Dict::getResetSoftBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + nextEl() + "clear();");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Dict::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Dict::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
auto type_s = FromAronType(*type.getAcceptedType());
std::string nextVariantAccessor;
Path nextPath = p.withAcceptedType(true);
- b->appendBlock(type_s->getWriteTypeBlock(type_s->getInstantiatedCppTypename(), cppAccessor + nextEl() + "accepted_type", nextPath, nextVariantAccessor));
+ b->appendBlock(type_s->getWriteTypeBlock(type_s->getInstantiatedCppTypename(),
+ cppAccessor + nextEl() + "accepted_type",
+ nextPath,
+ nextVariantAccessor));
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeDict(" + nextVariantAccessor + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeDict(" +
+ nextVariantAccessor + ", " + conversion::Maybe2CppString.at(type.getMaybe()) +
+ ", " + "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return b;
}
- CppBlockPtr Dict::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Dict::getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
const std::string elementsAccessor = variantAccessor + "_dictElements";
block_if_data->addLine("std::map<std::string, _Aron_T> " + elementsAccessor + ";");
std::string accessor_iterator_key = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_key";
- std::string accessor_iterator_val = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_value";
+ std::string accessor_iterator_val =
+ ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_value";
std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
- block_if_data->addLine("for (const auto& [" + accessor_iterator_key + ", " + accessor_iterator_val + "] : " + resolved_accessor + ") ");
+ block_if_data->addLine("for (const auto& [" + accessor_iterator_key + ", " +
+ accessor_iterator_val + "] : " + resolved_accessor + ") ");
{
auto type_s = FromAronType(*type.getAcceptedType());
CppBlockPtr for_loop = std::make_shared<CppBlock>();
std::string nextVariantAccessor;
Path nextPath = p.withElement(accessor_iterator_key);
- auto child_b = type_s->getWriteBlock(accessor_iterator_val, nextPath, nextVariantAccessor);
- for_loop->addLine("auto " + nextVariantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ auto child_b =
+ type_s->getWriteBlock(accessor_iterator_val, nextPath, nextVariantAccessor);
+ for_loop->addLine("auto " + nextVariantAccessor + " = " + ARON_WRITER_ACCESSOR +
+ ".writeNull();");
for_loop->appendBlock(child_b);
- for_loop->addLine(elementsAccessor + ".emplace(" + accessor_iterator_key + ", " + nextVariantAccessor + ");");
+ for_loop->addLine(elementsAccessor + ".emplace(" + accessor_iterator_key + ", " +
+ nextVariantAccessor + ");");
block_if_data->addBlock(for_loop);
}
- block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR +
- ".writeDict(" + elementsAccessor + ", " +
- "std::nullopt, " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeDict(" +
+ elementsAccessor + ", " + "std::nullopt, " + "armarx::aron::Path(" +
+ ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
+ "})); // of " + cppAccessor);
return resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Dict::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ Dict::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictElements";
- std::string accessor_iterator_value = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictValue";
- std::string accessor_iterator_key = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictKey";
+ std::string elements_accessor =
+ ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictElements";
+ std::string accessor_iterator_value =
+ ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictValue";
+ std::string accessor_iterator_key =
+ ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictKey";
block_if_data->addLine("std::map<std::string, _Aron_TNonConst> " + elements_accessor + ";");
- block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readDict(" + variantAccessor + ", " + elements_accessor + ");");
- block_if_data->addLine("for (const auto& [" + accessor_iterator_key + ", " + accessor_iterator_value + "] : " + elements_accessor + ")");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readDict(" + variantAccessor + ", " +
+ elements_accessor + ");");
+ block_if_data->addLine("for (const auto& [" + accessor_iterator_key + ", " +
+ accessor_iterator_value + "] : " + elements_accessor + ")");
{
auto type_s = FromAronType(*type.getAcceptedType());
CppBlockPtr for_loop = std::make_shared<CppBlock>();
- std::string accessor_iterator_tmp = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictTmp";
- for_loop->addLine(type_s->getFullInstantiatedCppTypename() + " " + accessor_iterator_tmp +";");
- for_loop->appendBlock(type_s->getReadBlock(accessor_iterator_tmp, accessor_iterator_value));
- for_loop->addLine(cppAccessor + nextEl() + "insert({" + accessor_iterator_key + ", " + accessor_iterator_tmp + "});");
+ std::string accessor_iterator_tmp =
+ ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_dictTmp";
+ for_loop->addLine(type_s->getFullInstantiatedCppTypename() + " " +
+ accessor_iterator_tmp + ";");
+ for_loop->appendBlock(
+ type_s->getReadBlock(accessor_iterator_tmp, accessor_iterator_value));
+ for_loop->addLine(cppAccessor + nextEl() + "insert({" + accessor_iterator_key + ", " +
+ accessor_iterator_tmp + "});");
block_if_data->addBlock(for_loop);
}
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
-}
-
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Dict.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Dict.h
index ae26828fd345157dadf1f74d89c871727e5e1524..e070a2a4545edd87eda32fd464f93e81a470874a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Dict.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Dict.h
@@ -23,13 +23,12 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/type/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Dict.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- class Dict :
- public detail::ContainerGenerator<type::Dict, Dict>
+ class Dict : public detail::ContainerGenerator<type::Dict, Dict>
{
public:
// constructors
@@ -39,8 +38,14 @@ namespace armarx::aron::codegenerator::cpp::generator
// virtual implementations
std::vector<std::string> getRequiredIncludes() const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/List.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/List.cpp
index e289361898f05b219b7036533c7d385c300fe180..ddf46f2a620edb65a3ce80c9b4ccc73e3860ea5e 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/List.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/List.cpp
@@ -26,34 +26,41 @@
#include <SimoxUtility/meta/type_name.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
// constructors
List::List(const type::List& e) :
detail::ContainerGenerator<type::List, List>(
- "std::vector<" + FromAronType(*e.getAcceptedType())->getFullInstantiatedCppTypename() + ">",
- "std::vector<" + FromAronType(*e.getAcceptedType())->getFullInstantiatedCppTypename() + ">",
+ "std::vector<" + FromAronType(*e.getAcceptedType())->getFullInstantiatedCppTypename() +
+ ">",
+ "std::vector<" + FromAronType(*e.getAcceptedType())->getFullInstantiatedCppTypename() +
+ ">",
simox::meta::get_type_name<data::dto::List>(),
- simox::meta::get_type_name<type::dto::List>(), e)
+ simox::meta::get_type_name<type::dto::List>(),
+ e)
{
}
-
- std::vector<std::string> List::getRequiredIncludes() const
+ std::vector<std::string>
+ List::getRequiredIncludes() const
{
auto c = FromAronType(*type.getAcceptedType());
return c->getRequiredIncludes();
}
- CppBlockPtr List::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ List::getResetSoftBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine(cppAccessor + nextEl() + "clear();");
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
- CppBlockPtr List::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ List::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr b = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
@@ -62,20 +69,27 @@ namespace armarx::aron::codegenerator::cpp::generator
auto type_s = FromAronType(*type.getAcceptedType());
std::string nextVariantAccessor;
Path nextPath = p.withAcceptedType(true);
- CppBlockPtr b2 = type_s->getWriteTypeBlock(type_s->getInstantiatedCppTypename(), cppAccessor + nextEl() + "accepted_type", nextPath, nextVariantAccessor);
+ CppBlockPtr b2 = type_s->getWriteTypeBlock(type_s->getInstantiatedCppTypename(),
+ cppAccessor + nextEl() + "accepted_type",
+ nextPath,
+ nextVariantAccessor);
b->appendBlock(b2);
- b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeList(" + nextVariantAccessor + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ b->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeList(" +
+ nextVariantAccessor + ", " + conversion::Maybe2CppString.at(type.getMaybe()) +
+ ", " + "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return b;
}
- CppBlockPtr List::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ List::getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
const std::string elementsAccessor = variantAccessor + "_listElements";
block_if_data->addLine("std::vector<_Aron_T> " + elementsAccessor + ";");
@@ -83,46 +97,62 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string accessor_iterator = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_it";
auto type_s = FromAronType(*type.getAcceptedType());
- block_if_data->addLine("for(unsigned int " + accessor_iterator + " = 0; " + accessor_iterator + " < " + cppAccessor + nextEl() + "size(); ++" + accessor_iterator + ")");
+ block_if_data->addLine("for(unsigned int " + accessor_iterator + " = 0; " +
+ accessor_iterator + " < " + cppAccessor + nextEl() + "size(); ++" +
+ accessor_iterator + ")");
{
std::string nextVariantAccessor;
auto for_loop = std::make_shared<CppBlock>();
Path nextPath = p.withElement("std::to_string(" + accessor_iterator + ")");
- auto child_b = type_s->getWriteBlock(cppAccessor + nextEl() + "at(" + accessor_iterator + ")", nextPath, nextVariantAccessor);
- for_loop->addLine("auto " + nextVariantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ auto child_b =
+ type_s->getWriteBlock(cppAccessor + nextEl() + "at(" + accessor_iterator + ")",
+ nextPath,
+ nextVariantAccessor);
+ for_loop->addLine("auto " + nextVariantAccessor + " = " + ARON_WRITER_ACCESSOR +
+ ".writeNull();");
for_loop->appendBlock(child_b);
for_loop->addLine(elementsAccessor + ".push_back(" + nextVariantAccessor + ");");
block_if_data->addBlock(for_loop);
}
Path path = this->type.getPath();
- block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeList(" + elementsAccessor + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeList(" +
+ elementsAccessor + ", " + "armarx::aron::Path(" +
+ ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
+ "})); // of " + cppAccessor);
return resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
- CppBlockPtr List::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ List::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listElements";
- std::string accessor_iterator_value = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listValue";
+ std::string elements_accessor =
+ ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listElements";
+ std::string accessor_iterator_value =
+ ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listValue";
block_if_data->addLine("std::vector<_Aron_TNonConst> " + elements_accessor + ";");
- block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList(" + variantAccessor + ", " + elements_accessor + ");");
- block_if_data->addLine("for (const auto& " + accessor_iterator_value + " : " + elements_accessor + ")");
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList(" + variantAccessor + ", " +
+ elements_accessor + ");");
+ block_if_data->addLine("for (const auto& " + accessor_iterator_value + " : " +
+ elements_accessor + ")");
{
CppBlockPtr for_loop = std::make_shared<CppBlock>();
auto type_s = FromAronType(*type.getAcceptedType());
- std::string accessor_iterator_tmp = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listTmp";
- for_loop->addLine(type_s->getFullInstantiatedCppTypename() + " " + accessor_iterator_tmp + ";");
- for_loop->appendBlock(type_s->getReadBlock(accessor_iterator_tmp, accessor_iterator_value));
+ std::string accessor_iterator_tmp =
+ ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_listTmp";
+ for_loop->addLine(type_s->getFullInstantiatedCppTypename() + " " +
+ accessor_iterator_tmp + ";");
+ for_loop->appendBlock(
+ type_s->getReadBlock(accessor_iterator_tmp, accessor_iterator_value));
for_loop->addLine(cppAccessor + nextEl() + "push_back(" + accessor_iterator_tmp + ");");
block_if_data->addBlock(for_loop);
}
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/List.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/List.h
index 5c7302a684364a52fda82727c3c6f73a883222c4..a32b3c3cd80a71352cb45348d6f4a1e6337da80e 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/List.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/List.h
@@ -23,14 +23,12 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/type/variant/container/List.h>
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h>
-
+#include <RobotAPI/libraries/aron/core/type/variant/container/List.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- class List :
- public detail::ContainerGenerator<type::List, List>
+ class List : public detail::ContainerGenerator<type::List, List>
{
public:
// constructors
@@ -40,8 +38,14 @@ namespace armarx::aron::codegenerator::cpp::generator
// virtual implementations
std::vector<std::string> getRequiredIncludes() const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.h
index 942dc161a8792b9b9455c08db5c40eba79260605..3abea62a82c94b6703818092a24800fc8dd2c38d 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Object.h
@@ -26,7 +26,6 @@
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
-
namespace armarx::aron::codegenerator::cpp::generator
{
struct DTOObjectReplacement
@@ -36,12 +35,13 @@ namespace armarx::aron::codegenerator::cpp::generator
std::string replacedTypename;
std::string replacedInstantiatedTypename;
- std::vector<std::string> additionalIncludes; // additional includes for the replaced type and aron conversions
- std::vector<std::string> disallowedBases; // disallow replacement if used in a specific class
+ std::vector<std::string>
+ additionalIncludes; // additional includes for the replaced type and aron conversions
+ std::vector<std::string>
+ disallowedBases; // disallow replacement if used in a specific class
};
- class Object :
- public detail::ContainerGenerator<type::Object, Object>
+ class Object : public detail::ContainerGenerator<type::Object, Object>
{
using Base = detail::ContainerGenerator<type::Object, Object>;
@@ -52,13 +52,19 @@ namespace armarx::aron::codegenerator::cpp::generator
// virtual implementations
std::vector<std::string> getRequiredIncludes() const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
private:
bool has_been_replaced = false;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Pair.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Pair.cpp
index 000938acceed8cf947f4317204431ae930534a8d..499d99fb051f2606c6dc35ee0f31aac4ac1f2431 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Pair.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Pair.cpp
@@ -24,22 +24,25 @@
// Header
#include "Pair.h"
-#include <SimoxUtility/meta/type_name.h>
#include <SimoxUtility/algorithm/vector.hpp>
+#include <SimoxUtility/meta/type_name.h>
namespace armarx::aron::codegenerator::cpp::generator
{
Pair::Pair(const type::Pair& e) :
detail::ContainerGenerator<type::Pair, Pair>(
- "std::pair<" + ExtractCppTypename(*e.getFirstAcceptedType()) + ", " + ExtractCppTypename(*e.getSecondAcceptedType()) + ">",
- "std::pair<" + ExtractCppTypename(*e.getFirstAcceptedType()) + ", " + ExtractCppTypename(*e.getSecondAcceptedType()) + ">",
+ "std::pair<" + ExtractCppTypename(*e.getFirstAcceptedType()) + ", " +
+ ExtractCppTypename(*e.getSecondAcceptedType()) + ">",
+ "std::pair<" + ExtractCppTypename(*e.getFirstAcceptedType()) + ", " +
+ ExtractCppTypename(*e.getSecondAcceptedType()) + ">",
simox::meta::get_type_name<data::dto::List>(),
- simox::meta::get_type_name<type::dto::Pair>(), e)
+ simox::meta::get_type_name<type::dto::Pair>(),
+ e)
{
}
-
- std::vector<std::string> Pair::getRequiredIncludes() const
+ std::vector<std::string>
+ Pair::getRequiredIncludes() const
{
auto child_s1 = FromAronType(*type.getFirstAcceptedType());
auto child_s2 = FromAronType(*type.getSecondAcceptedType());
@@ -50,7 +53,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return simox::alg::appended(l1, l2);
}
- CppBlockPtr Pair::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Pair::getResetSoftBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
@@ -65,79 +69,102 @@ namespace armarx::aron::codegenerator::cpp::generator
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Pair::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Pair::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
auto child_s1 = FromAronType(*type.getFirstAcceptedType());
std::string accessor_iterator1 = cppAccessor + nextEl() + "first";
std::string firstVariantAccessor;
Path firstPath = p.withAcceptedTypeIndex(0);
- CppBlockPtr b21 = child_s1->getWriteTypeBlock(child_s1->getInstantiatedCppTypename(), accessor_iterator1, firstPath, firstVariantAccessor);
+ CppBlockPtr b21 = child_s1->getWriteTypeBlock(child_s1->getInstantiatedCppTypename(),
+ accessor_iterator1,
+ firstPath,
+ firstVariantAccessor);
block_if_data->appendBlock(b21);
auto child_s2 = FromAronType(*type.getSecondAcceptedType());
std::string accessor_iterator2 = cppAccessor + nextEl() + "second";
std::string secondVariantAccessor;
Path secondPath = p.withAcceptedTypeIndex(1);
- CppBlockPtr b22 = child_s2->getWriteTypeBlock(child_s2->getInstantiatedCppTypename(), accessor_iterator2, secondPath, secondVariantAccessor);
+ CppBlockPtr b22 = child_s2->getWriteTypeBlock(child_s2->getInstantiatedCppTypename(),
+ accessor_iterator2,
+ secondPath,
+ secondVariantAccessor);
block_if_data->appendBlock(b22);
- block_if_data->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePair(" + firstVariantAccessor + ", " +
- secondVariantAccessor + ", " +
- conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ block_if_data->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR +
+ ".writePair(" + firstVariantAccessor + ", " + secondVariantAccessor +
+ ", " + conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return block_if_data;
}
- CppBlockPtr Pair::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Pair::getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
auto child_s1 = FromAronType(*type.getFirstAcceptedType());
std::string accessor_iterator1 = cppAccessor + nextEl() + "first";
std::string firstVariantAccessor;
Path firstPath = p.withElement("\"0\"");
- CppBlockPtr b21 = child_s1->getWriteBlock(accessor_iterator1, firstPath, firstVariantAccessor);
- block_if_data->addLine("auto " + firstVariantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ CppBlockPtr b21 =
+ child_s1->getWriteBlock(accessor_iterator1, firstPath, firstVariantAccessor);
+ block_if_data->addLine("auto " + firstVariantAccessor + " = " + ARON_WRITER_ACCESSOR +
+ ".writeNull();");
block_if_data->appendBlock(b21);
auto child_s2 = FromAronType(*type.getSecondAcceptedType());
std::string accessor_iterator2 = cppAccessor + nextEl() + "second";
std::string secondVariantAccessor;
Path secondPath = p.withElement("\"1\"");
- CppBlockPtr b22 = child_s2->getWriteBlock(accessor_iterator2, secondPath, secondVariantAccessor);
- block_if_data->addLine("auto " + secondVariantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ CppBlockPtr b22 =
+ child_s2->getWriteBlock(accessor_iterator2, secondPath, secondVariantAccessor);
+ block_if_data->addLine("auto " + secondVariantAccessor + " = " + ARON_WRITER_ACCESSOR +
+ ".writeNull();");
block_if_data->appendBlock(b22);
- block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePair("+firstVariantAccessor+", "+
- secondVariantAccessor+", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {" + simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writePair(" +
+ firstVariantAccessor + ", " + secondVariantAccessor + ", " +
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Pair::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ Pair::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_pairElements";
+ std::string elements_accessor =
+ ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_pairElements";
block_if_data->addLine("std::vector<_Aron_TNonConst> " + elements_accessor + ";");
- block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList("+elements_accessor+"); // of " + cppAccessor);
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList(" + elements_accessor +
+ "); // of " + cppAccessor);
auto child_s1 = FromAronType(*type.getFirstAcceptedType());
- CppBlockPtr b21 = child_s1->getReadBlock(cppAccessor + nextEl() + "first", elements_accessor+"[0]");
+ CppBlockPtr b21 =
+ child_s1->getReadBlock(cppAccessor + nextEl() + "first", elements_accessor + "[0]");
block_if_data->appendBlock(b21);
auto child_s2 = FromAronType(*type.getSecondAcceptedType());
- CppBlockPtr b22 = child_s2->getReadBlock(cppAccessor + nextEl() + "second", elements_accessor+"[1]");
+ CppBlockPtr b22 =
+ child_s2->getReadBlock(cppAccessor + nextEl() + "second", elements_accessor + "[1]");
block_if_data->appendBlock(b22);
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
-}
-
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Pair.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Pair.h
index f37087d03021659aa0b7d362caeb9ff12eeffb69..5d820d510aca43d3499269b56449b77c8a0fe255 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Pair.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Pair.h
@@ -23,14 +23,12 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/type/variant/container/Pair.h>
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h>
-
+#include <RobotAPI/libraries/aron/core/type/variant/container/Pair.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- class Pair :
- public detail::ContainerGenerator<type::Pair, Pair>
+ class Pair : public detail::ContainerGenerator<type::Pair, Pair>
{
public:
// constructors
@@ -40,8 +38,14 @@ namespace armarx::aron::codegenerator::cpp::generator
// virtual implementations
std::vector<std::string> getRequiredIncludes() const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Tuple.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Tuple.cpp
index 2d49addc569ae4e5f1400f7d18794c1221115fd9..8316ae8c13d3723b0d7d729291f2dcb6cd60c562 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Tuple.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Tuple.cpp
@@ -24,8 +24,8 @@
// Header
#include "Tuple.h"
-#include <SimoxUtility/meta/type_name.h>
#include <SimoxUtility/algorithm/vector.hpp>
+#include <SimoxUtility/meta/type_name.h>
namespace armarx::aron::codegenerator::cpp::generator
{
@@ -34,12 +34,13 @@ namespace armarx::aron::codegenerator::cpp::generator
"std::tuple<" + simox::alg::join(ExtractCppTypenames(e.getAcceptedTypes()), ", ") + ">",
"std::tuple<" + simox::alg::join(ExtractCppTypenames(e.getAcceptedTypes()), ", ") + ">",
simox::meta::get_type_name<data::dto::List>(),
- simox::meta::get_type_name<type::dto::Tuple>(), e)
+ simox::meta::get_type_name<type::dto::Tuple>(),
+ e)
{
}
-
- std::vector<std::string> Tuple::getRequiredIncludes() const
+ std::vector<std::string>
+ Tuple::getRequiredIncludes() const
{
std::vector<std::string> ret;
for (const auto& child : type.getAcceptedTypes())
@@ -50,7 +51,8 @@ namespace armarx::aron::codegenerator::cpp::generator
return ret;
}
- CppBlockPtr Tuple::getResetSoftBlock(const std::string& cppAccessor) const
+ CppBlockPtr
+ Tuple::getResetSoftBlock(const std::string& cppAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
@@ -59,18 +61,23 @@ namespace armarx::aron::codegenerator::cpp::generator
for (const auto& child : type.getAcceptedTypes())
{
auto child_s = FromAronType(*child);
- CppBlockPtr b2 = child_s->getResetSoftBlock("std::get<" + std::to_string(i++) + ">(" + resolved_accessor + ")");
+ CppBlockPtr b2 = child_s->getResetSoftBlock("std::get<" + std::to_string(i++) + ">(" +
+ resolved_accessor + ")");
block_if_data->appendBlock(b2);
}
return this->resolveMaybeResetSoftBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Tuple::getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Tuple::getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
const std::string acceptedTypesAccessor = variantAccessor + "_tupleAcceptedTypes";
block_if_data->addLine("std::vector<_Aron_T> " + acceptedTypesAccessor + ";");
@@ -78,26 +85,36 @@ namespace armarx::aron::codegenerator::cpp::generator
unsigned int i = 0;
for (const auto& type : type.getAcceptedTypes())
{
- std::string accessor_iterator = "std::get<" + std::to_string(i) + ">("+resolved_accessor+");";
+ std::string accessor_iterator =
+ "std::get<" + std::to_string(i) + ">(" + resolved_accessor + ");";
auto type_s = FromAronType(*type);
std::string nextVariantAccessor;
Path nextPath = p.withAcceptedTypeIndex(i++);
- CppBlockPtr b2 = type_s->getWriteTypeBlock(type_s->getInstantiatedCppTypename(), accessor_iterator, nextPath, nextVariantAccessor);
+ CppBlockPtr b2 = type_s->getWriteTypeBlock(type_s->getInstantiatedCppTypename(),
+ accessor_iterator,
+ nextPath,
+ nextVariantAccessor);
block_if_data->appendBlock(b2);
- block_if_data->addLine(acceptedTypesAccessor + ".push_back(" + nextVariantAccessor + ");");
+ block_if_data->addLine(acceptedTypesAccessor + ".push_back(" + nextVariantAccessor +
+ ");");
}
- block_if_data->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeTuple(" + acceptedTypesAccessor + ", " +
+ block_if_data->addLine("auto " + variantAccessor + " = " + ARON_WRITER_ACCESSOR +
+ ".writeTuple(" + acceptedTypesAccessor + ", " +
conversion::Maybe2CppString.at(type.getMaybe()) + ", " +
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {"+simox::alg::join(p.getPath(), ", ")+"})); // of " + cppAccessor);
+ "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
+ simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return block_if_data;
}
- CppBlockPtr Tuple::getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const
+ CppBlockPtr
+ Tuple::getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
std::string escaped_accessor = EscapeAccessor(cppAccessor);
- variantAccessor = ARON_VARIANT_RETURN_ACCESSOR+ "_" + escaped_accessor;
+ variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
const std::string elementsAccessor = variantAccessor + "_tupleElements";
block_if_data->addLine("std::vector<_Aron_T> " + elementsAccessor + ";");
@@ -105,40 +122,49 @@ namespace armarx::aron::codegenerator::cpp::generator
unsigned int i = 0;
for (const auto& type : type.getAcceptedTypes())
{
- std::string accessor_iterator = "std::get<" + std::to_string(i) + ">("+resolved_accessor+");";
+ std::string accessor_iterator =
+ "std::get<" + std::to_string(i) + ">(" + resolved_accessor + ");";
auto type_s = FromAronType(*type);
std::string nextVariantAccessor;
Path nextPath = p.withElement("\"" + std::to_string(i++) + "\"");
- CppBlockPtr b2 = type_s->getWriteBlock(accessor_iterator, nextPath, nextVariantAccessor);
- block_if_data->addLine("auto " + nextVariantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNull();");
+ CppBlockPtr b2 =
+ type_s->getWriteBlock(accessor_iterator, nextPath, nextVariantAccessor);
+ block_if_data->addLine("auto " + nextVariantAccessor + " = " + ARON_WRITER_ACCESSOR +
+ ".writeNull();");
block_if_data->appendBlock(b2);
block_if_data->addLine(elementsAccessor + ".push_back(" + nextVariantAccessor + ");");
}
- block_if_data->addLine(variantAccessor+ " = " + ARON_WRITER_ACCESSOR + ".writeTuple("+elementsAccessor+", "+
- "armarx::aron::Path("+ARON_PATH_ACCESSOR+", {" + simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
+ block_if_data->addLine(variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeTuple(" +
+ elementsAccessor + ", " + "armarx::aron::Path(" +
+ ARON_PATH_ACCESSOR + ", {" + simox::alg::join(p.getPath(), ", ") +
+ "})); // of " + cppAccessor);
return resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
- CppBlockPtr Tuple::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
+ CppBlockPtr
+ Tuple::getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
std::string escaped_accessor = EscapeAccessor(cppAccessor);
std::string resolved_accessor = resolveMaybeAccessor(cppAccessor);
- std::string elements_accessor = ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_tupleElements";
+ std::string elements_accessor =
+ ARON_VARIABLE_PREFIX + "_" + escaped_accessor + "_tupleElements";
block_if_data->addLine("std::vector<_Aron_TNonConst> " + elements_accessor + ";");
- block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList("+elements_accessor+"); // of " + cppAccessor);
+ block_if_data->addLine("" + ARON_READER_ACCESSOR + ".readList(" + elements_accessor +
+ "); // of " + cppAccessor);
unsigned int i = 0;
for (const auto& type : type.getAcceptedTypes())
{
auto type_s = FromAronType(*type);
- CppBlockPtr b2 = type_s->getReadBlock("std::get<" + std::to_string(i) + ">(" + resolved_accessor + ")", elements_accessor+"[" + std::to_string(i) + "]");
+ CppBlockPtr b2 = type_s->getReadBlock(
+ "std::get<" + std::to_string(i) + ">(" + resolved_accessor + ")",
+ elements_accessor + "[" + std::to_string(i) + "]");
block_if_data->appendBlock(b2);
i++;
}
return resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
-}
-
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Tuple.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Tuple.h
index 52e565e896f2ed27bc88ef892a3cde348122865f..7a3924bb831e6351f704382e9c4f4f81fd3f304a 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Tuple.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/container/Tuple.h
@@ -23,14 +23,12 @@
#pragma once
-#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h>
-
+#include <RobotAPI/libraries/aron/core/type/variant/container/Tuple.h>
namespace armarx::aron::codegenerator::cpp::generator
{
- class Tuple :
- public detail::ContainerGenerator<type::Tuple, Tuple>
+ class Tuple : public detail::ContainerGenerator<type::Tuple, Tuple>
{
public:
// constructors
@@ -40,8 +38,14 @@ namespace armarx::aron::codegenerator::cpp::generator
// virtual implementations
std::vector<std::string> getRequiredIncludes() const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
- CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor, const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path&, std::string& variantAccessor) const final;
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const final;
+ CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
+ const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getWriteBlock(const std::string& cppAccessor,
+ const Path&,
+ std::string& variantAccessor) const final;
+ CppBlockPtr getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const final;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/AnyGenerator.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/AnyGenerator.h
index 5260c3407b672a9cc99e03f63ae22048e57a9b48..a1f948724a2827aa350ff86ed0a78c27d39e9f8e 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/AnyGenerator.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/AnyGenerator.h
@@ -23,19 +23,17 @@
#pragma once
-#include "SpecializedGenerator.h"
-
#include <string>
+#include "SpecializedGenerator.h"
namespace armarx::aron::codegenerator::cpp::generator::detail
{
- template<typename typeT, typename DerivedT>
- class AnyGenerator :
- public SpecializedGeneratorBase<typeT, DerivedT>
+ template <typename typeT, typename DerivedT>
+ class AnyGenerator : public SpecializedGeneratorBase<typeT, DerivedT>
{
public:
using SpecializedGeneratorBase<typeT, DerivedT>::SpecializedGeneratorBase;
virtual ~AnyGenerator() = default;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator::detail
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h
index a391de695dc589500bff7a8cb13793801f242744..3ff288bb5547ff9c4c97b2e65d7451a505a37d12 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/ContainerGenerator.h
@@ -27,12 +27,11 @@
namespace armarx::aron::codegenerator::cpp::generator::detail
{
- template<typename typeT, typename DerivedT>
- class ContainerGenerator :
- public SpecializedGeneratorBase<typeT, DerivedT>
+ template <typename typeT, typename DerivedT>
+ class ContainerGenerator : public SpecializedGeneratorBase<typeT, DerivedT>
{
public:
using SpecializedGeneratorBase<typeT, DerivedT>::SpecializedGeneratorBase;
virtual ~ContainerGenerator() = default;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator::detail
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h
index 834262520b99edede248c28b01fa39f9b3aa8153..b7c74f0d2dbe773ea9ac50f3ce83df66bf965513 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/NDArrayGenerator.h
@@ -25,15 +25,13 @@
#include "SpecializedGenerator.h"
-
namespace armarx::aron::codegenerator::cpp::generator::detail
{
- template<typename typeT, typename DerivedT>
- class NDArrayGenerator :
- public SpecializedGeneratorBase<typeT, DerivedT>
+ template <typename typeT, typename DerivedT>
+ class NDArrayGenerator : public SpecializedGeneratorBase<typeT, DerivedT>
{
public:
using SpecializedGeneratorBase<typeT, DerivedT>::SpecializedGeneratorBase;
virtual ~NDArrayGenerator() = default;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator::detail
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h
index c4ebdfacb0c7622493726d37eb6245c52b8179b3..bdb1cd1a4b6b589c31b875c35b0ffabaf9278d7e 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/PrimitiveGenerator.h
@@ -23,42 +23,49 @@
#pragma once
-#include "SpecializedGenerator.h"
-
#include <string>
+#include "SpecializedGenerator.h"
namespace armarx::aron::codegenerator::cpp::generator::detail
{
- template<typename typeT, typename DerivedT>
- class PrimitiveGenerator :
- public SpecializedGeneratorBase<typeT, DerivedT>
+ template <typename typeT, typename DerivedT>
+ class PrimitiveGenerator : public SpecializedGeneratorBase<typeT, DerivedT>
{
public:
using SpecializedGeneratorBase<typeT, DerivedT>::SpecializedGeneratorBase;
virtual ~PrimitiveGenerator() = default;
- CppBlockPtr getWriteBlock(const std::string& cppAccessor, const Path& p, std::string& variantAccessor) const override
+ CppBlockPtr
+ getWriteBlock(const std::string& cppAccessor,
+ const Path& p,
+ std::string& variantAccessor) const override
{
auto block_if_data = std::make_shared<CppBlock>();
std::string resolved_accessor = this->resolveMaybeAccessor(cppAccessor);
std::string escaped_accessor = this->EscapeAccessor(cppAccessor);
variantAccessor = Generator::ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
- block_if_data->addLine(variantAccessor + " = " + this->ARON_WRITER_ACCESSOR + ".writePrimitive(" + resolved_accessor + ", "+
- "armarx::aron::Path("+this->ARON_PATH_ACCESSOR+", {" + simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
+ block_if_data->addLine(
+ variantAccessor + " = " + this->ARON_WRITER_ACCESSOR + ".writePrimitive(" +
+ resolved_accessor + ", " + "armarx::aron::Path(" + this->ARON_PATH_ACCESSOR +
+ ", {" + simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
return this->resolveMaybeWriteBlock(block_if_data, cppAccessor);
}
- CppBlockPtr getReadBlock(const std::string& cppAccessor, const std::string& variantAccessor) const override
+ CppBlockPtr
+ getReadBlock(const std::string& cppAccessor,
+ const std::string& variantAccessor) const override
{
auto block_if_data = std::make_shared<CppBlock>();
std::string resolved_accessor = this->resolveMaybeAccessor(cppAccessor);
std::string escaped_accessor = this->EscapeAccessor(cppAccessor);
- block_if_data->addLine("" + this->ARON_READER_ACCESSOR + ".readPrimitive("+variantAccessor+", " + resolved_accessor + "); // of " + cppAccessor);
+ block_if_data->addLine("" + this->ARON_READER_ACCESSOR + ".readPrimitive(" +
+ variantAccessor + ", " + resolved_accessor + "); // of " +
+ cppAccessor);
return this->resolveMaybeReadBlock(block_if_data, cppAccessor, variantAccessor);
}
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator::detail
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h
index ba487310e240d62c476ec40a74d55c91ed3c955e..634047199face5551902ab324768e095e2aa18af 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/detail/SpecializedGenerator.h
@@ -23,27 +23,34 @@
#pragma once
-#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.h>
-
#include <memory>
#include <string>
+#include <RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/Generator.h>
namespace armarx::aron::codegenerator::cpp::generator::detail
{
- template<typename TypeT, typename DerivedT>
- class SpecializedGeneratorBase :
- public codegenerator::cpp::Generator
+ template <typename TypeT, typename DerivedT>
+ class SpecializedGeneratorBase : public codegenerator::cpp::Generator
{
public:
- SpecializedGeneratorBase(const std::string& instantiatedCppTypename, const std::string& classCppTypename, const std::string& aronDataTypename, const std::string& aronTypeTypename, const TypeT& t) :
- Generator(instantiatedCppTypename, classCppTypename, aronDataTypename, aronTypeTypename),
+ SpecializedGeneratorBase(const std::string& instantiatedCppTypename,
+ const std::string& classCppTypename,
+ const std::string& aronDataTypename,
+ const std::string& aronTypeTypename,
+ const TypeT& t) :
+ Generator(instantiatedCppTypename,
+ classCppTypename,
+ aronDataTypename,
+ aronTypeTypename),
type(t)
{
}
+
virtual ~SpecializedGeneratorBase() = default;
- virtual const type::Variant& getType() const override
+ virtual const type::Variant&
+ getType() const override
{
return type;
}
@@ -51,4 +58,4 @@ namespace armarx::aron::codegenerator::cpp::generator::detail
protected:
TypeT type;
};
-}
+} // namespace armarx::aron::codegenerator::cpp::generator::detail
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/All.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/All.h
index c10804705a973fc89e66424d1f37b97a4503ac05..071dffed391febc9a267fcbbffb70602a369bccd 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/All.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/enum/All.h
@@ -2,6 +2,7 @@
#include "IntEnum.h"
-namespace {
+namespace
+{
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/All.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/All.h
index fc5eca9a839eaeade25b182e5bcd1026df66140c..6d44a3846f34bbb5bc6d6a338a366f19759e1980 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/All.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/All.h
@@ -1,11 +1,12 @@
#pragma once
-#include "NDArray.h"
-#include "Matrix.h"
-#include "Quaternion.h"
#include "Image.h"
+#include "Matrix.h"
+#include "NDArray.h"
#include "PointCloud.h"
+#include "Quaternion.h"
-namespace {
+namespace
+{
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
index 818ff92d715ea10294b1d0c9c60d01417952112c..15e610d3dc32bb65f7fc9da5ae409042cde60cbe 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
@@ -33,10 +33,14 @@ namespace armarx::aron::codegenerator::cpp::generator
const std::map<type::matrix::ElementType, std::tuple<std::string, int, std::string>>
ElementType2Cpp = {
{type::matrix::UINT8,
- {TypeName<unsigned char>::Get(), sizeof(unsigned char), "::armarx::aron::type::matrix::UINT8"}},
- {type::matrix::UINT16,
- {TypeName<unsigned short>::Get(), sizeof(unsigned short), "::armarx::aron::type::matrix::UINT16"}},
- {type::matrix::INT8,
+ {TypeName<unsigned char>::Get(),
+ sizeof(unsigned char),
+ "::armarx::aron::type::matrix::UINT8"}},
+ {type::matrix::UINT16,
+ {TypeName<unsigned short>::Get(),
+ sizeof(unsigned short),
+ "::armarx::aron::type::matrix::UINT16"}},
+ {type::matrix::INT8,
{TypeName<char>::Get(), sizeof(char), "::armarx::aron::type::matrix::INT8"}},
{type::matrix::INT16,
{TypeName<short>::Get(), sizeof(short), "::armarx::aron::type::matrix::INT16"}},
@@ -83,7 +87,8 @@ namespace armarx::aron::codegenerator::cpp::generator
{
return {{{name, getInstantiatedCppTypename() + "::Identity()"}}, true};
}
- else if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT || type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT ||
+ type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
{
return {{{name, getInstantiatedCppTypename() + "::Zero()"}}, true};
}
@@ -111,11 +116,14 @@ namespace armarx::aron::codegenerator::cpp::generator
else if (type.getDefaultValue() == aron::type::matrix::default_value::IDENTITY)
{
- block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::Identity();");
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
+ "::Identity();");
}
- else if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT || type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT ||
+ type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
{
- block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::Zero();");
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
+ "::Zero();");
}
else if (type.getDefaultValue() == aron::type::matrix::default_value::ONES)
{
@@ -124,7 +132,8 @@ namespace armarx::aron::codegenerator::cpp::generator
else if (not type.getDefaultValue().empty())
{
// try to parse num. We ensure from typereader that defaultValue is valid number
- block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::One() * " + type.getDefaultValue() + ";");
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
+ "::One() * " + type.getDefaultValue() + ";");
}
return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
index 2c5a6d1d30bd297e35a4b5592a8bf7e32ef8abcb..d63d6dd3776472ccf8791b9c100136b0808a2ef0 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/PointCloud.cpp
@@ -115,10 +115,10 @@ namespace armarx::aron::codegenerator::cpp::generator
variantAccessor = ARON_VARIANT_RETURN_ACCESSOR + "_" + escaped_accessor;
block_if_data->addLine(
- variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({ static_cast<int>(" + cppAccessor +
- nextEl() + "width), static_cast<int>(" + cppAccessor + nextEl() + "height), " +
- std::to_string(std::get<1>(VoxelType2Cpp.at(type.getVoxelType()))) + "}, " + "\"" +
- std::get<0>(VoxelType2Cpp.at(type.getVoxelType())) + "\", " +
+ variantAccessor + " = " + ARON_WRITER_ACCESSOR + ".writeNDArray({ static_cast<int>(" +
+ cppAccessor + nextEl() + "width), static_cast<int>(" + cppAccessor + nextEl() +
+ "height), " + std::to_string(std::get<1>(VoxelType2Cpp.at(type.getVoxelType()))) +
+ "}, " + "\"" + std::get<0>(VoxelType2Cpp.at(type.getVoxelType())) + "\", " +
"reinterpret_cast<const unsigned char*>(" + cppAccessor + nextEl() +
"points.data()), " + "armarx::aron::Path(" + ARON_PATH_ACCESSOR + ", {" +
simox::alg::join(p.getPath(), ", ") + "})); // of " + cppAccessor);
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
index 4e0b18b9f4b3da43090ece4da539b8ad4bbaf8c2..b3297323c181badedab9009d5225c376cb7fb9b3 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
@@ -88,20 +88,24 @@ namespace armarx::aron::codegenerator::cpp::generator
if (type.getDefaultValue() == aron::type::quaternion::default_value::DEFAULT)
{
- block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::Identity();");
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
+ "::Identity();");
}
else if (type.getDefaultValue() == aron::type::quaternion::default_value::ZEROS)
{
- block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(0, 0, 0, 0);");
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
+ "(0, 0, 0, 0);");
}
else if (type.getDefaultValue() == aron::type::quaternion::default_value::ONES)
{
- block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(1, 1, 1, 1);");
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() +
+ "(1, 1, 1, 1);");
}
else if (not type.getDefaultValue().empty())
{
// try to parse num. We ensure from typereader that defaultValue is valid number
- block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(" + type.getDefaultValue() + ");");
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(" +
+ type.getDefaultValue() + ");");
}
return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
}
@@ -177,7 +181,9 @@ namespace armarx::aron::codegenerator::cpp::generator
{
CppBlockPtr block_if_data = std::make_shared<CppBlock>();
block_if_data->addLine("if (not (" + accessor + nextEl() + "isApprox(" +
- (type.getMaybe() != type::Maybe::NONE ? "*" + otherInstanceAccessor : otherInstanceAccessor) + ")))");
+ (type.getMaybe() != type::Maybe::NONE ? "*" + otherInstanceAccessor
+ : otherInstanceAccessor) +
+ ")))");
block_if_data->addLineAsBlock("return false;");
return resolveMaybeEqualsBlock(block_if_data, accessor, otherInstanceAccessor);
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/All.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/All.h
index 5865212424750cfad551e5ed259d9b9dfc399438..2aa16196b46929cf9ea6f50397f66b637516ee46 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/All.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/primitive/All.h
@@ -1,12 +1,13 @@
#pragma once
+#include "Bool.h"
+#include "Double.h"
+#include "Float.h"
#include "Int.h"
#include "Long.h"
-#include "Float.h"
-#include "Double.h"
#include "String.h"
-#include "Bool.h"
-namespace {
+namespace
+{
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/All.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/All.h
index 310538fe36bf8db207f3bb04eb3e9ab1cb2d03ac..64758a762beaf8df350e542ab44e8c873286fefd 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/All.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/toplevel/All.h
@@ -3,6 +3,7 @@
#include "IntEnumClass.h"
#include "ObjectClass.h"
-namespace {
+namespace
+{
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/helper/ReaderInfo.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/helper/ReaderInfo.h
index 486a57879e42fa1eaced1d76231bf63f181a7312..a456868065fc5cc7cac9cf9deb1757484ad468e8 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/helper/ReaderInfo.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/helper/ReaderInfo.h
@@ -40,11 +40,10 @@ namespace armarx::aron::codegenerator
bool override = false;
};
-
struct StaticReaderInfo
{
std::string methodName;
std::string argumentType;
std::string returnType;
};
-}
+} // namespace armarx::aron::codegenerator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/helper/WriterInfo.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/helper/WriterInfo.h
index c7dc87033b6fe87356e2b79a42b477ed8bd37b3d..a3f28fd7c1ece6930bdfcd5f47b551629c58e64b 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/helper/WriterInfo.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/helper/WriterInfo.h
@@ -39,4 +39,4 @@ namespace armarx::aron::codegenerator
std::string enforceMemberAccess = "";
bool override = false;
};
-}
+} // namespace armarx::aron::codegenerator
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aronConversionTest.cpp b/source/RobotAPI/libraries/aron/codegeneration/test/aronConversionTest.cpp
index 636f9bde6472fb9596a0a1a07d6343f3690da808..81f33242ff705bbcced3792e200aabc8d21d0d92 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aronConversionTest.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aronConversionTest.cpp
@@ -25,17 +25,17 @@
#define ARMARX_BOOST_TEST
// STD/STL
-#include <iostream>
#include <cstdlib>
#include <ctime>
+#include <iostream>
#include <numeric>
// Test
#include <RobotAPI/Test.h>
// Aron
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/codegeneration/test/aron/HumanPoseTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
using namespace armarx;
@@ -51,13 +51,15 @@ namespace armarx
HumanPoseBO() = default;
- void fromAron(const HumanPoseTest& aron)
+ void
+ fromAron(const HumanPoseTest& aron)
{
jointValues = aron.jointValues;
reached = aron.reached;
}
- HumanPoseTest toAron()
+ HumanPoseTest
+ toAron()
{
HumanPoseTest aron;
aron.jointValues = jointValues;
@@ -66,13 +68,15 @@ namespace armarx
}
};
- void fromAron(const HumanPoseTest& aron, HumanPoseBO& bo)
+ void
+ fromAron(const HumanPoseTest& aron, HumanPoseBO& bo)
{
bo.jointValues = aron.jointValues;
bo.reached = aron.reached;
}
- void toAron(HumanPoseTest& aron, const HumanPoseBO& bo)
+ void
+ toAron(HumanPoseTest& aron, const HumanPoseBO& bo)
{
aron.jointValues = bo.jointValues;
aron.reached = bo.reached;
@@ -81,7 +85,7 @@ namespace armarx
BOOST_AUTO_TEST_CASE(AronAssignmentTest)
{
armarx::HumanPoseTest aron;
- aron.jointValues = {1,2,3,4};
+ aron.jointValues = {1, 2, 3, 4};
aron.reached = true;
HumanPoseBO bo;
@@ -94,4 +98,4 @@ namespace armarx
BOOST_CHECK((aron.jointValues == bo2.jointValues));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aronExtendsTest.cpp b/source/RobotAPI/libraries/aron/codegeneration/test/aronExtendsTest.cpp
index df673534548fb07a2a67e9eb80aed77ca249f988..d7b0b657b85de87c00613518d1cef19e2ccdde16 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aronExtendsTest.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aronExtendsTest.cpp
@@ -25,17 +25,18 @@
#define ARMARX_BOOST_TEST
// STD/STL
-#include <iostream>
#include <cstdlib>
#include <ctime>
+#include <iostream>
#include <numeric>
// Test
#include <RobotAPI/Test.h>
// ArmarX
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+
#include <RobotAPI/libraries/aron/core/Exception.h>
// Aron
@@ -73,9 +74,10 @@ BOOST_AUTO_TEST_CASE(AronExtendsTest)
BaseClassTest& casted = static_cast<BaseClassTest&>(derived);
BOOST_CHECK_EQUAL(casted.base_class_member1 == derived.base_class_member1, true);
BOOST_CHECK_EQUAL(casted.base_class_member2 == derived.base_class_member2, true);
- BOOST_CHECK_EQUAL(casted.base_class_member3.inner_class_member == derived.base_class_member3.inner_class_member, true);
+ BOOST_CHECK_EQUAL(casted.base_class_member3.inner_class_member ==
+ derived.base_class_member3.inner_class_member,
+ true);
DerivedClassTest& casted_back = dynamic_cast<DerivedClassTest&>(casted);
BOOST_CHECK_EQUAL(casted_back == derived, true);
}
-
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aronJsonExportTest.cpp b/source/RobotAPI/libraries/aron/codegeneration/test/aronJsonExportTest.cpp
index dea56801557a7890824c8f03d5b2cf5b2711d9a0..71d096891bd31964e1c638679945838f7732fd43 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aronJsonExportTest.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aronJsonExportTest.cpp
@@ -25,27 +25,28 @@
#define ARMARX_BOOST_TEST
// STD/STL
-#include <iostream>
#include <cstdlib>
#include <ctime>
-#include <numeric>
#include <fstream>
+#include <iostream>
+#include <numeric>
// Test
#include <RobotAPI/Test.h>
// ArmarX
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+
#include <RobotAPI/libraries/aron/core/Exception.h>
// Aron
-#include <RobotAPI/libraries/aron/core/data/variant/All.h>
-#include <RobotAPI/libraries/aron/core/type/variant/All.h>
-#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
-#include <RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
+#include <RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
+#include <RobotAPI/libraries/aron/core/type/variant/All.h>
// Generated File
#include <RobotAPI/libraries/aron/codegeneration/test/aron/MatrixTest.aron.generated.h>
@@ -83,4 +84,3 @@ BOOST_AUTO_TEST_CASE(AronJsonExportTest)
clazz.read(data_reader, data_json);
}
-
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aronNavigateTest.cpp b/source/RobotAPI/libraries/aron/codegeneration/test/aronNavigateTest.cpp
index a216940735679977b01382ca0c147c0596e1a025..1cbb511635581059179cad61323dfce45cc4a896 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aronNavigateTest.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aronNavigateTest.cpp
@@ -25,17 +25,18 @@
#define ARMARX_BOOST_TEST
// STD/STL
-#include <iostream>
#include <cstdlib>
#include <ctime>
+#include <iostream>
#include <numeric>
// Test
#include <RobotAPI/Test.h>
// ArmarX
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+
#include <RobotAPI/libraries/aron/core/Exception.h>
// Generated File
@@ -63,31 +64,34 @@ BOOST_AUTO_TEST_CASE(AronNavigateTest)
BOOST_CHECK_EQUAL(path.toString(), root);
BOOST_CHECK_EQUAL(path.size(), 0);
- BOOST_CHECK_EQUAL(memberReached.toString(), root+"->reached");
+ BOOST_CHECK_EQUAL(memberReached.toString(), root + "->reached");
BOOST_CHECK_EQUAL(memberReached.size(), 1);
- BOOST_CHECK_EQUAL(memberJointValues.toString(), root+"->jointValues");
+ BOOST_CHECK_EQUAL(memberJointValues.toString(), root + "->jointValues");
BOOST_CHECK_EQUAL(memberJointValues.size(), 1);
- BOOST_CHECK_EQUAL(indexJointValues0.toString(), root+"->jointValues->0");
+ BOOST_CHECK_EQUAL(indexJointValues0.toString(), root + "->jointValues->0");
BOOST_CHECK_EQUAL(indexJointValues0.size(), 2);
- BOOST_CHECK_EQUAL(indexJointValues1.toString(), root+"->jointValues->1");
+ BOOST_CHECK_EQUAL(indexJointValues1.toString(), root + "->jointValues->1");
BOOST_CHECK_EQUAL(indexJointValues1.size(), 2);
data::BoolPtr reached = data::Bool::DynamicCastAndCheck(aron->navigateAbsolute(memberReached));
- data::ListPtr jointValues = data::List::DynamicCastAndCheck(aron->navigateAbsolute(memberJointValues));
+ data::ListPtr jointValues =
+ data::List::DynamicCastAndCheck(aron->navigateAbsolute(memberJointValues));
if (jointValues->childrenSize() > 0)
{
- data::FloatPtr el = data::Float::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues0));
+ data::FloatPtr el =
+ data::Float::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues0));
}
if (jointValues->childrenSize() > 1)
{
- data::FloatPtr el = data::Float::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues1));
+ data::FloatPtr el =
+ data::Float::DynamicCastAndCheck(aron->navigateAbsolute(indexJointValues1));
}
Path diff = indexJointValues1.getWithoutPrefix(indexJointValues0);
- BOOST_CHECK_EQUAL(diff.toString(), root+"->1");
+ BOOST_CHECK_EQUAL(diff.toString(), root + "->1");
BOOST_CHECK_EQUAL(diff.size(), 1);
}
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aronOperatorTest.cpp b/source/RobotAPI/libraries/aron/codegeneration/test/aronOperatorTest.cpp
index 3594ba89ddfc6942694c53b54a3ddfaecf6f4a26..e710ff715e686cd0f2c2653d95ee3dc63616fd12 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aronOperatorTest.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aronOperatorTest.cpp
@@ -25,17 +25,18 @@
#define ARMARX_BOOST_TEST
// STD/STL
-#include <iostream>
#include <cstdlib>
#include <ctime>
+#include <iostream>
#include <numeric>
// Test
#include <RobotAPI/Test.h>
// ArmarX
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+
#include <RobotAPI/libraries/aron/core/Exception.h>
// Aron
@@ -61,12 +62,12 @@ BOOST_AUTO_TEST_CASE(AronAssignmentTest)
aron::data::String sn = s;
aron::data::Bool bn = b;
- BOOST_CHECK_EQUAL((float) fn == f, true);
- BOOST_CHECK_EQUAL((double) dn == d, true);
- BOOST_CHECK_EQUAL((int) in == i, true);
- BOOST_CHECK_EQUAL((long) ln == l, true);
- BOOST_CHECK_EQUAL((std::string) sn == s, true);
- BOOST_CHECK_EQUAL((bool) bn == b, true);
+ BOOST_CHECK_EQUAL((float)fn == f, true);
+ BOOST_CHECK_EQUAL((double)dn == d, true);
+ BOOST_CHECK_EQUAL((int)in == i, true);
+ BOOST_CHECK_EQUAL((long)ln == l, true);
+ BOOST_CHECK_EQUAL((std::string)sn == s, true);
+ BOOST_CHECK_EQUAL((bool)bn == b, true);
}
BOOST_AUTO_TEST_CASE(AronEqualsTest)
@@ -87,4 +88,3 @@ BOOST_AUTO_TEST_CASE(AronEqualsTest)
}
// TODO more tests (fabian.peller)
-
diff --git a/source/RobotAPI/libraries/aron/codegeneration/test/aronRandomizedTest.cpp b/source/RobotAPI/libraries/aron/codegeneration/test/aronRandomizedTest.cpp
index f0dd82776d995a27f9559781e182977358be6e6a..24c0918f79e53ac05f12506f5c3580a01578163c 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/test/aronRandomizedTest.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/test/aronRandomizedTest.cpp
@@ -26,40 +26,36 @@
// Generated File - include them first to check whether their includes are complete.
-#include <RobotAPI/libraries/aron/core/test/aron/ListTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/DictTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/EnumTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/ImageTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/ListTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/MatrixTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/QuaternionTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PointCloudTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/ObjectTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/OptionalTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/OrientationTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PointCloudTest.aron.generated.h>
#include <RobotAPI/libraries/aron/core/test/aron/PoseTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/EnumTest.aron.generated.h>
-#include <RobotAPI/libraries/aron/core/test/aron/OptionalTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PositionTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/PrimitiveTest.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/test/aron/QuaternionTest.aron.generated.h>
// Aron
-#include <RobotAPI/libraries/aron/core/test/Randomizer.h>
-
-#include <RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
-
-#include <RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h>
+#include <RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h>
#include <RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-#include <RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h>
+#include <RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h>
+#include <RobotAPI/libraries/aron/core/test/Randomizer.h>
#include <RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.h>
-
-#include <RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h>
+#include <RobotAPI/libraries/aron/core/type/rw/reader/variant/VariantReader.h>
#include <RobotAPI/libraries/aron/core/type/rw/writer/nlohmannJSON/NlohmannJSONWriter.h>
-
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/libraries/aron/core/type/rw/writer/variant/VariantWriter.h>
// ArmarX
-#include <ArmarXCore/libraries/cppgen/CppMethod.h>
#include <ArmarXCore/libraries/cppgen/CppClass.h>
+#include <ArmarXCore/libraries/cppgen/CppMethod.h>
+
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
@@ -79,9 +75,9 @@
// STD/STL
-#include <iostream>
#include <cstdlib>
#include <ctime>
+#include <iostream>
#include <numeric>
@@ -96,9 +92,9 @@
using namespace armarx;
using namespace aron;
-
template <typename T>
-void test_ToAronType(T& in, T& out)
+void
+test_ToAronType(T& in, T& out)
{
nlohmann::json in_type_json;
nlohmann::json out_type_json;
@@ -131,11 +127,9 @@ void test_ToAronType(T& in, T& out)
BOOST_CHECK_EQUAL(out_type_json.dump(2), in_type_json.dump(2));
}
-
-
-
template <typename T>
-void test_toAron(T& in, T& out)
+void
+test_toAron(T& in, T& out)
{
Randomizer r;
@@ -172,8 +166,13 @@ void test_toAron(T& in, T& out)
BOOST_CHECK(in == out);
aron::data::DictPtr in_aron_nav_again = in.toAron();
- BOOST_TEST_MESSAGE("in_aron_nav: \n" << converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(in_aron_nav).dump(2));
- BOOST_TEST_MESSAGE("in_aron_nav_again: \n" << converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(in_aron_nav_again).dump(2));
+ BOOST_TEST_MESSAGE(
+ "in_aron_nav: \n"
+ << converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(in_aron_nav).dump(2));
+ BOOST_TEST_MESSAGE(
+ "in_aron_nav_again: \n"
+ << converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(in_aron_nav_again)
+ .dump(2));
BOOST_CHECK(*in_aron_nav == *in_aron_nav_again);
}
@@ -237,7 +236,8 @@ void test_toJson(T& in, T& out)
template <typename T>
-void runTestWithInstances(T& in, T& out)
+void
+runTestWithInstances(T& in, T& out)
{
// assumes not nullptrs as in and out. If you have a maybe type then make sure that it is set properly
test_ToAronType(in, out);
@@ -247,7 +247,6 @@ void runTestWithInstances(T& in, T& out)
#endif
}
-
BOOST_AUTO_TEST_CASE(test_List)
{
BOOST_TEST_MESSAGE("Running List test");
@@ -366,7 +365,6 @@ BOOST_AUTO_TEST_CASE(test_Optional)
runTestWithInstances<OptionalTest>(pc, pc2);
}
-
BOOST_AUTO_TEST_CASE(test_Enum)
{
BOOST_TEST_MESSAGE("Running Optional test");
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/Reader.h b/source/RobotAPI/libraries/aron/codegeneration/typereader/Reader.h
index 97f38f8b848543fa44bd1db0cb05db2f16cd8c5e..86c5152cd0f9756e12104d9c821897561a3a5891 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/Reader.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/Reader.h
@@ -24,15 +24,15 @@
#pragma once
// STD/STL
-#include <memory>
#include <filesystem>
+#include <memory>
// ArmarX
-#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/helper/ReaderInfo.h>
#include <RobotAPI/libraries/aron/codegeneration/codegenerator/helper/WriterInfo.h>
-#include <RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateTypeInfo.h>
#include <RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateIntEnumInfo.h>
+#include <RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateTypeInfo.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
namespace armarx::aron::typereader
{
@@ -47,32 +47,45 @@ namespace armarx::aron::typereader
Reader() = default;
/// parse a filename
- virtual void parseFile(const std::string& filename, const std::vector<std::filesystem::path>& includePaths) = 0;
+ virtual void parseFile(const std::string& filename,
+ const std::vector<std::filesystem::path>& includePaths) = 0;
/// path a file given by std::filesystem
- virtual void parseFile(const std::filesystem::path& file, const std::vector<std::filesystem::path>& includePaths) = 0;
+ virtual void parseFile(const std::filesystem::path& file,
+ const std::vector<std::filesystem::path>& includePaths) = 0;
- std::vector<std::string> getCodeIncludes() const
+ std::vector<std::string>
+ getCodeIncludes() const
{
return systemIncludes;
}
- std::vector<std::string> getAronIncludes() const
+
+ std::vector<std::string>
+ getAronIncludes() const
{
return aronIncludes;
}
- std::vector<codegenerator::WriterInfo> getWriters() const
+
+ std::vector<codegenerator::WriterInfo>
+ getWriters() const
{
return writers;
}
- std::vector<codegenerator::ReaderInfo> getReaders() const
+
+ std::vector<codegenerator::ReaderInfo>
+ getReaders() const
{
return readers;
}
- std::vector<typereader::GenerateObjectInfo> getGenerateObjects() const
+
+ std::vector<typereader::GenerateObjectInfo>
+ getGenerateObjects() const
{
return generateObjects;
}
- std::vector<typereader::GenerateIntEnumInfo> getGenerateIntEnums() const
+
+ std::vector<typereader::GenerateIntEnumInfo>
+ getGenerateIntEnums() const
{
return generateIntEnums;
}
@@ -89,4 +102,4 @@ namespace armarx::aron::typereader
std::vector<typereader::GenerateIntEnumInfo> generateIntEnums;
std::vector<typereader::GenerateObjectInfo> generateObjects;
};
-}
+} // namespace armarx::aron::typereader
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateInfo.h b/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateInfo.h
index 1c9e82555da574a0101aa798e0402ddcead455ac..81f41ceb4ba319e152c012f970998b5b3fba76a7 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateInfo.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateInfo.h
@@ -29,6 +29,7 @@
#include <vector>
#include <SimoxUtility/algorithm/string.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::aron::typereader
@@ -41,14 +42,15 @@ namespace armarx::aron::typereader
std::string doc_brief;
std::string doc_author;
-
- std::string getNameWithoutNamespace() const
+ std::string
+ getNameWithoutNamespace() const
{
std::vector<std::string> split = simox::alg::split(typeName, "::");
- return split[split.size() -1];
+ return split[split.size() - 1];
}
- std::vector<std::string> getTemplates() const
+ std::vector<std::string>
+ getTemplates() const
{
auto first = typeName.find("<");
if (first == std::string::npos)
@@ -59,10 +61,11 @@ namespace armarx::aron::typereader
auto last = typeName.find(">");
ARMARX_CHECK_NOT_EQUAL(last, std::string::npos);
- return simox::alg::split(typeName.substr(first,last-first), ",");
+ return simox::alg::split(typeName.substr(first, last - first), ",");
}
- std::vector<std::string> getNamespaces() const
+ std::vector<std::string>
+ getNamespaces() const
{
std::vector<std::string> split = simox::alg::split(typeName, "::");
std::vector<std::string> namespaces(split);
@@ -70,4 +73,4 @@ namespace armarx::aron::typereader
return namespaces;
}
};
-}
+} // namespace armarx::aron::typereader
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateIntEnumInfo.h b/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateIntEnumInfo.h
index e6d92157f9ffc31bf2864bc1cae77927cb9951bf..f29f466f094c03c400667e5334f5d1887ac1b2b6 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateIntEnumInfo.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateIntEnumInfo.h
@@ -24,9 +24,9 @@
#pragma once
// STD/STL
+#include <map>
#include <memory>
#include <string>
-#include <map>
// BaseClass
#include "GenerateInfo.h"
@@ -42,4 +42,4 @@ namespace armarx::aron::typereader
std::map<std::string, std::string> doc_values;
type::IntEnumPtr correspondingType;
};
-}
+} // namespace armarx::aron::typereader
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateTypeInfo.h b/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateTypeInfo.h
index 895a1a59df583f4181a25516d2adf9ce462fab51..ebef523b86d67e7900cc9057350dc5d2929a236f 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateTypeInfo.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateTypeInfo.h
@@ -40,4 +40,4 @@ namespace armarx::aron::typereader
std::map<std::string, std::string> doc_members;
type::ObjectPtr correspondingType;
};
-}
+} // namespace armarx::aron::typereader
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.h b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.h
index 4771cdb4bad5bee3862f098538e7ca2111a2fd85..690fc20f01f605441648da4a67b61c53ac607107 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.h
@@ -24,18 +24,18 @@
#pragma once
// STD/STL
-#include <memory>
#include <map>
-#include <vector>
+#include <memory>
#include <stack>
+#include <vector>
// ArmarX
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateTypeInfo.h>
+
#include <RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateIntEnumInfo.h>
+#include <RobotAPI/libraries/aron/codegeneration/typereader/helper/GenerateTypeInfo.h>
#include <RobotAPI/libraries/aron/core/type/variant/Variant.h>
-
namespace armarx::aron::typereader::xml
{
/**
@@ -50,7 +50,6 @@ namespace armarx::aron::typereader::xml
type::VariantPtr create(const RapidXmlReaderNode&, const Path&);
private:
-
/// check, whether a given name corresponds to an already created object name.
type::VariantPtr findExistingObject(const std::string& n, const Path& path) const;
@@ -99,4 +98,4 @@ namespace armarx::aron::typereader::xml
private:
std::vector<std::string> allPreviouslyKnownPrivateTypes;
};
-}
+} // namespace armarx::aron::typereader::xml
diff --git a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Reader.h b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Reader.h
index 4cc71341c5f5791175c44f9785e7a54c7c699631..ed4b638204d7ef2908c91dcbbd43345c70d85c83 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Reader.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/typereader/xml/Reader.h
@@ -34,38 +34,57 @@
// ArmarX
#include <SimoxUtility/xml.h>
+
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
-#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
#include <RobotAPI/libraries/aron/codegeneration/typereader/xml/Factory.h>
+#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
namespace armarx::aron::typereader::xml
{
/**
* @brief The Reader class. It reads a xml-file and returns a type object and codegeneration information (such as additional includes etc)
*/
- class Reader :
- virtual public typereader::Reader<std::string>
+ class Reader : virtual public typereader::Reader<std::string>
{
public:
Reader() = default;
- void parseFile(const std::string& filename, const std::vector<std::filesystem::path>& includePaths = {}) override;
- void parseFile(const std::filesystem::path& file, const std::vector<std::filesystem::path>& includePaths = {}) override;
+ void parseFile(const std::string& filename,
+ const std::vector<std::filesystem::path>& includePaths = {}) override;
+ void parseFile(const std::filesystem::path& file,
+ const std::vector<std::filesystem::path>& includePaths = {}) override;
private:
- void parse(const RapidXmlReaderPtr& node, const std::filesystem::path& filePath, const std::vector<std::filesystem::path>& includePaths);
+ void parse(const RapidXmlReaderPtr& node,
+ const std::filesystem::path& filePath,
+ const std::vector<std::filesystem::path>& includePaths);
// replacement management
- std::vector<std::pair<std::string, std::string>> getAdditionalIncludesFromReplacements(const RapidXmlReaderNode& node, const std::filesystem::path& filePath);
+ std::vector<std::pair<std::string, std::string>>
+ getAdditionalIncludesFromReplacements(const RapidXmlReaderNode& node,
+ const std::filesystem::path& filePath);
// parse includes
- std::pair<std::string, std::string> readPackagePathInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filename, const std::vector<std::filesystem::path>& includePaths);
- std::pair<std::string, std::string> readInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filename, const std::vector<std::filesystem::path>& includePaths);
- std::pair<std::string, std::string> readSystemInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filename, const std::vector<std::filesystem::path>& includePaths);
+ std::pair<std::string, std::string>
+ readPackagePathInclude(const RapidXmlReaderNode& node,
+ const std::filesystem::path& filename,
+ const std::vector<std::filesystem::path>& includePaths);
+ std::pair<std::string, std::string>
+ readInclude(const RapidXmlReaderNode& node,
+ const std::filesystem::path& filename,
+ const std::vector<std::filesystem::path>& includePaths);
+ std::pair<std::string, std::string>
+ readSystemInclude(const RapidXmlReaderNode& node,
+ const std::filesystem::path& filename,
+ const std::vector<std::filesystem::path>& includePaths);
- std::string readCodeInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filename, const std::vector<std::filesystem::path>& includePaths);
- std::string readAronInclude(const RapidXmlReaderNode& node, const std::filesystem::path& filename, const std::vector<std::filesystem::path>& includePaths);
+ std::string readCodeInclude(const RapidXmlReaderNode& node,
+ const std::filesystem::path& filename,
+ const std::vector<std::filesystem::path>& includePaths);
+ std::string readAronInclude(const RapidXmlReaderNode& node,
+ const std::filesystem::path& filename,
+ const std::vector<std::filesystem::path>& includePaths);
// parse top-level objects
type::ObjectPtr readGenerateObject(const RapidXmlReaderNode& node);
@@ -78,4 +97,4 @@ namespace armarx::aron::typereader::xml
private:
ReaderFactory factory;
};
-}
+} // namespace armarx::aron::typereader::xml
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
index 2604d107087ce329285320e48d66e3ceab97d75d..710d267c25c06cacaf09db0f6effecb9cc337621 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
@@ -1,2 +1 @@
#include "armarx.h"
-
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/core.h b/source/RobotAPI/libraries/aron/common/aron_conversions/core.h
index af62fb4fa63d02203535faf58d1b8fab8053ab72..93c12f84ac56472b1a96b18bbc67872ac6603012 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/core.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/core.h
@@ -9,4 +9,4 @@ namespace armarx
{
-} // namespace armarx
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/stl.h b/source/RobotAPI/libraries/aron/common/aron_conversions/stl.h
index af62fb4fa63d02203535faf58d1b8fab8053ab72..93c12f84ac56472b1a96b18bbc67872ac6603012 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/stl.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/stl.h
@@ -9,4 +9,4 @@ namespace armarx
{
-} // namespace armarx
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/json_conversions/armarx.cpp
index 96a26aa0fbe0930037534dbce431fc715669c299..ad932c64a90ac92ea906fb076d6579e9dc5ac317 100644
--- a/source/RobotAPI/libraries/aron/common/json_conversions/armarx.cpp
+++ b/source/RobotAPI/libraries/aron/common/json_conversions/armarx.cpp
@@ -2,14 +2,15 @@
#include <RobotAPI/libraries/aron/common/aron/Names.aron.generated.h>
-
-void armarx::arondto::to_json(nlohmann::json& j, const Names& bo)
+void
+armarx::arondto::to_json(nlohmann::json& j, const Names& bo)
{
j["recognized"] = bo.recognized;
j["spoken"] = bo.spoken;
}
-void armarx::arondto::from_json(const nlohmann::json& j, Names& bo)
+void
+armarx::arondto::from_json(const nlohmann::json& j, Names& bo)
{
j.at("recognized").get_to(bo.recognized);
j.at("spoken").get_to(bo.spoken);
diff --git a/source/RobotAPI/libraries/aron/common/json_conversions/armarx.h b/source/RobotAPI/libraries/aron/common/json_conversions/armarx.h
index 4da7677a12e5537ae2d41b0cb67362cc112d49c6..2e4b8677c8d84d6a6ba2fe7ec95ad5389c47efd9 100644
--- a/source/RobotAPI/libraries/aron/common/json_conversions/armarx.h
+++ b/source/RobotAPI/libraries/aron/common/json_conversions/armarx.h
@@ -4,11 +4,10 @@
#include <RobotAPI/libraries/aron/common/forward_declarations.h>
-
namespace armarx::arondto
{
void to_json(nlohmann::json& j, const Names& bo);
void from_json(const nlohmann::json& j, Names& bo);
-}
+} // namespace armarx::arondto
diff --git a/source/RobotAPI/libraries/aron/common/rw/eigen.h b/source/RobotAPI/libraries/aron/common/rw/eigen.h
index 848c5e36c950184161fc6c674a5208cfb3825ee9..db1acdd61a8c424831a0599ac99e96ab096409ff 100644
--- a/source/RobotAPI/libraries/aron/common/rw/eigen.h
+++ b/source/RobotAPI/libraries/aron/common/rw/eigen.h
@@ -11,14 +11,13 @@
#include <RobotAPI/libraries/aron/core/data/rw/Writer.h>
#include <RobotAPI/libraries/aron/core/type/rw/Writer.h>
-
namespace armarx
{
// Helper methods for code generation to convert json/aron to bo and vice versa
namespace aron
{
template <class ReaderT, class EigenT, int rows, int cols, int options>
- requires data::isReader<ReaderT>
+ requires data::isReader<ReaderT>
void
read(ReaderT& aron_r,
@@ -32,7 +31,8 @@ namespace armarx
std::stringstream ss;
ss << "Received wrong dimensions for matrix member. Dimensions are (" << shape.at(0)
- << "," << shape.at(1) << ") but should be (" << ret.rows() << "," << ret.cols() << ").";
+ << "," << shape.at(1) << ") but should be (" << ret.rows() << "," << ret.cols()
+ << ").";
ARMARX_CHECK_AND_THROW(
typeAsString == TypeName<EigenT>::Get(),
@@ -48,7 +48,7 @@ namespace armarx
}
template <class ReaderT, class EigenT, int cols, int options>
- requires data::isReader<ReaderT>
+ requires data::isReader<ReaderT>
void
read(ReaderT& aron_r,
@@ -76,7 +76,7 @@ namespace armarx
}
template <class ReaderT, class EigenT, int rows, int options>
- requires data::isReader<ReaderT>
+ requires data::isReader<ReaderT>
void
read(ReaderT& aron_r,
@@ -104,7 +104,7 @@ namespace armarx
}
template <class ReaderT, class EigenT, int options>
- requires data::isReader<ReaderT>
+ requires data::isReader<ReaderT>
void
read(ReaderT& aron_r,
@@ -132,7 +132,7 @@ namespace armarx
}
template <class WriterT, class EigenT, int rows, int cols, int options>
- requires data::isWriter<WriterT>
+ requires data::isWriter<WriterT>
void
write(WriterT& aron_w,
@@ -148,7 +148,7 @@ namespace armarx
}
template <class ReaderT, class EigenT>
- requires data::isReader<ReaderT>
+ requires data::isReader<ReaderT>
void
read(ReaderT& aron_r, typename ReaderT::InputType& input, Eigen::Quaternion<EigenT>& ret)
@@ -162,7 +162,7 @@ namespace armarx
}
template <class WriterT, class EigenT>
- requires data::isWriter<WriterT>
+ requires data::isWriter<WriterT>
void
write(WriterT& aron_w,
diff --git a/source/RobotAPI/libraries/aron/common/rw/framed.cpp b/source/RobotAPI/libraries/aron/common/rw/framed.cpp
index 3f88461dbf51e7634cb25a9f96fbbc6f05a08b95..516620a1391ac99431431b4a0c3125971d3567cb 100644
--- a/source/RobotAPI/libraries/aron/common/rw/framed.cpp
+++ b/source/RobotAPI/libraries/aron/common/rw/framed.cpp
@@ -1,2 +1 @@
#include "framed.h"
-
diff --git a/source/RobotAPI/libraries/aron/common/rw/framed.h b/source/RobotAPI/libraries/aron/common/rw/framed.h
index a0c84c292bd69484209627e369bdd42e4f426f10..7a6ff0fc91564cd8b43c722be8bda5be99ab5a96 100644
--- a/source/RobotAPI/libraries/aron/common/rw/framed.h
+++ b/source/RobotAPI/libraries/aron/common/rw/framed.h
@@ -13,7 +13,7 @@ namespace armarx
namespace aron
{
template <class ReaderT>
- requires data::isReader<ReaderT>
+ requires data::isReader<ReaderT>
void
read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::FramedPosition& ret)
@@ -25,7 +25,7 @@ namespace armarx
}
template <class WriterT>
- requires data::isWriter<WriterT>
+ requires data::isWriter<WriterT>
void
write(WriterT& aron_w,
@@ -39,7 +39,7 @@ namespace armarx
}
template <class ReaderT>
- requires data::isReader<ReaderT>
+ requires data::isReader<ReaderT>
void
read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::FramedOrientation& ret)
@@ -51,7 +51,7 @@ namespace armarx
}
template <class WriterT>
- requires data::isWriter<WriterT>
+ requires data::isWriter<WriterT>
void
write(WriterT& aron_w,
@@ -65,7 +65,7 @@ namespace armarx
}
template <class ReaderT>
- requires data::isReader<ReaderT>
+ requires data::isReader<ReaderT>
void
read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::FramedPose& ret)
@@ -77,7 +77,7 @@ namespace armarx
}
template <class WriterT>
- requires data::isWriter<WriterT>
+ requires data::isWriter<WriterT>
void
write(WriterT& aron_w,
diff --git a/source/RobotAPI/libraries/aron/common/rw/time.h b/source/RobotAPI/libraries/aron/common/rw/time.h
index c31b81991444b957b173066b27634456d0452529..0e966724581ef25f4e56f2e30553f9b170ba16e0 100644
--- a/source/RobotAPI/libraries/aron/common/rw/time.h
+++ b/source/RobotAPI/libraries/aron/common/rw/time.h
@@ -13,7 +13,7 @@ namespace armarx
namespace aron
{
template <class ReaderT>
- requires data::isReader<ReaderT>
+ requires data::isReader<ReaderT>
void
read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::core::time::DateTime& ret)
@@ -25,7 +25,7 @@ namespace armarx
}
template <class WriterT>
- requires data::isWriter<WriterT>
+ requires data::isWriter<WriterT>
void
write(WriterT& aron_w,
@@ -39,7 +39,7 @@ namespace armarx
}
template <class ReaderT>
- requires data::isReader<ReaderT>
+ requires data::isReader<ReaderT>
void
read(ReaderT& aron_r, typename ReaderT::InputType& input, armarx::core::time::Duration& ret)
@@ -51,7 +51,7 @@ namespace armarx
}
template <class WriterT>
- requires data::isWriter<WriterT>
+ requires data::isWriter<WriterT>
void
write(WriterT& aron_w,
diff --git a/source/RobotAPI/libraries/aron/common/test/MyCustomType.cpp b/source/RobotAPI/libraries/aron/common/test/MyCustomType.cpp
index 3d4ba61b8466b65d12334533d25720fca40ce1b8..8b4893b5f7a0348d4075c654b35ca5c1777cc99d 100644
--- a/source/RobotAPI/libraries/aron/common/test/MyCustomType.cpp
+++ b/source/RobotAPI/libraries/aron/common/test/MyCustomType.cpp
@@ -25,41 +25,42 @@
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
-
template <class CustomTypeT>
-std::ostream& ostreamop(std::ostream& os, const CustomTypeT& rhs)
+std::ostream&
+ostreamop(std::ostream& os, const CustomTypeT& rhs)
{
return os << "(name='" << rhs.name << "'"
- << " index=" << rhs.index
- << " value=" << rhs.value
- << ")";
+ << " index=" << rhs.index << " value=" << rhs.value << ")";
}
-std::ostream& my::operator<<(std::ostream& os, const CustomType& rhs)
+std::ostream&
+my::operator<<(std::ostream& os, const CustomType& rhs)
{
return ostreamop(os, rhs);
}
-std::ostream& my::arondto::operator<<(std::ostream& os, const CustomType& rhs)
+std::ostream&
+my::arondto::operator<<(std::ostream& os, const CustomType& rhs)
{
return ostreamop(os, rhs);
}
-bool my::operator==(const CustomType& lhs, const arondto::CustomType& rhs)
+bool
+my::operator==(const CustomType& lhs, const arondto::CustomType& rhs)
{
- return lhs.name == rhs.name
- && lhs.index == rhs.index
- && lhs.value == rhs.value;
+ return lhs.name == rhs.name && lhs.index == rhs.index && lhs.value == rhs.value;
}
-
-void my::toAron(arondto::CustomType& dto, const CustomType& bo)
+void
+my::toAron(arondto::CustomType& dto, const CustomType& bo)
{
dto.name = bo.name;
armarx::aron::toAron(dto.index, bo.index);
armarx::aron::toAron(dto.value, bo.value);
}
-void my::fromAron(const arondto::CustomType& dto, CustomType& bo)
+
+void
+my::fromAron(const arondto::CustomType& dto, CustomType& bo)
{
bo.name = dto.name;
armarx::aron::fromAron(dto.index, bo.index);
diff --git a/source/RobotAPI/libraries/aron/common/test/MyCustomType.h b/source/RobotAPI/libraries/aron/common/test/MyCustomType.h
index f266c7a371070495b8d1d8d3331efc66e95b5d89..3b826c56f2f2741f6e99f0c8eb6d7259c4a86340 100644
--- a/source/RobotAPI/libraries/aron/common/test/MyCustomType.h
+++ b/source/RobotAPI/libraries/aron/common/test/MyCustomType.h
@@ -37,7 +37,6 @@ namespace my
std::ostream& operator<<(std::ostream& os, const CustomType& rhs);
-
namespace arondto
{
class CustomType : public armarx::aron::cpp::AronGeneratedClass
@@ -48,50 +47,59 @@ namespace my
float value;
CustomType() = default;
- CustomType(const std::string& n, int i, float f) : name(n), index(i), value(f) {}
+
+ CustomType(const std::string& n, int i, float f) : name(n), index(i), value(f)
+ {
+ }
};
std::ostream& operator<<(std::ostream& os, const arondto::CustomType& rhs);
- }
+ } // namespace arondto
bool operator==(const CustomType& lhs, const arondto::CustomType& rhs);
- bool operator!=(const CustomType& lhs, const arondto::CustomType& rhs)
+
+ bool
+ operator!=(const CustomType& lhs, const arondto::CustomType& rhs)
{
return !(lhs == rhs);
}
- bool operator==(const arondto::CustomType& lhs, const CustomType& rhs)
+
+ bool
+ operator==(const arondto::CustomType& lhs, const CustomType& rhs)
{
return rhs == lhs;
}
- bool operator!=(const arondto::CustomType& lhs, const CustomType& rhs)
+
+ bool
+ operator!=(const arondto::CustomType& lhs, const CustomType& rhs)
{
return !(rhs == lhs);
}
void toAron(arondto::CustomType& dto, const CustomType& bo);
void fromAron(const arondto::CustomType& dto, CustomType& bo);
-}
-
+} // namespace my
namespace std
{
template <class L1, class L2, class R1, class R2>
- bool operator==(const std::pair<L1, L2>& lhs,
- const std::pair<R1, R2>& rhs)
+ bool
+ operator==(const std::pair<L1, L2>& lhs, const std::pair<R1, R2>& rhs)
{
return lhs.first == rhs.first && lhs.second == rhs.second;
}
+
template <class L1, class L2, class R1, class R2>
- bool operator!=(const std::pair<L1, L2>& lhs,
- const std::pair<R1, R2>& rhs)
+ bool
+ operator!=(const std::pair<L1, L2>& lhs, const std::pair<R1, R2>& rhs)
{
return !(lhs == rhs);
}
-
template <class L, class R>
- bool operator==(const std::vector<L>& lhs, const std::vector<R>& rhs)
+ bool
+ operator==(const std::vector<L>& lhs, const std::vector<R>& rhs)
{
if (lhs.size() != rhs.size())
{
@@ -106,11 +114,12 @@ namespace std
}
return true;
}
+
template <class L, class R>
- bool operator!=(const std::vector<L>& lhs, const std::vector<R>& rhs)
+ bool
+ operator!=(const std::vector<L>& lhs, const std::vector<R>& rhs)
{
return !(lhs == rhs);
}
-}
-
+} // namespace std
diff --git a/source/RobotAPI/libraries/aron/common/test/aron_common_test.cpp b/source/RobotAPI/libraries/aron/common/test/aron_common_test.cpp
index 6c66ae28fbbc8d498d937376a6a638f63c6bee5b..8d8a4fe96535323f772de1d930b7474bdb1ddfe8 100644
--- a/source/RobotAPI/libraries/aron/common/test/aron_common_test.cpp
+++ b/source/RobotAPI/libraries/aron/common/test/aron_common_test.cpp
@@ -26,16 +26,15 @@
#include <RobotAPI/Test.h>
-#include <RobotAPI/libraries/aron/core/aron_conversions.h>
-
#include <iostream>
-#include "MyCustomType.h"
+#include <RobotAPI/libraries/aron/core/aron_conversions.h>
+#include "MyCustomType.h"
BOOST_AUTO_TEST_CASE(test_direct)
{
- const my::CustomType bo { "name", -10, 42.42f };
+ const my::CustomType bo{"name", -10, 42.42f};
my::arondto::CustomType dto;
toAron(dto, bo);
@@ -46,75 +45,64 @@ BOOST_AUTO_TEST_CASE(test_direct)
BOOST_CHECK_EQUAL(boOut, dto);
}
-
template <class BOs, class DTOs>
-void test_complex(const BOs bos, DTOs& dtos)
+void
+test_complex(const BOs bos, DTOs& dtos)
{
armarx::aron::toAron(dtos, bos);
- BOOST_CHECK_EQUAL_COLLECTIONS(dtos.begin(), dtos.end(),
- bos.begin(), bos.end());
+ BOOST_CHECK_EQUAL_COLLECTIONS(dtos.begin(), dtos.end(), bos.begin(), bos.end());
BOs bosOut;
armarx::aron::fromAron(dtos, bosOut);
- BOOST_CHECK_EQUAL_COLLECTIONS(bosOut.begin(), bosOut.end(),
- dtos.begin(), dtos.end());
+ BOOST_CHECK_EQUAL_COLLECTIONS(bosOut.begin(), bosOut.end(), dtos.begin(), dtos.end());
}
-
BOOST_AUTO_TEST_CASE(test_stl_vector)
{
- const std::vector<my::CustomType> bos
- {
- { "name", -10, 42.42f },
- { "name2", 20, -128.128f },
+ const std::vector<my::CustomType> bos{
+ {"name", -10, 42.42f},
+ {"name2", 20, -128.128f},
};
std::vector<my::arondto::CustomType> dtos;
test_complex(bos, dtos);
}
-
BOOST_AUTO_TEST_CASE(test_stl_map)
{
- const std::map<std::string, my::CustomType> bos
- {
- { "key1", { "name", -10, 42.42f } },
- { "key2", { "name2", 20, -128.128f } },
+ const std::map<std::string, my::CustomType> bos{
+ {"key1", {"name", -10, 42.42f}},
+ {"key2", {"name2", 20, -128.128f}},
};
std::map<std::string, my::arondto::CustomType> dtos;
test_complex(bos, dtos);
}
-
BOOST_AUTO_TEST_CASE(test_stl_vector_of_vector)
{
- const std::vector<std::vector<my::CustomType>> bos
- {
- { { "name", -10, 42.42f } },
- { },
- { { "name2", 20, -128.128f }, { "name2.1", 40, -64.64f } },
+ const std::vector<std::vector<my::CustomType>> bos{
+ {{"name", -10, 42.42f}},
+ {},
+ {{"name2", 20, -128.128f}, {"name2.1", 40, -64.64f}},
};
std::vector<std::vector<my::arondto::CustomType>> dtos;
test_complex(bos, dtos);
}
-
BOOST_AUTO_TEST_CASE(test_stl_map_of_vector)
{
- const std::map<std::string, std::vector<my::CustomType>> bos
- {
- { "key1", { { "name", -10, 42.42f } } },
- { "key2", { { "name2", 20, -128.128f }, { "name2.1", 40, -64.64f } } },
- { "key3", { } },
+ const std::map<std::string, std::vector<my::CustomType>> bos{
+ {"key1", {{"name", -10, 42.42f}}},
+ {"key2", {{"name2", 20, -128.128f}, {"name2.1", 40, -64.64f}}},
+ {"key3", {}},
};
std::map<std::string, std::vector<my::arondto::CustomType>> dtos;
test_complex(bos, dtos);
}
-
BOOST_AUTO_TEST_CASE(test_optional)
{
using std::nullopt;
@@ -122,9 +110,8 @@ BOOST_AUTO_TEST_CASE(test_optional)
std::optional<my::CustomType> bo;
std::optional<my::arondto::CustomType> dto;
- auto reset = [&](
- std::optional<my::CustomType> theBo,
- std::optional<my::arondto::CustomType> theDto)
+ auto reset =
+ [&](std::optional<my::CustomType> theBo, std::optional<my::arondto::CustomType> theDto)
{
bo = theBo;
dto = theDto;
@@ -141,38 +128,38 @@ BOOST_AUTO_TEST_CASE(test_optional)
BOOST_CHECK(!bo.has_value());
}
- reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ reset(my::CustomType{"bo", 30, 16.16f}, nullopt);
{
armarx::aron::toAron(dto, bo);
BOOST_CHECK(dto.has_value());
BOOST_CHECK_EQUAL(dto.value(), bo.value());
}
- reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ reset(my::CustomType{"bo", 30, 16.16f}, nullopt);
{
armarx::aron::fromAron(dto, bo);
BOOST_CHECK(!bo.has_value());
}
- reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ reset(nullopt, my::arondto::CustomType{"dto", 30, 16.16f});
{
armarx::aron::toAron(dto, bo);
BOOST_CHECK(!dto.has_value());
}
- reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ reset(nullopt, my::arondto::CustomType{"dto", 30, 16.16f});
{
armarx::aron::fromAron(dto, bo);
BOOST_CHECK(bo.has_value());
BOOST_CHECK_EQUAL(bo.value(), dto.value());
}
- reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ reset(my::CustomType{"bo", -30, -16.16f}, my::arondto::CustomType{"dto", 30, 16.16f});
{
armarx::aron::toAron(dto, bo);
BOOST_CHECK(dto.has_value());
BOOST_CHECK_EQUAL(dto.value(), bo.value());
BOOST_CHECK_EQUAL(dto->name, "bo");
}
- reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ reset(my::CustomType{"bo", -30, -16.16f}, my::arondto::CustomType{"dto", 30, 16.16f});
{
armarx::aron::fromAron(dto, bo);
BOOST_CHECK(bo.has_value());
@@ -181,8 +168,6 @@ BOOST_AUTO_TEST_CASE(test_optional)
}
}
-
-
BOOST_AUTO_TEST_CASE(test_optional_value_flagged)
{
using std::nullopt;
@@ -191,9 +176,8 @@ BOOST_AUTO_TEST_CASE(test_optional_value_flagged)
my::arondto::CustomType dto;
bool dtoValid;
- auto reset = [&](
- std::optional<my::CustomType> theBo,
- std::optional<my::arondto::CustomType> theDto)
+ auto reset =
+ [&](std::optional<my::CustomType> theBo, std::optional<my::arondto::CustomType> theDto)
{
bo = theBo;
if (theDto)
@@ -219,38 +203,38 @@ BOOST_AUTO_TEST_CASE(test_optional_value_flagged)
BOOST_CHECK(!bo.has_value());
}
- reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ reset(my::CustomType{"bo", 30, 16.16f}, nullopt);
{
armarx::aron::toAron(dto, dtoValid, bo);
BOOST_CHECK(dtoValid);
BOOST_CHECK_EQUAL(dto, bo.value());
}
- reset(my::CustomType{ "bo", 30, 16.16f }, nullopt);
+ reset(my::CustomType{"bo", 30, 16.16f}, nullopt);
{
armarx::aron::fromAron(dto, dtoValid, bo);
BOOST_CHECK(!bo.has_value());
}
- reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ reset(nullopt, my::arondto::CustomType{"dto", 30, 16.16f});
{
armarx::aron::toAron(dto, dtoValid, bo);
BOOST_CHECK(!dtoValid);
}
- reset(nullopt, my::arondto::CustomType{ "dto", 30, 16.16f });
+ reset(nullopt, my::arondto::CustomType{"dto", 30, 16.16f});
{
armarx::aron::fromAron(dto, dtoValid, bo);
BOOST_CHECK(bo.has_value());
BOOST_CHECK_EQUAL(bo.value(), dto);
}
- reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ reset(my::CustomType{"bo", -30, -16.16f}, my::arondto::CustomType{"dto", 30, 16.16f});
{
armarx::aron::toAron(dto, dtoValid, bo);
BOOST_CHECK(dtoValid);
BOOST_CHECK_EQUAL(dto, bo.value());
BOOST_CHECK_EQUAL(dto.name, "bo");
}
- reset(my::CustomType{ "bo", -30, -16.16f }, my::arondto::CustomType{ "dto", 30, 16.16f });
+ reset(my::CustomType{"bo", -30, -16.16f}, my::arondto::CustomType{"dto", 30, 16.16f});
{
armarx::aron::fromAron(dto, dtoValid, bo);
BOOST_CHECK(bo.has_value());
diff --git a/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h b/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h
index a43b018193a146e1bba88d1228d20f3eb584ce23..77c2f054e154308d359eb05c18d075a75d527e91 100644
--- a/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/opencv/OpenCVConverter.h
@@ -24,9 +24,8 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/interface/aron.h>
-
+#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
namespace armarx::aron::data::converter
diff --git a/source/RobotAPI/libraries/aron/core/Descriptor.h b/source/RobotAPI/libraries/aron/core/Descriptor.h
index e2228b3b547c34a8881d8203d7f2db1f7eefb870..132f20af641d85e12390328c276e60fc418e2ec9 100644
--- a/source/RobotAPI/libraries/aron/core/Descriptor.h
+++ b/source/RobotAPI/libraries/aron/core/Descriptor.h
@@ -23,19 +23,20 @@
#pragma once
-#include <typeinfo>
#include <map>
#include <string>
+#include <typeinfo>
#include <RobotAPI/interface/aron.h>
namespace armarx::aron::conversion::util
{
template <class T1, class T2>
- std::map<T2, T1> InvertMap(const std::map<T1, T2>& m)
+ std::map<T2, T1>
+ InvertMap(const std::map<T1, T2>& m)
{
std::map<T2, T1> ret;
- for (const auto &[key, val] : m)
+ for (const auto& [key, val] : m)
{
ret.emplace(val, key);
}
@@ -43,28 +44,27 @@ namespace armarx::aron::conversion::util
}
template <class T>
- std::map<T, std::string> InvertMap(const std::map<std::string, T>& m)
+ std::map<T, std::string>
+ InvertMap(const std::map<std::string, T>& m)
{
return InvertMap<std::string, T>(m);
}
template <class T>
- std::map<std::string, T> InvertMap(const std::map<T, std::string>& m)
+ std::map<std::string, T>
+ InvertMap(const std::map<T, std::string>& m)
{
return InvertMap<T, std::string>(m);
}
-}
+} // namespace armarx::aron::conversion::util
namespace armarx::aron::type
{
- const std::vector<type::Maybe> AllMaybeTypes =
- {
- type::Maybe::NONE,
- type::Maybe::OPTIONAL,
- type::Maybe::RAW_PTR,
- type::Maybe::SHARED_PTR,
- type::Maybe::UNIQUE_PTR
- };
+ const std::vector<type::Maybe> AllMaybeTypes = {type::Maybe::NONE,
+ type::Maybe::OPTIONAL,
+ type::Maybe::RAW_PTR,
+ type::Maybe::SHARED_PTR,
+ type::Maybe::UNIQUE_PTR};
enum class Descriptor
{
@@ -89,44 +89,38 @@ namespace armarx::aron::type
UNKNOWN = -1
};
- const std::vector<type::Descriptor> AllDescriptors =
- {
- Descriptor::LIST,
- Descriptor::OBJECT,
- Descriptor::TUPLE,
- Descriptor::PAIR,
- Descriptor::DICT,
- Descriptor::NDARRAY,
- Descriptor::MATRIX,
- Descriptor::QUATERNION,
- Descriptor::POINTCLOUD,
- Descriptor::IMAGE,
- Descriptor::INT_ENUM,
- Descriptor::INT,
- Descriptor::LONG,
- Descriptor::FLOAT,
- Descriptor::DOUBLE,
- Descriptor::BOOL,
- Descriptor::STRING,
- Descriptor::ANY_OBJECT,
- Descriptor::UNKNOWN
- };
+ const std::vector<type::Descriptor> AllDescriptors = {Descriptor::LIST,
+ Descriptor::OBJECT,
+ Descriptor::TUPLE,
+ Descriptor::PAIR,
+ Descriptor::DICT,
+ Descriptor::NDARRAY,
+ Descriptor::MATRIX,
+ Descriptor::QUATERNION,
+ Descriptor::POINTCLOUD,
+ Descriptor::IMAGE,
+ Descriptor::INT_ENUM,
+ Descriptor::INT,
+ Descriptor::LONG,
+ Descriptor::FLOAT,
+ Descriptor::DOUBLE,
+ Descriptor::BOOL,
+ Descriptor::STRING,
+ Descriptor::ANY_OBJECT,
+ Descriptor::UNKNOWN};
namespace defaultconversion::string
{
// Maybe
- const std::map<type::Maybe, std::string> Maybe2String =
- {
+ const std::map<type::Maybe, std::string> Maybe2String = {
{Maybe::NONE, "armarx::aron::type::Maybe::NONE"},
{Maybe::RAW_PTR, "armarx::aron::type::Maybe::RAW_PTR"},
{Maybe::SHARED_PTR, "armarx::aron::type::Maybe::SHARED_PTR"},
{Maybe::UNIQUE_PTR, "armarx::aron::type::Maybe::UNIQUE_PTR"},
- {Maybe::OPTIONAL, "armarx::aron::type::Maybe::OPTIONAL"}
- };
+ {Maybe::OPTIONAL, "armarx::aron::type::Maybe::OPTIONAL"}};
// Descriptor
- const std::map<type::Descriptor, std::string> Descriptor2String =
- {
+ const std::map<type::Descriptor, std::string> Descriptor2String = {
{Descriptor::LIST, "armarx::aron::type::Descriptor::LIST"},
{Descriptor::OBJECT, "armarx::aron::type::Descriptor::OBJECT"},
{Descriptor::DICT, "armarx::aron::type::Descriptor::DICT"},
@@ -146,9 +140,8 @@ namespace armarx::aron::type
{Descriptor::BOOL, "armarx::aron::type::Descriptor::BOOL"},
{Descriptor::STRING, "armarx::aron::type::Descriptor::STRING"},
{Descriptor::ANY_OBJECT, "armarx::aron::type::Descriptor::ANY_OBJECT"},
- {Descriptor::UNKNOWN, "armarx::aron::type::Descriptor::UNKNOWN"}
- };
- }
+ {Descriptor::UNKNOWN, "armarx::aron::type::Descriptor::UNKNOWN"}};
+ } // namespace defaultconversion::string
namespace defaultconversion::typeinfo
{
@@ -171,15 +164,15 @@ namespace armarx::aron::type
{typeid(aron::type::dto::AronDouble).hash_code(), Descriptor::DOUBLE},
{typeid(aron::type::dto::AronString).hash_code(), Descriptor::STRING},
{typeid(aron::type::dto::AronBool).hash_code(), Descriptor::BOOL},
- {typeid(aron::type::dto::AnyObject).hash_code(), Descriptor::ANY_OBJECT}
- };
- }
+ {typeid(aron::type::dto::AnyObject).hash_code(), Descriptor::ANY_OBJECT}};
+ } // namespace defaultconversion::typeinfo
- inline type::Descriptor Aron2Descriptor(const type::dto::GenericType& t)
+ inline type::Descriptor
+ Aron2Descriptor(const type::dto::GenericType& t)
{
return defaultconversion::typeinfo::TypeId2Descriptor.at(typeid(t).hash_code());
}
-}
+} // namespace armarx::aron::type
namespace armarx::aron::data
{
@@ -197,25 +190,21 @@ namespace armarx::aron::data
UNKNOWN = -1
};
- const std::vector<data::Descriptor> AllDescriptors =
- {
- Descriptor::LIST,
- Descriptor::DICT,
- Descriptor::NDARRAY,
- Descriptor::INT,
- Descriptor::LONG,
- Descriptor::FLOAT,
- Descriptor::DOUBLE,
- Descriptor::STRING,
- Descriptor::BOOL,
- Descriptor::UNKNOWN
- };
+ const std::vector<data::Descriptor> AllDescriptors = {Descriptor::LIST,
+ Descriptor::DICT,
+ Descriptor::NDARRAY,
+ Descriptor::INT,
+ Descriptor::LONG,
+ Descriptor::FLOAT,
+ Descriptor::DOUBLE,
+ Descriptor::STRING,
+ Descriptor::BOOL,
+ Descriptor::UNKNOWN};
namespace defaultconversion::string
{
// Descriptor
- const std::map<data::Descriptor, std::string> Descriptor2String =
- {
+ const std::map<data::Descriptor, std::string> Descriptor2String = {
{Descriptor::LIST, "armarx::aron::data::Descriptor::LIST"},
{Descriptor::DICT, "armarx::aron::data::Descriptor::DICT"},
{Descriptor::NDARRAY, "armarx::aron::data::Descriptor::NDARRAY"},
@@ -225,9 +214,8 @@ namespace armarx::aron::data
{Descriptor::DOUBLE, "armarx::aron::data::Descriptor::DOUBLE"},
{Descriptor::STRING, "armarx::aron::data::Descriptor::STRING"},
{Descriptor::BOOL, "armarx::aron::data::Descriptor::BOOL"},
- {Descriptor::UNKNOWN, "armarx::aron::data::Descriptor::UNKNOWN"}
- };
- }
+ {Descriptor::UNKNOWN, "armarx::aron::data::Descriptor::UNKNOWN"}};
+ } // namespace defaultconversion::string
namespace defaultconversion::typeinfo
{
@@ -243,9 +231,10 @@ namespace armarx::aron::data
{typeid(aron::data::dto::AronString).hash_code(), Descriptor::STRING},
{typeid(aron::data::dto::AronBool).hash_code(), Descriptor::BOOL},
};
- }
+ } // namespace defaultconversion::typeinfo
- inline data::Descriptor Aron2Descriptor(const data::dto::GenericData& t)
+ inline data::Descriptor
+ Aron2Descriptor(const data::dto::GenericData& t)
{
return defaultconversion::typeinfo::TypeId2Descriptor.at(typeid(t).hash_code());
}
@@ -263,8 +252,7 @@ namespace armarx::aron::data
{Descriptor::DOUBLE, aron::type::Descriptor::DOUBLE},
{Descriptor::STRING, aron::type::Descriptor::STRING},
{Descriptor::BOOL, aron::type::Descriptor::BOOL},
- {Descriptor::UNKNOWN, aron::type::Descriptor::UNKNOWN}
- };
+ {Descriptor::UNKNOWN, aron::type::Descriptor::UNKNOWN}};
const std::map<aron::type::Descriptor, Descriptor> Type2DataDescriptor = {
// containers
@@ -291,7 +279,7 @@ namespace armarx::aron::data
{aron::type::Descriptor::DOUBLE, Descriptor::DOUBLE},
{aron::type::Descriptor::STRING, Descriptor::STRING},
{aron::type::Descriptor::BOOL, Descriptor::BOOL},
- {aron::type::Descriptor::UNKNOWN, Descriptor::UNKNOWN},
+ {aron::type::Descriptor::UNKNOWN, Descriptor::UNKNOWN},
};
- }
-}
+ } // namespace defaultconversion
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/Exception.h b/source/RobotAPI/libraries/aron/core/Exception.h
index ef0d2fa2e32567ad3d2fcc56768aa4bdf1c71098..1234ca71cbea260e6b7fcc41b93e5f6e823d7724 100644
--- a/source/RobotAPI/libraries/aron/core/Exception.h
+++ b/source/RobotAPI/libraries/aron/core/Exception.h
@@ -24,6 +24,7 @@
#pragma once
#include <ArmarXCore/core/exceptions/LocalException.h>
+
#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/libraries/aron/core/Path.h>
diff --git a/source/RobotAPI/libraries/aron/core/Path.cpp b/source/RobotAPI/libraries/aron/core/Path.cpp
index ac91613ddb4dfc1dcae0f4fd09571783bd72d625..2d91de4d6531fcbde7fca0ee45f95d0ad6769772 100644
--- a/source/RobotAPI/libraries/aron/core/Path.cpp
+++ b/source/RobotAPI/libraries/aron/core/Path.cpp
@@ -23,6 +23,7 @@
// Header
#include "Path.h"
+
#include <SimoxUtility/algorithm/string/string_tools.h>
// ArmarX
diff --git a/source/RobotAPI/libraries/aron/core/aron_conversions.h b/source/RobotAPI/libraries/aron/core/aron_conversions.h
index cbd97326e69b9885b5b0fd31d9dde372dc266ecb..b85a1969105ef91d651775372885e3a6230e1a28 100644
--- a/source/RobotAPI/libraries/aron/core/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/core/aron_conversions.h
@@ -1,29 +1,33 @@
#pragma once
+#include <algorithm>
#include <map>
#include <memory>
#include <optional>
#include <type_traits>
#include <vector>
-#include <algorithm>
namespace armarx::aron
{
-namespace detail
-{
+ namespace detail
+ {
- // Helper concept to avoid ambiguities
- template<typename DtoT, typename BoT>
- concept DtoAndBoAreSame = std::is_same<DtoT, BoT>::value;
+ // Helper concept to avoid ambiguities
+ template <typename DtoT, typename BoT>
+ concept DtoAndBoAreSame = std::is_same<DtoT, BoT>::value;
- template <class ...>
- struct is_optional : public std::false_type {};
+ template <class...>
+ struct is_optional : public std::false_type
+ {
+ };
- template <class ...Ts>
- struct is_optional<std::optional<Ts...>> : public std::true_type {};
+ template <class... Ts>
+ struct is_optional<std::optional<Ts...>> : public std::true_type
+ {
+ };
-}
+ } // namespace detail
/**
* Framework for converting ARON DTOs (Data Transfer Objects) to C++ BOs
@@ -69,42 +73,42 @@ namespace detail
*/
// Same type
template <class T>
- void toAron(T& dto, const T& bo)
+ void
+ toAron(T& dto, const T& bo)
{
dto = bo;
}
+
template <class T>
- void fromAron(const T& dto, T& bo)
+ void
+ fromAron(const T& dto, T& bo)
{
bo = dto;
}
-
// Generic return version
template <class DtoT, class BoT>
- DtoT toAron(const BoT& bo)
+ DtoT
+ toAron(const BoT& bo)
{
DtoT dto;
toAron(dto, bo);
return dto;
}
+
template <class BoT, class DtoT>
- BoT fromAron(const DtoT& dto)
+ BoT
+ fromAron(const DtoT& dto)
{
BoT bo;
fromAron(dto, bo);
return bo;
}
-
-
-
-
-
-
// std::unique_ptr
template <class DtoT, class BoT>
- void toAron(DtoT& dto, const std::unique_ptr<BoT>& bo)
+ void
+ toAron(DtoT& dto, const std::unique_ptr<BoT>& bo)
{
if (bo)
{
@@ -113,17 +117,18 @@ namespace detail
}
template <class DtoT, class BoT>
- void fromAron(const DtoT& dto, std::unique_ptr<BoT>& bo)
+ void
+ fromAron(const DtoT& dto, std::unique_ptr<BoT>& bo)
{
bo = std::make_unique<BoT>();
fromAron(dto, *bo);
}
-
// std::optional
template <class DtoT, class BoT>
- requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
- void toAron(std::optional<DtoT>& dto, const std::optional<BoT>& bo)
+ requires(!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
+ void
+ toAron(std::optional<DtoT>& dto, const std::optional<BoT>& bo)
{
if (bo.has_value())
{
@@ -137,8 +142,9 @@ namespace detail
}
template <class DtoT, class BoT>
- requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
- void fromAron(const std::optional<DtoT>& dto, std::optional<BoT>& bo)
+ requires(!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
+ void
+ fromAron(const std::optional<DtoT>& dto, std::optional<BoT>& bo)
{
if (dto.has_value())
{
@@ -151,27 +157,29 @@ namespace detail
}
}
-
// One-sided optional
template <class DtoT, class BoT>
- requires (not detail::is_optional<BoT>::value)
- void toAron(std::optional<DtoT>& dto, const BoT& bo)
+ requires(not detail::is_optional<BoT>::value)
+ void
+ toAron(std::optional<DtoT>& dto, const BoT& bo)
{
dto = DtoT{};
toAron(*dto, bo);
}
+
template <class DtoT, class BoT>
- requires (not detail::is_optional<DtoT>::value)
- void fromAron(DtoT& dto, const std::optional<BoT>& bo)
+ requires(not detail::is_optional<DtoT>::value)
+ void
+ fromAron(DtoT& dto, const std::optional<BoT>& bo)
{
bo = BoT{};
fromAron(dto, *bo);
}
-
// Flag-controlled optional
template <class DtoT, class BoT>
- void toAron(DtoT& dto, bool& dtoValid, const BoT& bo, bool boValid)
+ void
+ toAron(DtoT& dto, bool& dtoValid, const BoT& bo, bool boValid)
{
dtoValid = boValid;
if (boValid)
@@ -184,9 +192,9 @@ namespace detail
}
}
-
template <class DtoT, class BoT>
- void fromAron(const DtoT& dto, bool dtoValid, BoT& bo, bool& boValid)
+ void
+ fromAron(const DtoT& dto, bool dtoValid, BoT& bo, bool& boValid)
{
boValid = dtoValid;
if (dtoValid)
@@ -200,7 +208,8 @@ namespace detail
}
template <class DtoT, class BoT>
- void toAron(DtoT& dto, bool& dtoValid, const std::optional<BoT>& bo)
+ void
+ toAron(DtoT& dto, bool& dtoValid, const std::optional<BoT>& bo)
{
dtoValid = bo.has_value();
if (dtoValid)
@@ -214,7 +223,8 @@ namespace detail
}
template <class DtoT, class BoT>
- void fromAron(const DtoT& dto, bool dtoValid, std::optional<BoT>& bo)
+ void
+ fromAron(const DtoT& dto, bool dtoValid, std::optional<BoT>& bo)
{
if (dtoValid)
{
@@ -227,11 +237,11 @@ namespace detail
}
}
-
// std::vector
template <class DtoT, class BoT>
- requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
- void toAron(std::vector<DtoT>& dtos, const std::vector<BoT>& bos)
+ requires(!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
+ void
+ toAron(std::vector<DtoT>& dtos, const std::vector<BoT>& bos)
{
dtos.clear();
dtos.reserve(bos.size());
@@ -240,9 +250,11 @@ namespace detail
toAron(dtos.emplace_back(), bo);
}
}
+
template <class DtoT, class BoT>
- requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
- void fromAron(const std::vector<DtoT>& dtos, std::vector<BoT>& bos)
+ requires(!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
+ void
+ fromAron(const std::vector<DtoT>& dtos, std::vector<BoT>& bos)
{
bos.clear();
bos.reserve(dtos.size());
@@ -252,12 +264,12 @@ namespace detail
}
}
-
// std::map
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
- void toAron(std::map<DtoKeyT, DtoValueT>& dtoMap,
- const std::map<BoKeyT, BoValueT>& boMap)
+ requires(!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and
+ detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ void
+ toAron(std::map<DtoKeyT, DtoValueT>& dtoMap, const std::map<BoKeyT, BoValueT>& boMap)
{
dtoMap.clear();
for (const auto& [boKey, boValue] : boMap)
@@ -268,10 +280,12 @@ namespace detail
toAron(it->second, boValue);
}
}
+
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
- void fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap,
- std::map<BoKeyT, BoValueT>& boMap)
+ requires(!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and
+ detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ void
+ fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap, std::map<BoKeyT, BoValueT>& boMap)
{
boMap.clear();
for (const auto& [dtoKey, dtoValue] : dtoMap)
@@ -283,20 +297,17 @@ namespace detail
}
}
-
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
- std::map<DtoKeyT, DtoValueT> toAron(const std::map<BoKeyT, BoValueT>& boMap)
+ requires(!(detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and
+ detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ std::map<DtoKeyT, DtoValueT>
+ toAron(const std::map<BoKeyT, BoValueT>& boMap)
{
std::map<DtoKeyT, DtoValueT> dtoMap;
toAron(dtoMap, boMap);
return dtoMap;
}
-}
-
-
-
-
+} // namespace armarx::aron
// And do the same for the armarx namespace to ensure consistency with all the other aron_conversions declaraions
// (which are usually in the armarx namespace)
@@ -304,118 +315,144 @@ namespace armarx
{
// Same type
template <class T>
- void toAron(T& dto, const T& bo)
+ void
+ toAron(T& dto, const T& bo)
{
armarx::aron::toAron(dto, bo);
}
+
template <class T>
- void fromAron(const T& dto, T& bo)
+ void
+ fromAron(const T& dto, T& bo)
{
armarx::aron::fromAron(dto, bo);
}
-
// Generic return version
template <class DtoT, class BoT>
- DtoT toAron(const BoT& bo)
+ DtoT
+ toAron(const BoT& bo)
{
return armarx::aron::toAron<DtoT, BoT>(bo);
}
+
template <class BoT, class DtoT>
- BoT fromAron(const DtoT& dto)
+ BoT
+ fromAron(const DtoT& dto)
{
return armarx::aron::fromAron<BoT, DtoT>(dto);
}
// std::unique_ptr
template <class DtoT, class BoT>
- void toAron(DtoT& dto, const std::unique_ptr<BoT>& bo)
+ void
+ toAron(DtoT& dto, const std::unique_ptr<BoT>& bo)
{
armarx::aron::toAron(dto, bo);
}
+
template <class DtoT, class BoT>
- void fromAron(const DtoT& dto, std::unique_ptr<BoT>& bo)
+ void
+ fromAron(const DtoT& dto, std::unique_ptr<BoT>& bo)
{
armarx::aron::fromAron(dto, bo);
}
// std::optional
template <class DtoT, class BoT>
- requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
- void toAron(std::optional<DtoT>& dto, const std::optional<BoT>& bo)
+ requires(!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
+ void
+ toAron(std::optional<DtoT>& dto, const std::optional<BoT>& bo)
{
armarx::aron::toAron(dto, bo);
}
+
template <class DtoT, class BoT>
- requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
- void fromAron(const std::optional<DtoT>& dto, std::optional<BoT>& bo)
+ requires(!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
+ void
+ fromAron(const std::optional<DtoT>& dto, std::optional<BoT>& bo)
{
armarx::aron::fromAron(dto, bo);
}
// Flag-controlled optional
template <class DtoT, class BoT>
- void toAron(DtoT& dto, bool& dtoValid, const BoT& bo, bool boValid)
+ void
+ toAron(DtoT& dto, bool& dtoValid, const BoT& bo, bool boValid)
{
armarx::aron::toAron(dto, dtoValid, bo, boValid);
}
+
template <class DtoT, class BoT>
- void fromAron(const DtoT& dto, bool dtoValid, BoT& bo, bool& boValid)
+ void
+ fromAron(const DtoT& dto, bool dtoValid, BoT& bo, bool& boValid)
{
armarx::aron::fromAron(dto, dtoValid, bo, boValid);
}
template <class DtoT, class BoT>
- void toAron(DtoT& dto, bool& dtoValid, const std::optional<BoT>& bo)
+ void
+ toAron(DtoT& dto, bool& dtoValid, const std::optional<BoT>& bo)
{
armarx::aron::toAron(dto, dtoValid, bo);
}
+
template <class DtoT, class BoT>
- void fromAron(const DtoT& dto, bool dtoValid, std::optional<BoT>& bo)
+ void
+ fromAron(const DtoT& dto, bool dtoValid, std::optional<BoT>& bo)
{
armarx::aron::fromAron(dto, dtoValid, bo);
}
// std::vector
template <class DtoT, class BoT>
- requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
- void toAron(std::vector<DtoT>& dtos, const std::vector<BoT>& bos)
+ requires(!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
+ void
+ toAron(std::vector<DtoT>& dtos, const std::vector<BoT>& bos)
{
armarx::aron::toAron(dtos, bos);
}
+
template <class DtoT, class BoT>
- requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
- void fromAron(const std::vector<DtoT>& dtos, std::vector<BoT>& bos)
+ requires(!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
+ void
+ fromAron(const std::vector<DtoT>& dtos, std::vector<BoT>& bos)
{
armarx::aron::fromAron(dtos, bos);
}
template <class DtoT, class BoT>
- requires (!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
- std::vector<DtoT> toAron(const std::vector<BoT>& bos)
+ requires(!aron::detail::DtoAndBoAreSame<DtoT, BoT>)
+ std::vector<DtoT>
+ toAron(const std::vector<BoT>& bos)
{
return armarx::aron::toAron<DtoT, BoT>(bos);
}
-
// std::map
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
- void toAron(std::map<DtoKeyT, DtoValueT>& dtoMap, const std::map<BoKeyT, BoValueT>& boMap)
+ requires(!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and
+ aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ void
+ toAron(std::map<DtoKeyT, DtoValueT>& dtoMap, const std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::toAron(dtoMap, boMap);
}
+
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
- void fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap, std::map<BoKeyT, BoValueT>& boMap)
+ requires(!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and
+ aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ void
+ fromAron(const std::map<DtoKeyT, DtoValueT>& dtoMap, std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::fromAron(dtoMap, boMap);
}
-
template <class DtoKeyT, class DtoValueT, class BoKeyT, class BoValueT>
- requires (!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
- std::map<DtoKeyT, DtoValueT> toAron(const std::map<BoKeyT, BoValueT>& boMap)
+ requires(!(aron::detail::DtoAndBoAreSame<DtoKeyT, BoKeyT> and
+ aron::detail::DtoAndBoAreSame<DtoValueT, BoValueT>))
+ std::map<DtoKeyT, DtoValueT>
+ toAron(const std::map<BoKeyT, BoValueT>& boMap)
{
armarx::aron::toAron<DtoKeyT, DtoValueT, BoKeyT, BoValueT>(boMap);
}
@@ -432,32 +469,28 @@ namespace armarx
* @return the vector of aron elements
*/
template <typename T>
- auto fromAron(const std::vector<T>& v) -> std::vector<decltype(fromAron(T()))>
+ auto
+ fromAron(const std::vector<T>& v) -> std::vector<decltype(fromAron(T()))>
{
std::vector<decltype(fromAron(T()))> r;
r.reserve(v.size());
- std::transform(v.begin(), v.end(), std::back_inserter(r),
- [](const T & t)
- {
- return fromAron(t);
- });
+ std::transform(
+ v.begin(), v.end(), std::back_inserter(r), [](const T& t) { return fromAron(t); });
return r;
}
-
- template <typename T> auto toAron(const std::vector<T>& v)
+ template <typename T>
+ auto
+ toAron(const std::vector<T>& v)
{
std::vector<decltype(toAron(T()))> r;
r.reserve(v.size());
- std::transform(v.begin(), v.end(), std::back_inserter(r),
- [](const T & t)
- {
- return toAron(t);
- });
+ std::transform(
+ v.begin(), v.end(), std::back_inserter(r), [](const T& t) { return toAron(t); });
return r;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedIntEnum.h b/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedIntEnum.h
index f173775dabfd4faf404ea47564f3e74183e32de8..69f686d9c1563963fe4a987a46fe403f83c83597 100644
--- a/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedIntEnum.h
+++ b/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedIntEnum.h
@@ -25,7 +25,6 @@
#include "AronGeneratedClass.h"
-
namespace armarx::aron::cpp
{
class AronGeneratedIntEnumBase : public AronGeneratedClass
@@ -48,16 +47,14 @@ namespace armarx::aron::cpp
};
template <class Derived>
- class AronGeneratedIntEnum :
- public AronGeneratedIntEnumBase
+ class AronGeneratedIntEnum : public AronGeneratedIntEnumBase
{
public:
-
};
template <class T>
concept isAronGeneratedIntEnum = std::is_base_of<AronGeneratedIntEnumBase, T>::value;
-}
+} // namespace armarx::aron::cpp
namespace armarx::aron::codegenerator::cpp
{
@@ -65,4 +62,4 @@ namespace armarx::aron::codegenerator::cpp
template <class Derived>
using AronGeneratedIntEnum = aron::cpp::AronGeneratedIntEnum<Derived>;
-}
+} // namespace armarx::aron::codegenerator::cpp
diff --git a/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedObject.h b/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedObject.h
index 17d94831b41120c40dbb56932427e003fe3de1fd..12308f1c628492ab078b0b008b86f2910bc05fcc 100644
--- a/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedObject.h
+++ b/source/RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedObject.h
@@ -25,12 +25,10 @@
#include "AronGeneratedClass.h"
-
namespace armarx::aron::cpp
{
- class AronGeneratedObjectBase :
- public AronGeneratedClass
+ class AronGeneratedObjectBase : public AronGeneratedClass
{
public:
AronGeneratedObjectBase() = default;
@@ -50,15 +48,15 @@ namespace armarx::aron::cpp
};
template <class Derived>
- class AronGeneratedObject :
- public AronGeneratedObjectBase
+ class AronGeneratedObject : public AronGeneratedObjectBase
{
public:
AronGeneratedObject() = default;
virtual ~AronGeneratedObject() = default;
template <class T>
- void to(T& t) const
+ void
+ to(T& t) const
{
const Derived* d = dynamic_cast<const Derived*>(this);
if (d)
@@ -68,7 +66,8 @@ namespace armarx::aron::cpp
}
template <class T>
- void from(const T& t)
+ void
+ from(const T& t)
{
Derived* d = dynamic_cast<Derived*>(this);
if (d)
@@ -78,7 +77,8 @@ namespace armarx::aron::cpp
}
template <class T>
- void to(T& t, void(*fromAron)(const Derived&, T&)) const
+ void
+ to(T& t, void (*fromAron)(const Derived&, T&)) const
{
const Derived* d = dynamic_cast<const Derived*>(this);
if (d)
@@ -88,7 +88,8 @@ namespace armarx::aron::cpp
}
template <class T>
- void from(const T& t, void(*toAron)(Derived&, const T&))
+ void
+ from(const T& t, void (*toAron)(Derived&, const T&))
{
Derived* d = dynamic_cast<Derived*>(this);
if (d)
@@ -100,7 +101,7 @@ namespace armarx::aron::cpp
template <class T>
concept isAronGeneratedObject = std::is_base_of<AronGeneratedObjectBase, T>::value;
-}
+} // namespace armarx::aron::cpp
namespace armarx::aron::codegenerator::cpp
{
@@ -108,4 +109,4 @@ namespace armarx::aron::codegenerator::cpp
template <class Derived>
using AronGeneratedObject = aron::cpp::AronGeneratedObject<Derived>;
-}
+} // namespace armarx::aron::codegenerator::cpp
diff --git a/source/RobotAPI/libraries/aron/core/data/converter/Converter.h b/source/RobotAPI/libraries/aron/core/data/converter/Converter.h
index 0d86883385879df1f84b5bffd69cab1bdba070dd..13beb3ec58e48e674e08e665d775ea2e0d2ba030 100644
--- a/source/RobotAPI/libraries/aron/core/data/converter/Converter.h
+++ b/source/RobotAPI/libraries/aron/core/data/converter/Converter.h
@@ -23,28 +23,31 @@
#pragma once
-#include "../visitor/Visitor.h"
#include "../rw/Reader.h"
#include "../rw/Writer.h"
+#include "../visitor/Visitor.h"
namespace armarx::aron::data
{
// prototypes
template <class ReaderImplementation, class WriterImplementation, class DerivedT>
- requires isReader<ReaderImplementation> && isWriter<WriterImplementation>
+ requires isReader<ReaderImplementation> && isWriter<WriterImplementation>
struct Converter;
template <class T>
- concept isConverter = std::is_base_of<Converter<typename T::ReaderType, typename T::WriterType, typename T::This>, T>::value;
+ concept isConverter =
+ std::is_base_of<Converter<typename T::ReaderType, typename T::WriterType, typename T::This>,
+ T>::value;
template <class ConverterImplementation>
- requires isConverter<ConverterImplementation>
- typename ConverterImplementation::WriterReturnType readAndWrite(typename ConverterImplementation::ReaderInputType& o);
+ requires isConverter<ConverterImplementation>
+ typename ConverterImplementation::WriterReturnType
+ readAndWrite(typename ConverterImplementation::ReaderInputType& o);
/// Converter struct providing the needed methods.
/// WriterImplementation is a writer class, TODO: add concepts
template <class ReaderImplementation, class WriterImplementation, class DerivedT>
- requires isReader<ReaderImplementation> && isWriter<WriterImplementation>
+ requires isReader<ReaderImplementation> && isWriter<WriterImplementation>
struct Converter : virtual public Visitor<typename ReaderImplementation::InputType>
{
using WriterType = WriterImplementation;
@@ -60,12 +63,14 @@ namespace armarx::aron::data
virtual ~Converter() = default;
- data::Descriptor getDescriptor(ReaderInputType& o) final
+ data::Descriptor
+ getDescriptor(ReaderInputType& o) final
{
return r.getDescriptor(o);
}
- void visitDict(ReaderInputType& o) final
+ void
+ visitDict(ReaderInputType& o) final
{
std::map<std::string, ReaderInputTypeNonConst> elementsOfInput;
std::map<std::string, WriterReturnType> elementsReturn;
@@ -80,7 +85,8 @@ namespace armarx::aron::data
last_returned = w.writeDict(elementsReturn, std::nullopt, p);
};
- void visitList(ReaderInputType& o) final
+ void
+ visitList(ReaderInputType& o) final
{
std::vector<ReaderInputTypeNonConst> elementsOfInput;
std::vector<WriterReturnType> elementsReturn;
@@ -94,7 +100,8 @@ namespace armarx::aron::data
last_returned = w.writeList(elementsReturn, p);
};
- void visitNDArray(ReaderInputType& o) final
+ void
+ visitNDArray(ReaderInputType& o) final
{
std::string type;
std::vector<int> shape;
@@ -104,7 +111,8 @@ namespace armarx::aron::data
last_returned = w.writeNDArray(shape, type, data.data(), p);
};
- void visitInt(ReaderInputType& o) final
+ void
+ visitInt(ReaderInputType& o) final
{
int i;
Path p;
@@ -112,7 +120,8 @@ namespace armarx::aron::data
last_returned = w.writeInt(i, p);
};
- void visitLong(ReaderInputType& o) final
+ void
+ visitLong(ReaderInputType& o) final
{
long i;
Path p;
@@ -120,7 +129,8 @@ namespace armarx::aron::data
last_returned = w.writeLong(i, p);
};
- void visitFloat(ReaderInputType& o) final
+ void
+ visitFloat(ReaderInputType& o) final
{
float i;
Path p;
@@ -128,7 +138,8 @@ namespace armarx::aron::data
last_returned = w.writeFloat(i, p);
};
- void visitDouble(ReaderInputType& o) final
+ void
+ visitDouble(ReaderInputType& o) final
{
double i;
Path p;
@@ -136,7 +147,8 @@ namespace armarx::aron::data
last_returned = w.writeDouble(i, p);
};
- void visitBool(ReaderInputType& o) final
+ void
+ visitBool(ReaderInputType& o) final
{
bool i;
Path p;
@@ -144,7 +156,8 @@ namespace armarx::aron::data
last_returned = w.writeBool(i, p);
};
- void visitString(ReaderInputType& o) final
+ void
+ visitString(ReaderInputType& o) final
{
std::string i;
Path p;
@@ -152,11 +165,13 @@ namespace armarx::aron::data
last_returned = w.writeString(i, p);
};
- void visitUnknown(ReaderInputType& o) final
+ void
+ visitUnknown(ReaderInputType& o) final
{
if (!r.readNull(o))
{
- throw error::AronException(__PRETTY_FUNCTION__, "A visitor got data but the enum is unknown.");
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "A visitor got data but the enum is unknown.");
}
w.writeNull();
};
@@ -165,11 +180,12 @@ namespace armarx::aron::data
/// the function to read from a variant and write to a writer T
/// returns the returntype of T
template <class ConverterImplementation>
- requires isConverter<ConverterImplementation>
- typename ConverterImplementation::WriterReturnType readAndWrite(typename ConverterImplementation::ReaderInputType& o)
+ requires isConverter<ConverterImplementation>
+ typename ConverterImplementation::WriterReturnType
+ readAndWrite(typename ConverterImplementation::ReaderInputType& o)
{
ConverterImplementation v;
data::visit(v, o);
return v.last_returned;
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.h b/source/RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.h
index dea8f1751f8cdfe25cef3491b058e7f0b5b64602..23f4583c6f8cb09fbb6afa98a7b24b56807ba815 100644
--- a/source/RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.h
+++ b/source/RobotAPI/libraries/aron/core/data/converter/nlohmannJSON/NlohmannJSONConverter.h
@@ -23,28 +23,32 @@
#pragma once
-#include "../Converter.h"
-#include "../../visitor/nlohmannJSON/NlohmannJSONVisitor.h"
#include "../../rw/reader/nlohmannJSON/NlohmannJSONReader.h"
#include "../../rw/writer/nlohmannJSON/NlohmannJSONWriter.h"
+#include "../../visitor/nlohmannJSON/NlohmannJSONVisitor.h"
+#include "../Converter.h"
namespace armarx::aron::data
{
// WriterImplementation is a writer class
template <class WriterImplementation>
- requires isWriter<WriterImplementation>
+ requires isWriter<WriterImplementation>
struct FromNlohmannJSONConverter :
- virtual public Converter<aron::data::reader::NlohmannJSONReader, WriterImplementation, FromNlohmannJSONConverter<WriterImplementation>>
+ virtual public Converter<aron::data::reader::NlohmannJSONReader,
+ WriterImplementation,
+ FromNlohmannJSONConverter<WriterImplementation>>
{
virtual ~FromNlohmannJSONConverter() = default;
};
// ReaderImplementation is a reader class
template <class ReaderImplementation>
- requires isReader<ReaderImplementation>
+ requires isReader<ReaderImplementation>
struct ToNlohmannJSONConverter :
- virtual public Converter<ReaderImplementation, aron::data::writer::NlohmannJSONWriter, ToNlohmannJSONConverter<ReaderImplementation>>
+ virtual public Converter<ReaderImplementation,
+ aron::data::writer::NlohmannJSONWriter,
+ ToNlohmannJSONConverter<ReaderImplementation>>
{
virtual ~ToNlohmannJSONConverter() = default;
};
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h b/source/RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h
index 47a0e9fa1ad1c49256c2d203b35ce85f7461f868..d9b1f93aa36c339d2b10d1d8e0623b771a16d7a0 100644
--- a/source/RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h
+++ b/source/RobotAPI/libraries/aron/core/data/converter/variant/VariantConverter.h
@@ -23,29 +23,32 @@
#pragma once
-#include "../Converter.h"
-#include "../../visitor/variant/VariantVisitor.h"
#include "../../rw/reader/variant/VariantReader.h"
#include "../../rw/writer/variant/VariantWriter.h"
-
+#include "../../visitor/variant/VariantVisitor.h"
+#include "../Converter.h"
namespace armarx::aron::data
{
// WriterImplementation is a writer class
template <class WriterImplementation>
- requires isWriter<WriterImplementation>
+ requires isWriter<WriterImplementation>
struct FromVariantConverter :
- virtual public Converter<aron::data::reader::VariantReader, WriterImplementation, FromVariantConverter<WriterImplementation>>
+ virtual public Converter<aron::data::reader::VariantReader,
+ WriterImplementation,
+ FromVariantConverter<WriterImplementation>>
{
virtual ~FromVariantConverter() = default;
};
// ReaderImplementation is a reader class
template <class ReaderImplementation>
- requires isReader<ReaderImplementation>
+ requires isReader<ReaderImplementation>
struct ToVariantConverter :
- virtual public Converter<ReaderImplementation, aron::data::writer::VariantWriter, ToVariantConverter<ReaderImplementation>>
+ virtual public Converter<ReaderImplementation,
+ aron::data::writer::VariantWriter,
+ ToVariantConverter<ReaderImplementation>>
{
virtual ~ToVariantConverter() = default;
};
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/Reader.cpp b/source/RobotAPI/libraries/aron/core/data/rw/Reader.cpp
index 9889cf16f3f395296092e74c17ddde5e16e99833..6cbe231d3938d8dafc3b410306686df98f17deba 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/Reader.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Reader.cpp
@@ -24,4 +24,3 @@ namespace armarx::aron::data
{
}
-
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/Reader.h b/source/RobotAPI/libraries/aron/core/data/rw/Reader.h
index c486d26a5d71a4c419aa8ea086d9ace2342ba6c6..ed7ea4a021118ff1bd4fc563b5965fef0dd2a1a2 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/Reader.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Reader.h
@@ -22,13 +22,13 @@
// STD/STL
#include <memory>
+#include <optional>
#include <string>
#include <vector>
-#include <optional>
// ArmarX
-#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
+#include <RobotAPI/libraries/aron/core/Path.h>
namespace armarx::aron::data
{
@@ -43,10 +43,16 @@ namespace armarx::aron::data
virtual data::Descriptor getDescriptor(InputType& input) = 0;
- virtual void readList(InputType& input, std::vector<InputTypeNonConst>& elements, Path&) = 0;
- virtual void readDict(InputType& input, std::map<std::string, InputTypeNonConst>& elements, Path&) = 0;
+ virtual void
+ readList(InputType& input, std::vector<InputTypeNonConst>& elements, Path&) = 0;
+ virtual void
+ readDict(InputType& input, std::map<std::string, InputTypeNonConst>& elements, Path&) = 0;
- virtual void readNDArray(InputType& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data, Path&) = 0;
+ virtual void readNDArray(InputType& input,
+ std::vector<int>& shape,
+ std::string& typeAsString,
+ std::vector<unsigned char>& data,
+ Path&) = 0;
virtual void readInt(InputType& input, int& i, Path&) = 0;
virtual void readLong(InputType& input, long& i, Path&) = 0;
@@ -55,131 +61,154 @@ namespace armarx::aron::data
virtual void readString(InputType& input, std::string& s, Path&) = 0;
virtual void readBool(InputType& input, bool& i, Path&) = 0;
- virtual bool readNull(InputType& input) // defaulted implementation
+ virtual bool
+ readNull(InputType& input) // defaulted implementation
{
InputType nil = {};
return (input == nil);
}
// Convenience methods
- void readPrimitive(InputType& input, int& i, Path& p)
+ void
+ readPrimitive(InputType& input, int& i, Path& p)
{
return readInt(input, i, p);
}
- void readPrimitive(InputType& input, long& i, Path& p)
+ void
+ readPrimitive(InputType& input, long& i, Path& p)
{
return readLong(input, i, p);
}
- void readPrimitive(InputType& input, float& i, Path& p)
+ void
+ readPrimitive(InputType& input, float& i, Path& p)
{
return readFloat(input, i, p);
}
- void readPrimitive(InputType& input, double& i, Path& p)
+ void
+ readPrimitive(InputType& input, double& i, Path& p)
{
return readDouble(input, i, p);
}
- void readPrimitive(InputType& input, std::string& i, Path& p)
+ void
+ readPrimitive(InputType& input, std::string& i, Path& p)
{
return readString(input, i, p);
}
- void readPrimitive(InputType& input, bool& i, Path& p)
+ void
+ readPrimitive(InputType& input, bool& i, Path& p)
{
return readBool(input, i, p);
}
// Convenience methods without path object
- void readList(InputType& input, std::vector<InputTypeNonConst>& elements)
+ void
+ readList(InputType& input, std::vector<InputTypeNonConst>& elements)
{
Path p;
return readList(input, elements, p);
}
- void readDict(InputType& input, std::map<std::string, InputTypeNonConst>& elements)
+ void
+ readDict(InputType& input, std::map<std::string, InputTypeNonConst>& elements)
{
Path p;
return readDict(input, elements, p);
}
- void readNDArray(InputType& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data)
+ void
+ readNDArray(InputType& input,
+ std::vector<int>& shape,
+ std::string& typeAsString,
+ std::vector<unsigned char>& data)
{
Path p;
return readNDArray(input, shape, typeAsString, data, p);
}
- void readInt(InputType& input, int& i)
+ void
+ readInt(InputType& input, int& i)
{
Path p;
return readInt(input, i, p);
}
- void readLong(InputType& input, long& i)
+ void
+ readLong(InputType& input, long& i)
{
Path p;
return readLong(input, i, p);
}
- void readFloat(InputType& input, float& i)
+ void
+ readFloat(InputType& input, float& i)
{
Path p;
return readFloat(input, i, p);
}
- void readDouble(InputType& input, double& i)
+ void
+ readDouble(InputType& input, double& i)
{
Path p;
return readDouble(input, i, p);
}
- void readString(InputType& input, std::string& s)
+ void
+ readString(InputType& input, std::string& s)
{
Path p;
return readString(input, s, p);
}
- void readBool(InputType& input, bool& i)
+ void
+ readBool(InputType& input, bool& i)
{
Path p;
return readBool(input, i, p);
}
- void readPrimitive(InputType& input, int& i)
+ void
+ readPrimitive(InputType& input, int& i)
{
return readInt(input, i);
}
- void readPrimitive(InputType& input, long& i)
+ void
+ readPrimitive(InputType& input, long& i)
{
return readLong(input, i);
}
- void readPrimitive(InputType& input, float& i)
+ void
+ readPrimitive(InputType& input, float& i)
{
return readFloat(input, i);
}
- void readPrimitive(InputType& input, double& i)
+ void
+ readPrimitive(InputType& input, double& i)
{
return readDouble(input, i);
}
- void readPrimitive(InputType& input, std::string& i)
+ void
+ readPrimitive(InputType& input, std::string& i)
{
return readString(input, i);
}
- void readPrimitive(InputType& input, bool& i)
+ void
+ readPrimitive(InputType& input, bool& i)
{
return readBool(input, i);
}
-
-
};
template <class T>
concept isReader = std::is_base_of<ReaderInterface<typename T::InputType>, T>::value;
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/Writer.cpp b/source/RobotAPI/libraries/aron/core/data/rw/Writer.cpp
index 68a462aac0cea4bdea7c22c456b34d131751462a..a9fa41173b1bb763c133814e89c65c5683397d84 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/Writer.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Writer.cpp
@@ -20,7 +20,6 @@
#include "Writer.h"
-
namespace armarx::aron::data
{
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/Writer.h b/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
index c5b063f3afed5dbd50cee9dddde6f408c018b3bd..3166679131e723eadda30aadee27ea2301b271c9 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/Writer.h
@@ -21,8 +21,8 @@
#pragma once
// STD/STL
-#include <string>
#include <optional>
+#include <string>
// ArmarX
#include <RobotAPI/interface/aron.h>
@@ -46,9 +46,14 @@ namespace armarx::aron::data
virtual data::Descriptor getDescriptor(ReturnTypeConst& input) = 0;
virtual ReturnType writeList(const std::vector<ReturnType>& elements, const Path& p) = 0;
- virtual ReturnType writeDict(const std::map<std::string, ReturnType>& elements, const std::optional<ReturnType>& extends, const Path& p) = 0;
+ virtual ReturnType writeDict(const std::map<std::string, ReturnType>& elements,
+ const std::optional<ReturnType>& extends,
+ const Path& p) = 0;
- virtual ReturnType writeNDArray(const std::vector<int>& shape, const std::string& typeAsString, const unsigned char* data, const Path& p) = 0;
+ virtual ReturnType writeNDArray(const std::vector<int>& shape,
+ const std::string& typeAsString,
+ const unsigned char* data,
+ const Path& p) = 0;
virtual ReturnType writeInt(const int i, const Path& p) = 0;
virtual ReturnType writeLong(const long i, const Path& p) = 0;
@@ -57,38 +62,45 @@ namespace armarx::aron::data
virtual ReturnType writeString(const std::string& i, const Path& p) = 0;
virtual ReturnType writeBool(const bool i, const Path& p) = 0;
- virtual ReturnType writeNull() // defaulted implementation
+ virtual ReturnType
+ writeNull() // defaulted implementation
{
return {};
}
// Convenience methods
- ReturnType writePrimitive(const int i, const Path& p = Path())
+ ReturnType
+ writePrimitive(const int i, const Path& p = Path())
{
return writeInt(i, p);
}
- ReturnType writePrimitive(const long i, const Path& p = Path())
+ ReturnType
+ writePrimitive(const long i, const Path& p = Path())
{
return writeLong(i, p);
}
- ReturnType writePrimitive(const float i, const Path& p = Path())
+ ReturnType
+ writePrimitive(const float i, const Path& p = Path())
{
return writeFloat(i, p);
}
- ReturnType writePrimitive(const double i, const Path& p = Path())
+ ReturnType
+ writePrimitive(const double i, const Path& p = Path())
{
return writeDouble(i, p);
}
- ReturnType writePrimitive(const std::string& i, const Path& p = Path())
+ ReturnType
+ writePrimitive(const std::string& i, const Path& p = Path())
{
return writeString(i, p);
}
- ReturnType writePrimitive(const bool i, const Path& p = Path())
+ ReturnType
+ writePrimitive(const bool i, const Path& p = Path())
{
return writeBool(i, p);
}
@@ -96,4 +108,4 @@ namespace armarx::aron::data
template <class T>
concept isWriter = std::is_base_of<WriterInterface<typename T::ReturnType>, T>::value;
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h b/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h
index 83357fe1c7e0e9d128d7bd499ac3b39a63b8384e..7af8d1d0adf5aa973eff4db717686a6e3252fd04 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/json/Data.h
@@ -55,7 +55,7 @@ namespace armarx::aron::data::rw::json
const std::string STRING_TYPENAME_SLUG = "_ARON_STRING";
const std::string BOOL_TYPENAME_SLUG = "_ARON_BOOL";
const std::string TIME_TYPENAME_SLUG = "_ARON_TIME";
- }
+ } // namespace constantes
namespace conversion
{
@@ -68,22 +68,18 @@ namespace armarx::aron::data::rw::json
{data::Descriptor::FLOAT, rw::json::constantes::FLOAT_TYPENAME_SLUG},
{data::Descriptor::DOUBLE, rw::json::constantes::DOUBLE_TYPENAME_SLUG},
{data::Descriptor::BOOL, rw::json::constantes::BOOL_TYPENAME_SLUG},
- {data::Descriptor::STRING, rw::json::constantes::STRING_TYPENAME_SLUG}
- };
+ {data::Descriptor::STRING, rw::json::constantes::STRING_TYPENAME_SLUG}};
const auto String2Descriptor = aron::conversion::util::InvertMap(Descriptor2String);
- const std::map<type::Maybe, std::string> Maybe2String =
- {
+ const std::map<type::Maybe, std::string> Maybe2String = {
{type::Maybe::NONE, "type::maybe::NONE"},
{type::Maybe::OPTIONAL, "type::maybe::OPTIONAL"},
{type::Maybe::RAW_PTR, "type::maybe::RAW_PTR"},
{type::Maybe::SHARED_PTR, "type::maybe::SHARED_PTR"},
- {type::Maybe::UNIQUE_PTR, "type::maybe::UNIQUE_PTR"}
- };
+ {type::Maybe::UNIQUE_PTR, "type::maybe::UNIQUE_PTR"}};
const auto String2Maybe = aron::conversion::util::InvertMap(Maybe2String);
- const std::map<type::ndarray::ElementType, std::string> NDArrayType2String =
- {
+ const std::map<type::ndarray::ElementType, std::string> NDArrayType2String = {
{type::ndarray::ElementType::INT8, "type::ndarray::INT8"},
{type::ndarray::ElementType::INT16, "type::ndarray::INT16"},
{type::ndarray::ElementType::INT32, "type::ndarray::INT32"},
@@ -91,12 +87,10 @@ namespace armarx::aron::data::rw::json
{type::ndarray::ElementType::UINT16, "type::ndarray::UINT16"},
{type::ndarray::ElementType::UINT32, "type::ndarray::UINT32"},
{type::ndarray::ElementType::FLOAT32, "type::ndarray::FLOAT32"},
- {type::ndarray::ElementType::FLOAT64, "type::ndarray::FLOAT64"}
- };
+ {type::ndarray::ElementType::FLOAT64, "type::ndarray::FLOAT64"}};
const auto String2NDArrayType = aron::conversion::util::InvertMap(NDArrayType2String);
- const std::map<type::matrix::ElementType, std::string> MatrixType2String =
- {
+ const std::map<type::matrix::ElementType, std::string> MatrixType2String = {
{type::matrix::ElementType::UINT8, "type::matrix::UINT8"},
{type::matrix::ElementType::UINT16, "type::matrix::UINT16"},
{type::matrix::ElementType::UINT32, "type::matrix::UINT32"},
@@ -105,34 +99,27 @@ namespace armarx::aron::data::rw::json
{type::matrix::ElementType::INT32, "type::matrix::INT32"},
{type::matrix::ElementType::INT64, "type::matrix::INT64"},
{type::matrix::ElementType::FLOAT32, "type::matrix::FLOAT32"},
- {type::matrix::ElementType::FLOAT64, "type::matrix::FLOAT64"}
- };
+ {type::matrix::ElementType::FLOAT64, "type::matrix::FLOAT64"}};
const auto String2MatrixType = aron::conversion::util::InvertMap(MatrixType2String);
- const std::map<type::quaternion::ElementType, std::string> QuaternionType2String =
- {
+ const std::map<type::quaternion::ElementType, std::string> QuaternionType2String = {
{type::quaternion::ElementType::FLOAT32, "type::quaternion::FLOAT32"},
- {type::quaternion::ElementType::FLOAT64, "type::quaternion::FLOAT64"}
- };
+ {type::quaternion::ElementType::FLOAT64, "type::quaternion::FLOAT64"}};
const auto String2QuaternionType = aron::conversion::util::InvertMap(QuaternionType2String);
- const std::map<type::image::PixelType, std::string> PixelType2String =
- {
+ const std::map<type::image::PixelType, std::string> PixelType2String = {
{type::image::PixelType::RGB24, "type::image::RGB24"},
- {type::image::PixelType::DEPTH32, "type::image::DEPTH32"}
- };
+ {type::image::PixelType::DEPTH32, "type::image::DEPTH32"}};
const auto String2PixelType = aron::conversion::util::InvertMap(PixelType2String);
- const std::map<type::pointcloud::VoxelType, std::string> VoxelType2String =
- {
+ const std::map<type::pointcloud::VoxelType, std::string> VoxelType2String = {
{type::pointcloud::VoxelType::POINT_XYZ, "type::pointcloud::POINT_XYZ"},
{type::pointcloud::VoxelType::POINT_XYZI, "type::pointcloud::POINT_XYZI"},
{type::pointcloud::VoxelType::POINT_XYZL, "type::pointcloud::POINT_XYZL"},
{type::pointcloud::VoxelType::POINT_XYZRGB, "type::pointcloud::POINT_XYZRGB"},
{type::pointcloud::VoxelType::POINT_XYZRGBA, "type::pointcloud::POINT_XYZRGBA"},
{type::pointcloud::VoxelType::POINT_XYZRGBL, "type::pointcloud::POINT_XYZRGBL"},
- {type::pointcloud::VoxelType::POINT_XYZHSV, "type::pointcloud::POINT_XYZHSV"}
- };
+ {type::pointcloud::VoxelType::POINT_XYZHSV, "type::pointcloud::POINT_XYZHSV"}};
const auto String2VoxelType = aron::conversion::util::InvertMap(VoxelType2String);
- }
-}
+ } // namespace conversion
+} // namespace armarx::aron::data::rw::json
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
index e6beb0e9aab36bc5aa245155b7a42b3d0acb82ab..cb3a3c5c47584727969995c542ad2aba849d46ac 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -26,49 +26,68 @@
#include "NlohmannJSONReader.h"
// ArmarX
-#include "../../json/Data.h"
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h>
+#include "../../json/Data.h"
namespace armarx::aron::data::reader
{
namespace
{
/// Throw an exception if the type is other than expected
- void getAronMetaInformationForType(const nlohmann::json& input, const std::string& expectedType, Path& p)
+ void
+ getAronMetaInformationForType(const nlohmann::json& input,
+ const std::string& expectedType,
+ Path& p)
{
// check type
if (input.at(rw::json::constantes::TYPE_SLUG) != expectedType)
{
- throw error::ValueNotValidException(__PRETTY_FUNCTION__, "Wrong type in json encountered.", input.at(rw::json::constantes::TYPE_SLUG), expectedType);
+ throw error::ValueNotValidException(__PRETTY_FUNCTION__,
+ "Wrong type in json encountered.",
+ input.at(rw::json::constantes::TYPE_SLUG),
+ expectedType);
}
// set path
std::vector<std::string> pathElements = input.at(rw::json::constantes::PATH_SLUG);
p = Path(pathElements);
}
- }
+ } // namespace
- data::Descriptor NlohmannJSONReader::getDescriptor(InputType& input)
+ data::Descriptor
+ NlohmannJSONReader::getDescriptor(InputType& input)
{
auto ret = ConstNlohmannJSONVisitor::GetDescriptor(input);
return ret;
}
- void NlohmannJSONReader::readList(const nlohmann::json& input, std::vector<nlohmann::json>& elements, Path& p)
+ void
+ NlohmannJSONReader::readList(const nlohmann::json& input,
+ std::vector<nlohmann::json>& elements,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::LIST_TYPENAME_SLUG, p);
elements = input.at(rw::json::constantes::ELEMENTS_SLUG).get<std::vector<nlohmann::json>>();
}
- void NlohmannJSONReader::readDict(const nlohmann::json& input, std::map<std::string, nlohmann::json>& elements, Path& p)
+ void
+ NlohmannJSONReader::readDict(const nlohmann::json& input,
+ std::map<std::string, nlohmann::json>& elements,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::DICT_TYPENAME_SLUG, p);
- elements = input.at(rw::json::constantes::ELEMENTS_SLUG).get<std::map<std::string, nlohmann::json>>();
+ elements = input.at(rw::json::constantes::ELEMENTS_SLUG)
+ .get<std::map<std::string, nlohmann::json>>();
}
- void NlohmannJSONReader::readNDArray(const nlohmann::json& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data, Path& p)
+ void
+ NlohmannJSONReader::readNDArray(const nlohmann::json& input,
+ std::vector<int>& shape,
+ std::string& typeAsString,
+ std::vector<unsigned char>& data,
+ Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::NDARRAY_TYPENAME_SLUG, p);
shape = input.at(rw::json::constantes::DIMENSIONS_SLUG).get<std::vector<int>>();
@@ -76,39 +95,45 @@ namespace armarx::aron::data::reader
data = input.at(rw::json::constantes::DATA_SLUG).get<std::vector<unsigned char>>();
}
- void NlohmannJSONReader::readInt(const nlohmann::json& input, int& i, Path& p)
+ void
+ NlohmannJSONReader::readInt(const nlohmann::json& input, int& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::INT_TYPENAME_SLUG, p);
i = input.at(rw::json::constantes::VALUE_SLUG);
}
- void NlohmannJSONReader::readLong(const nlohmann::json& input, long& i, Path& p)
+ void
+ NlohmannJSONReader::readLong(const nlohmann::json& input, long& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::LONG_TYPENAME_SLUG, p);
i = input.at(rw::json::constantes::VALUE_SLUG);
}
- void NlohmannJSONReader::readFloat(const nlohmann::json& input, float& i, Path& p)
+ void
+ NlohmannJSONReader::readFloat(const nlohmann::json& input, float& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::FLOAT_TYPENAME_SLUG, p);
i = input.at(rw::json::constantes::VALUE_SLUG);
}
- void NlohmannJSONReader::readDouble(const nlohmann::json& input, double& i, Path& p)
+ void
+ NlohmannJSONReader::readDouble(const nlohmann::json& input, double& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::DOUBLE_TYPENAME_SLUG, p);
i = input.at(rw::json::constantes::VALUE_SLUG);
}
- void NlohmannJSONReader::readString(const nlohmann::json& input, std::string& i, Path& p)
+ void
+ NlohmannJSONReader::readString(const nlohmann::json& input, std::string& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::STRING_TYPENAME_SLUG, p);
i = input.at(rw::json::constantes::VALUE_SLUG);
}
- void NlohmannJSONReader::readBool(const nlohmann::json& input, bool& i, Path& p)
+ void
+ NlohmannJSONReader::readBool(const nlohmann::json& input, bool& i, Path& p)
{
getAronMetaInformationForType(input, rw::json::constantes::BOOL_TYPENAME_SLUG, p);
i = input.at(rw::json::constantes::VALUE_SLUG);
}
-}
+} // namespace armarx::aron::data::reader
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h
index 509aae5ddd0426236cb8bd2765367c7704fd4228..d2d4c9370c5df53dc838ba0cb10c995cac30ce77 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReader.h
@@ -32,8 +32,7 @@
namespace armarx::aron::data::reader
{
- class NlohmannJSONReader :
- public ReaderInterface<const nlohmann::json>
+ class NlohmannJSONReader : public ReaderInterface<const nlohmann::json>
{
using Base = ReaderInterface<const nlohmann::json>;
@@ -43,20 +42,26 @@ namespace armarx::aron::data::reader
data::Descriptor getDescriptor(InputType& input) final;
- using Base::readList;
+ using Base::readBool;
using Base::readDict;
- using Base::readNDArray;
+ using Base::readDouble;
+ using Base::readFloat;
using Base::readInt;
+ using Base::readList;
using Base::readLong;
- using Base::readFloat;
- using Base::readDouble;
+ using Base::readNDArray;
using Base::readString;
- using Base::readBool;
void readList(InputType& input, std::vector<InputTypeNonConst>& elements, Path& p) override;
- void readDict(InputType& input, std::map<std::string, InputTypeNonConst>& elements, Path& p) override;
+ void readDict(InputType& input,
+ std::map<std::string, InputTypeNonConst>& elements,
+ Path& p) override;
- void readNDArray(InputType& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data, Path& p) override;
+ void readNDArray(InputType& input,
+ std::vector<int>& shape,
+ std::string& typeAsString,
+ std::vector<unsigned char>& data,
+ Path& p) override;
void readInt(InputType& input, int& i, Path& p) override;
void readLong(InputType& input, long& i, Path& p) override;
@@ -65,4 +70,4 @@ namespace armarx::aron::data::reader
void readString(InputType& input, std::string& s, Path& p) override;
void readBool(InputType& input, bool& i, Path& p) override;
};
-}
+} // namespace armarx::aron::data::reader
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
index e6289f74d2bc80cb1057cd329d59ac31bf9e6c95..3b2caa7ecd14fa11c6accfafde17dd53542c4392 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.cpp
@@ -33,8 +33,6 @@
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h>
-
-
namespace armarx::aron::data::reader
{
data::Descriptor
@@ -59,7 +57,6 @@ namespace armarx::aron::data::reader
elements = input.get<std::map<std::string, nlohmann::json>>();
}
-
const std::map<std::string, type::matrix::ElementType> ElementTypeAsString = {
{"unsigned char", ::armarx::aron::type::matrix::UINT8},
{"unsigned short", ::armarx::aron::type::matrix::UINT16},
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h
index 2d17af7f5ead72cc51f04623a816e1fb8236c8ad..e0bed7b71869e2abc074484fe3fb9774e0ef6730 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h
@@ -69,6 +69,5 @@ namespace armarx::aron::data::reader
void readDouble(InputType& input, double& i, Path& p) override;
void readString(InputType& input, std::string& i, Path& p) override;
void readBool(InputType& input, bool& i, Path& p) override;
-
};
} // namespace armarx::aron::data::reader
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.cpp b/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.cpp
index cee7c8a1b90fef095514b7bc09fb56bb32856754..ca8a240d6a4381318d94a68f391a8f80255e0534 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.cpp
@@ -33,12 +33,16 @@
namespace armarx::aron::data::reader
{
- data::Descriptor VariantReader::getDescriptor(InputType& input)
+ data::Descriptor
+ VariantReader::getDescriptor(InputType& input)
{
return ConstVariantVisitor::GetDescriptor(input);
}
- void VariantReader::readList(const data::VariantPtr& input, std::vector<data::VariantPtr>& elements, Path& p)
+ void
+ VariantReader::readList(const data::VariantPtr& input,
+ std::vector<data::VariantPtr>& elements,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = data::List::DynamicCastAndCheck(input);
@@ -46,7 +50,10 @@ namespace armarx::aron::data::reader
p = o->getPath();
}
- void VariantReader::readDict(const data::VariantPtr& input, std::map<std::string, data::VariantPtr>& elements, Path& p)
+ void
+ VariantReader::readDict(const data::VariantPtr& input,
+ std::map<std::string, data::VariantPtr>& elements,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = data::Dict::DynamicCastAndCheck(input);
@@ -54,7 +61,12 @@ namespace armarx::aron::data::reader
p = o->getPath();
}
- void VariantReader::readNDArray(const data::VariantPtr& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data, Path& p)
+ void
+ VariantReader::readNDArray(const data::VariantPtr& input,
+ std::vector<int>& shape,
+ std::string& typeAsString,
+ std::vector<unsigned char>& data,
+ Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = data::NDArray::DynamicCastAndCheck(input);
@@ -64,7 +76,8 @@ namespace armarx::aron::data::reader
p = o->getPath();
}
- void VariantReader::readInt(const data::VariantPtr& input, int& i, Path& p)
+ void
+ VariantReader::readInt(const data::VariantPtr& input, int& i, Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = data::Int::DynamicCastAndCheck(input);
@@ -72,7 +85,8 @@ namespace armarx::aron::data::reader
p = o->getPath();
}
- void VariantReader::readLong(const data::VariantPtr& input, long& i, Path& p)
+ void
+ VariantReader::readLong(const data::VariantPtr& input, long& i, Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = data::Long::DynamicCastAndCheck(input);
@@ -80,7 +94,8 @@ namespace armarx::aron::data::reader
p = o->getPath();
}
- void VariantReader::readFloat(const data::VariantPtr& input, float& i, Path& p)
+ void
+ VariantReader::readFloat(const data::VariantPtr& input, float& i, Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = data::Float::DynamicCastAndCheck(input);
@@ -88,7 +103,8 @@ namespace armarx::aron::data::reader
p = o->getPath();
}
- void VariantReader::readDouble(const data::VariantPtr& input, double& i, Path& p)
+ void
+ VariantReader::readDouble(const data::VariantPtr& input, double& i, Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = data::Double::DynamicCastAndCheck(input);
@@ -96,7 +112,8 @@ namespace armarx::aron::data::reader
p = o->getPath();
}
- void VariantReader::readString(const data::VariantPtr& input, std::string& i, Path& p)
+ void
+ VariantReader::readString(const data::VariantPtr& input, std::string& i, Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = data::String::DynamicCastAndCheck(input);
@@ -104,11 +121,12 @@ namespace armarx::aron::data::reader
p = o->getPath();
}
- void VariantReader::readBool(const data::VariantPtr& input, bool& i, Path& p)
+ void
+ VariantReader::readBool(const data::VariantPtr& input, bool& i, Path& p)
{
ARMARX_CHECK_NOT_NULL(input);
auto o = data::Bool::DynamicCastAndCheck(input);
i = o->getValue();
p = o->getPath();
}
-}
+} // namespace armarx::aron::data::reader
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h b/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h
index e92102233835f47d785c2cbb4848b34a82b5a306..372751a5816c15cbfa8acf71b47d21ee977af1d9 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/reader/variant/VariantReader.h
@@ -30,11 +30,9 @@
// ArmarX
#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
-
namespace armarx::aron::data::reader
{
- class VariantReader :
- public ReaderInterface<const data::VariantPtr>
+ class VariantReader : public ReaderInterface<const data::VariantPtr>
{
using Base = ReaderInterface<const data::VariantPtr>;
@@ -42,22 +40,28 @@ namespace armarx::aron::data::reader
// constructors
VariantReader() = default;
- using Base::readList;
+ using Base::readBool;
using Base::readDict;
- using Base::readNDArray;
+ using Base::readDouble;
+ using Base::readFloat;
using Base::readInt;
+ using Base::readList;
using Base::readLong;
- using Base::readFloat;
- using Base::readDouble;
+ using Base::readNDArray;
using Base::readString;
- using Base::readBool;
data::Descriptor getDescriptor(InputType& input) final;
void readList(InputType& input, std::vector<InputTypeNonConst>& elements, Path& p) final;
- void readDict(InputType& input, std::map<std::string, InputTypeNonConst>& elements, Path& p) final;
+ void readDict(InputType& input,
+ std::map<std::string, InputTypeNonConst>& elements,
+ Path& p) final;
- void readNDArray(InputType& input, std::vector<int>& shape, std::string& typeAsString, std::vector<unsigned char>& data, Path& p) final;
+ void readNDArray(InputType& input,
+ std::vector<int>& shape,
+ std::string& typeAsString,
+ std::vector<unsigned char>& data,
+ Path& p) final;
void readInt(InputType& input, int& i, Path& p) final;
void readLong(InputType& input, long& i, Path& p) final;
@@ -66,4 +70,4 @@ namespace armarx::aron::data::reader
void readString(InputType& input, std::string& s, Path& p) final;
void readBool(InputType& input, bool& i, Path& p) final;
};
-}
+} // namespace armarx::aron::data::reader
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp b/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
index c73819295d362a0969ea6fd35ed176bd69a30853..97d0900795f86ff461566c8a04ec4789f59a5bd0 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.cpp
@@ -27,27 +27,33 @@
#include "NlohmannJSONWriter.h"
// ArmarX
-#include "../../json/Data.h"
#include <RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h>
+#include "../../json/Data.h"
+
namespace armarx::aron::data::writer
{
namespace
{
/// Set important members for json object (aron meta information)
- void setupAronMetaInformationForType(nlohmann::json& json, const std::string& type, const Path& p)
+ void
+ setupAronMetaInformationForType(nlohmann::json& json,
+ const std::string& type,
+ const Path& p)
{
json[rw::json::constantes::TYPE_SLUG] = type;
json[rw::json::constantes::PATH_SLUG] = p.getPath();
}
- }
+ } // namespace
- data::Descriptor NlohmannJSONWriter::getDescriptor(ReturnTypeConst& input)
+ data::Descriptor
+ NlohmannJSONWriter::getDescriptor(ReturnTypeConst& input)
{
return ConstNlohmannJSONVisitor::GetDescriptor(input);
}
- nlohmann::json NlohmannJSONWriter::writeList(const std::vector<nlohmann::json>& elements, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeList(const std::vector<nlohmann::json>& elements, const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::LIST_TYPENAME_SLUG, p);
@@ -55,7 +61,10 @@ namespace armarx::aron::data::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeDict(const std::map<std::string, nlohmann::json>& elements, const std::optional<nlohmann::json>& extends, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeDict(const std::map<std::string, nlohmann::json>& elements,
+ const std::optional<nlohmann::json>& extends,
+ const Path& p)
{
auto o = extends ? extends.value() : nlohmann::json();
@@ -64,21 +73,29 @@ namespace armarx::aron::data::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeNDArray(const std::vector<int>& shape, const std::string& typeAsString, const unsigned char* data, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeNDArray(const std::vector<int>& shape,
+ const std::string& typeAsString,
+ const unsigned char* data,
+ const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::NDARRAY_TYPENAME_SLUG, p);
o[rw::json::constantes::DIMENSIONS_SLUG] = shape;
o[rw::json::constantes::USED_TYPE_SLUG] = typeAsString;
- int elements = shape.empty() ? 0 : std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>());
+ int elements =
+ shape.empty()
+ ? 0
+ : std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>());
std::vector<unsigned char> d = std::vector<unsigned char>(elements);
memcpy(d.data(), data, elements);
o[rw::json::constantes::DATA_SLUG] = d;
return o;
}
- nlohmann::json NlohmannJSONWriter::writeInt(const int i, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeInt(const int i, const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::INT_TYPENAME_SLUG, p);
@@ -86,7 +103,8 @@ namespace armarx::aron::data::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeLong(const long i, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeLong(const long i, const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::LONG_TYPENAME_SLUG, p);
@@ -94,7 +112,8 @@ namespace armarx::aron::data::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeFloat(const float i, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeFloat(const float i, const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::FLOAT_TYPENAME_SLUG, p);
@@ -102,7 +121,8 @@ namespace armarx::aron::data::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeDouble(const double i, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeDouble(const double i, const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::DOUBLE_TYPENAME_SLUG, p);
@@ -110,7 +130,8 @@ namespace armarx::aron::data::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeString(const std::string& i, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeString(const std::string& i, const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::STRING_TYPENAME_SLUG, p);
@@ -118,11 +139,12 @@ namespace armarx::aron::data::writer
return o;
}
- nlohmann::json NlohmannJSONWriter::writeBool(const bool i, const Path& p)
+ nlohmann::json
+ NlohmannJSONWriter::writeBool(const bool i, const Path& p)
{
nlohmann::json o;
setupAronMetaInformationForType(o, rw::json::constantes::BOOL_TYPENAME_SLUG, p);
o[rw::json::constantes::VALUE_SLUG] = i;
return o;
}
-}
+} // namespace armarx::aron::data::writer
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h b/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
index 05be3e9a682dbdd944b8f973a79bc60272e24380..84d92edb8f89953d27e8cf88dcbab2769caea670 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/nlohmannJSON/NlohmannJSONWriter.h
@@ -22,8 +22,8 @@
// STD / STL
#include <memory>
-#include <stack>
#include <sstream>
+#include <stack>
// Simox
#include <SimoxUtility/json.h>
@@ -33,18 +33,23 @@
namespace armarx::aron::data::writer
{
- class NlohmannJSONWriter :
- virtual public WriterInterface<nlohmann::json>
+ class NlohmannJSONWriter : virtual public WriterInterface<nlohmann::json>
{
public:
NlohmannJSONWriter() = default;
data::Descriptor getDescriptor(ReturnTypeConst& input) final;
- nlohmann::json writeList(const std::vector<nlohmann::json>& elements, const Path& p = Path()) override;
- nlohmann::json writeDict(const std::map<std::string, nlohmann::json>& elements, const std::optional<nlohmann::json>& extends = std::nullopt, const Path& p = Path()) override;
+ nlohmann::json writeList(const std::vector<nlohmann::json>& elements,
+ const Path& p = Path()) override;
+ nlohmann::json writeDict(const std::map<std::string, nlohmann::json>& elements,
+ const std::optional<nlohmann::json>& extends = std::nullopt,
+ const Path& p = Path()) override;
- nlohmann::json writeNDArray(const std::vector<int>& shape, const std::string& typeAsString, const unsigned char* data, const Path& p = Path()) override;
+ nlohmann::json writeNDArray(const std::vector<int>& shape,
+ const std::string& typeAsString,
+ const unsigned char* data,
+ const Path& p = Path()) override;
nlohmann::json writeInt(const int i, const Path& p = Path()) override;
nlohmann::json writeLong(const long i, const Path& p = Path()) override;
@@ -53,4 +58,4 @@ namespace armarx::aron::data::writer
nlohmann::json writeString(const std::string& i, const Path& p = Path()) override;
nlohmann::json writeBool(const bool i, const Path& p = Path()) override;
};
-}
+} // namespace armarx::aron::data::writer
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.cpp b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.cpp
index 40098d2e56feffdebc1ad3299eaf3fe557d0405b..b7e8b376c5fceeb4c1b326a1002d0380d912a312 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.cpp
@@ -32,12 +32,14 @@
namespace armarx::aron::data::writer
{
- data::Descriptor VariantWriter::getDescriptor(ReturnTypeConst& input)
+ data::Descriptor
+ VariantWriter::getDescriptor(ReturnTypeConst& input)
{
return ConstVariantVisitor::GetDescriptor(input);
}
- data::VariantPtr VariantWriter::writeList(const std::vector<data::VariantPtr>& elements, const Path& p)
+ data::VariantPtr
+ VariantWriter::writeList(const std::vector<data::VariantPtr>& elements, const Path& p)
{
auto o = std::make_shared<data::List>(p);
for (const auto& el : elements)
@@ -47,66 +49,83 @@ namespace armarx::aron::data::writer
return o;
}
- data::VariantPtr VariantWriter::writeDict(const std::map<std::string, data::VariantPtr>& elements, const std::optional<data::VariantPtr>& extends, const Path& p)
+ data::VariantPtr
+ VariantWriter::writeDict(const std::map<std::string, data::VariantPtr>& elements,
+ const std::optional<data::VariantPtr>& extends,
+ const Path& p)
{
- auto o = extends ? data::Dict::DynamicCastAndCheck(extends.value()) : std::make_shared<data::Dict>(p);
+ auto o = extends ? data::Dict::DynamicCastAndCheck(extends.value())
+ : std::make_shared<data::Dict>(p);
- for(const auto& [key, value] : elements)
+ for (const auto& [key, value] : elements)
{
o->addElement(key, value);
}
return o;
}
- data::VariantPtr VariantWriter::writeNDArray(const std::vector<int>& shape, const std::string& typeAsString, const unsigned char* data, const Path& p)
+ data::VariantPtr
+ VariantWriter::writeNDArray(const std::vector<int>& shape,
+ const std::string& typeAsString,
+ const unsigned char* data,
+ const Path& p)
{
auto o = std::make_shared<data::NDArray>(p);
o->setShape(shape);
o->setType(typeAsString);
- int size = shape.empty() ? 0 : std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>());
+ int size =
+ shape.empty()
+ ? 0
+ : std::accumulate(std::begin(shape), std::end(shape), 1, std::multiplies<int>());
o->setData(static_cast<unsigned int>(size), data);
return o;
}
- data::VariantPtr VariantWriter::writeInt(const int i, const Path& p)
+ data::VariantPtr
+ VariantWriter::writeInt(const int i, const Path& p)
{
auto o = std::make_shared<data::Int>(p);
o->setValue(i);
return o;
}
- data::VariantPtr VariantWriter::writeLong(const long i, const Path& p)
+ data::VariantPtr
+ VariantWriter::writeLong(const long i, const Path& p)
{
auto o = std::make_shared<data::Long>(p);
o->setValue(i);
return o;
}
- data::VariantPtr VariantWriter::writeFloat(const float i, const Path& p)
+ data::VariantPtr
+ VariantWriter::writeFloat(const float i, const Path& p)
{
auto o = std::make_shared<data::Float>(p);
o->setValue(i);
return o;
}
- data::VariantPtr VariantWriter::writeDouble(const double i, const Path& p)
+ data::VariantPtr
+ VariantWriter::writeDouble(const double i, const Path& p)
{
auto o = std::make_shared<data::Double>(p);
o->setValue(i);
return o;
}
- data::VariantPtr VariantWriter::writeString(const std::string& i, const Path& p)
+ data::VariantPtr
+ VariantWriter::writeString(const std::string& i, const Path& p)
{
auto o = std::make_shared<data::String>(p);
o->setValue(i);
return o;
}
- data::VariantPtr VariantWriter::writeBool(const bool i, const Path& p)
+ data::VariantPtr
+ VariantWriter::writeBool(const bool i, const Path& p)
{
auto o = std::make_shared<data::Bool>(p);
o->setValue(i);
return o;
}
-}
+} // namespace armarx::aron::data::writer
diff --git a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
index c2ee596803dde93081f3ff235f2c099a54ced113..5047cd5f197d4d028c9d51f17bd6ade442fc7bdb 100644
--- a/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
+++ b/source/RobotAPI/libraries/aron/core/data/rw/writer/variant/VariantWriter.h
@@ -28,18 +28,23 @@
namespace armarx::aron::data::writer
{
- class VariantWriter :
- public WriterInterface<data::VariantPtr>
+ class VariantWriter : public WriterInterface<data::VariantPtr>
{
public:
VariantWriter() = default;
data::Descriptor getDescriptor(ReturnTypeConst& input) final;
- data::VariantPtr writeList(const std::vector<data::VariantPtr>& elements, const Path& p = Path()) override;
- data::VariantPtr writeDict(const std::map<std::string, data::VariantPtr>& elements, const std::optional<data::VariantPtr>& extends = std::nullopt, const Path& p = Path()) override;
+ data::VariantPtr writeList(const std::vector<data::VariantPtr>& elements,
+ const Path& p = Path()) override;
+ data::VariantPtr writeDict(const std::map<std::string, data::VariantPtr>& elements,
+ const std::optional<data::VariantPtr>& extends = std::nullopt,
+ const Path& p = Path()) override;
- data::VariantPtr writeNDArray(const std::vector<int>& shape, const std::string& typeAsString, const unsigned char* data, const Path& p = Path()) override;
+ data::VariantPtr writeNDArray(const std::vector<int>& shape,
+ const std::string& typeAsString,
+ const unsigned char* data,
+ const Path& p = Path()) override;
data::VariantPtr writeInt(const int i, const Path& p = Path()) override;
data::VariantPtr writeLong(const long i, const Path& p = Path()) override;
@@ -50,4 +55,4 @@ namespace armarx::aron::data::writer
using WriterInterface<data::VariantPtr>::writeDict;
};
-}
+} // namespace armarx::aron::data::writer
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/All.h b/source/RobotAPI/libraries/aron/core/data/variant/All.h
index 90d8058061612af5e948a76712108a2974dd7731..749e93257fbee88185e2a1d6df8ddd52e7443c46 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/All.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/All.h
@@ -1,7 +1,7 @@
#pragma once
-#include "container/All.h"
#include "complex/All.h"
+#include "container/All.h"
#include "primitive/All.h"
/**
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
index 5619e10ad9fba861b617006dadb2d6083d4e28c2..9e4e4edd3feb7a5a6426b19b90ef8f7b9aed9983 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Factory.cpp
@@ -25,6 +25,7 @@
// Header
#include "Factory.h"
+
#include <ArmarXCore/core/logging/Logging.h>
// ArmarX
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Factory.h b/source/RobotAPI/libraries/aron/core/data/variant/Factory.h
index bfb3d09127cee8bfe5297ab9176e993b35878569..6e1bfd83ad0428bea6477c027d8559d64d5da922 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/Factory.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Factory.h
@@ -28,8 +28,8 @@
#include <unordered_map>
// ArmarX
-#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
namespace armarx::aron::data
{
@@ -44,4 +44,4 @@ namespace armarx::aron::data
virtual ~VariantFactory() = default;
};
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/Variant.cpp b/source/RobotAPI/libraries/aron/core/data/variant/Variant.cpp
index ed6b9396cc78aeac9c98e98d28acd932f4d2cf67..2274ad0ba7607998b136ac29413da5c44ab772c7 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/Variant.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/Variant.cpp
@@ -29,19 +29,20 @@
#include "Factory.h"
#include "container/Dict.h"
-
namespace armarx::aron::data
{
// static data members
const VariantFactoryPtr Variant::FACTORY = VariantFactoryPtr(new VariantFactory());
// static methods
- VariantPtr Variant::FromAronDTO(const data::dto::GenericDataPtr& a, const Path& path)
+ VariantPtr
+ Variant::FromAronDTO(const data::dto::GenericDataPtr& a, const Path& path)
{
return FACTORY->create(a, path);
}
- std::vector<VariantPtr> Variant::FromAronDTO(const std::vector<data::dto::GenericDataPtr>& a, const Path& path)
+ std::vector<VariantPtr>
+ Variant::FromAronDTO(const std::vector<data::dto::GenericDataPtr>& a, const Path& path)
{
std::vector<VariantPtr> ret;
for (const auto& aron : a)
@@ -51,7 +52,8 @@ namespace armarx::aron::data
return ret;
}
- std::vector<data::dto::GenericDataPtr> Variant::ToAronDTO(const std::vector<VariantPtr>& a)
+ std::vector<data::dto::GenericDataPtr>
+ Variant::ToAronDTO(const std::vector<VariantPtr>& a)
{
std::vector<data::dto::GenericDataPtr> ret;
for (const auto& v : a)
@@ -67,4 +69,4 @@ namespace armarx::aron::data
}
return ret;
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
index 8bf1da973e412737f8ede1e1aa52376b8b969acc..ff58faee40c8b6e6cf040f160a6027da1733afed 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/complex/NDArray.cpp
@@ -26,6 +26,7 @@
// Simox
#include <SimoxUtility/algorithm/string.h>
+
#include <ArmarXCore/util/CPPUtility/trace.h>
// ArmarX
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.cpp b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.cpp
index 578d58c2b1e84647cb68e05c80963943d0fc8f1e..90c59644028f31b2d2d340fcc4adac1444e77804 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.cpp
@@ -23,4 +23,3 @@
// Header
#include "PrimitiveVariant.h"
-
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/forward_declarations.h b/source/RobotAPI/libraries/aron/core/data/variant/forward_declarations.h
index 45855e03ef9bb364b44da1cc7acd7ec810008a86..cf72bc46765059b124deba1177674b001872ccbf 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/forward_declarations.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/forward_declarations.h
@@ -36,4 +36,4 @@ namespace armarx::aron::data
class Bool;
using BoolPtr = std::shared_ptr<Bool>;
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/All.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/All.h
index c6fcdf6c4405e8ce348a89195b506aedaa030878..77d6abd28e637d5a6220b1c262413182e79c74d2 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/All.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/All.h
@@ -1,11 +1,11 @@
#pragma once
+#include "Bool.h"
+#include "Double.h"
+#include "Float.h"
#include "Int.h"
#include "Long.h"
-#include "Float.h"
-#include "Double.h"
#include "String.h"
-#include "Bool.h"
namespace armarx::aron::data
{
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
index 618be3850da1674379b30f214eab5a7a7484b198..65ff102c18729c385168263d8f9d66d38123c8bb 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.cpp
@@ -22,6 +22,7 @@
*/
#include "Bool.h"
+
#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
index 94349fb2c608936b6e395a1af2090713e8372148..6cb9f49867341bbb499f3e235f15940b80b0724e 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h
@@ -38,8 +38,7 @@ namespace armarx::aron::data
class Bool;
typedef std::shared_ptr<Bool> BoolPtr;
- class Bool :
- public detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>
+ class Bool : public detail::PrimitiveVariant<data::dto::AronBool, bool, Bool>
{
public:
/* constructors */
@@ -69,13 +68,14 @@ namespace armarx::aron::data
type::VariantPtr recalculateType() const override;
bool fullfillsType(const type::VariantPtr&) const override;
};
-}
+} // namespace armarx::aron::data
namespace armarx::aron
{
- template<typename... _Args>
- aron::data::BoolPtr make_bool(_Args&&... args)
+ template <typename... _Args>
+ aron::data::BoolPtr
+ make_bool(_Args&&... args)
{
return std::make_shared<aron::data::Bool>(args...);
}
-}
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
index c118ea66dc523e5fdfe7cea68e95d38d0acca719..06dce5d7d3eb15814af577798e0e7f49d7b175fd 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.cpp
@@ -23,6 +23,7 @@
// Header
#include "Double.h"
+
#include <ArmarXCore/util/CPPUtility/trace.h>
// ArmarX
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
index 59447e7def921da7face8213f24372bf5d5865df..9d8303652e7beaaa14943fe55a9726c9627cb1fc 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Double.h
@@ -38,8 +38,7 @@ namespace armarx::aron::data
class Double;
typedef std::shared_ptr<Double> DoublePtr;
- class Double :
- public detail::PrimitiveVariant<data::dto::AronDouble, double, Double>
+ class Double : public detail::PrimitiveVariant<data::dto::AronDouble, double, Double>
{
public:
/* constructors */
@@ -69,13 +68,14 @@ namespace armarx::aron::data
type::VariantPtr recalculateType() const override;
bool fullfillsType(const type::VariantPtr&) const override;
};
-}
+} // namespace armarx::aron::data
namespace armarx::aron
{
- template<typename... _Args>
- aron::data::DoublePtr make_double(_Args&&... args)
+ template <typename... _Args>
+ aron::data::DoublePtr
+ make_double(_Args&&... args)
{
return std::make_shared<aron::data::Double>(args...);
}
-}
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
index 6ebe3a5a42544573ba348ecee8c1768cff956c9b..658502909dbc75eb240712e35d76d6ed1f60f7ba 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.cpp
@@ -22,6 +22,7 @@
*/
#include "Float.h"
+
#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
index 96f84a14c7978ab6675661c6d5fab261e0ad3b92..e3df70f39c2ebfaee8d474812f30aef4cc98fb49 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Float.h
@@ -38,8 +38,7 @@ namespace armarx::aron::data
class Float;
typedef std::shared_ptr<Float> FloatPtr;
- class Float :
- public detail::PrimitiveVariant<data::dto::AronFloat, float, Float>
+ class Float : public detail::PrimitiveVariant<data::dto::AronFloat, float, Float>
{
public:
/* constructors */
@@ -69,13 +68,14 @@ namespace armarx::aron::data
type::VariantPtr recalculateType() const override;
bool fullfillsType(const type::VariantPtr&) const override;
};
-}
+} // namespace armarx::aron::data
namespace armarx::aron
{
- template<typename... _Args>
- aron::data::FloatPtr make_float(_Args&&... args)
+ template <typename... _Args>
+ aron::data::FloatPtr
+ make_float(_Args&&... args)
{
return std::make_shared<aron::data::Float>(args...);
}
-}
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
index 18bb4ed52fd1d1bf692c7d95edb7b2da6dc19163..977eabda3ebeb06b0d6242802fafdbe11b02b6da 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.cpp
@@ -22,6 +22,7 @@
*/
#include "Int.h"
+
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/libraries/aron/core/data/variant/Factory.h>
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
index 738496f71bc51b4f0bdca92f57405a84a0dc2d22..89d525a38b042ef790326fa398b08605a63d99ef 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Int.h
@@ -30,8 +30,8 @@
#include "../detail/PrimitiveVariant.h"
// ArmarX
-#include "../../../type/variant/primitive/Int.h"
#include "../../../type/variant/enum/IntEnum.h"
+#include "../../../type/variant/primitive/Int.h"
namespace armarx::aron::data
{
@@ -39,8 +39,7 @@ namespace armarx::aron::data
class Int;
typedef std::shared_ptr<Int> IntPtr;
- class Int :
- public detail::PrimitiveVariant<data::dto::AronInt, int, Int>
+ class Int : public detail::PrimitiveVariant<data::dto::AronInt, int, Int>
{
public:
/* constructors */
@@ -70,13 +69,14 @@ namespace armarx::aron::data
type::VariantPtr recalculateType() const override;
bool fullfillsType(const type::VariantPtr&) const override;
};
-}
+} // namespace armarx::aron::data
namespace armarx::aron
{
- template<typename... _Args>
- aron::data::IntPtr make_int(_Args&&... args)
+ template <typename... _Args>
+ aron::data::IntPtr
+ make_int(_Args&&... args)
{
return std::make_shared<aron::data::Int>(args...);
}
-}
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
index 4fc08e880b209a284bd4e4461a4633a6d6ee0fc2..454d47a87041465cce879ba69bfc671f37e8c3c9 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.cpp
@@ -23,6 +23,7 @@
// Header
#include "Long.h"
+
#include <ArmarXCore/util/CPPUtility/trace.h>
// ArmarX
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
index c28b603aa93f3b23750a310e84e9bcc6a9e7d501..9f3587e17731f721cd68119f29c5b9fd7cd3fff9 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/Long.h
@@ -38,8 +38,7 @@ namespace armarx::aron::data
class Long;
typedef std::shared_ptr<Long> LongPtr;
- class Long :
- public detail::PrimitiveVariant<data::dto::AronLong, long, Long>
+ class Long : public detail::PrimitiveVariant<data::dto::AronLong, long, Long>
{
public:
/* constructors */
@@ -69,13 +68,14 @@ namespace armarx::aron::data
type::VariantPtr recalculateType() const override;
bool fullfillsType(const type::VariantPtr&) const override;
};
-}
+} // namespace armarx::aron::data
namespace armarx::aron
{
- template<typename... _Args>
- aron::data::LongPtr make_long(_Args&&... args)
+ template <typename... _Args>
+ aron::data::LongPtr
+ make_long(_Args&&... args)
{
return std::make_shared<aron::data::Long>(args...);
}
-}
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
index 675a15cbab1c6ac2b5aec75fb742f255c7a1dd26..e02a9d18c09a9670ad5899c4f279c5c710497146 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.cpp
@@ -22,6 +22,7 @@
*/
#include "String.h"
+
#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
index def1c80315934c55073314483fcda063fbd50e76..d974fb6047e64f2f2fb6bb62324665f4c62abc2d 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/primitive/String.h
@@ -38,8 +38,7 @@ namespace armarx::aron::data
class String;
typedef std::shared_ptr<String> StringPtr;
- class String :
- public detail::PrimitiveVariant<data::dto::AronString, std::string, String>
+ class String : public detail::PrimitiveVariant<data::dto::AronString, std::string, String>
{
public:
/* constructors */
@@ -68,13 +67,14 @@ namespace armarx::aron::data
type::VariantPtr recalculateType() const override;
bool fullfillsType(const type::VariantPtr&) const override;
};
-}
+} // namespace armarx::aron::data
namespace armarx::aron
{
- template<typename... _Args>
- aron::data::StringPtr make_string(_Args&&... args)
+ template <typename... _Args>
+ aron::data::StringPtr
+ make_string(_Args&&... args)
{
return std::make_shared<aron::data::String>(args...);
}
-}
+} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h b/source/RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h
index b9a817f59a692b2b1e288391ff9f7e438eb42002..eac2adf84128bf77e30b7e3efdd689c333138a9f 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/RecursiveVisitor.h
@@ -45,29 +45,35 @@ namespace armarx::aron::data
virtual MapElements getDictElements(Input&) = 0;
virtual ListElements getListElements(Input&) = 0;
- virtual void visitDictOnEnter(Input& element) {};
- virtual void visitDictOnExit(Input& element) {};
- virtual void visitListOnEnter(Input& element) {};
- virtual void visitListOnExit(Input& element) {};
+ virtual void visitDictOnEnter(Input& element){};
+ virtual void visitDictOnExit(Input& element){};
+ virtual void visitListOnEnter(Input& element){};
+ virtual void visitListOnExit(Input& element){};
- virtual void visitNDArray(Input& element) {};
- virtual void visitInt(Input& element) {};
- virtual void visitLong(Input& element) {};
- virtual void visitFloat(Input& element) {};
- virtual void visitDouble(Input& element) {};
- virtual void visitBool(Input& element) {};
- virtual void visitString(Input& element) {};
- virtual void visitUnknown(Input& element)
+ virtual void visitNDArray(Input& element){};
+ virtual void visitInt(Input& element){};
+ virtual void visitLong(Input& element){};
+ virtual void visitFloat(Input& element){};
+ virtual void visitDouble(Input& element){};
+ virtual void visitBool(Input& element){};
+ virtual void visitString(Input& element){};
+
+ virtual void
+ visitUnknown(Input& element)
{
if (!element)
{
- throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor. The element was NULL.");
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "Unknown type in visitor. The element was NULL.");
}
else
{
- throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor. The element was set but descriptor was undefined.");
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Unknown type in visitor. The element was set but descriptor was undefined.");
}
}
+
virtual ~RecursiveVisitor() = default;
};
@@ -85,7 +91,8 @@ namespace armarx::aron::data
using MapElements = std::map<std::string, std::pair<DataInputNonConst, TypeInputNonConst>>;
using ListElements = std::vector<std::pair<DataInputNonConst, TypeInputNonConst>>;
- using PairElements = std::pair<std::pair<DataInputNonConst, TypeInputNonConst>, std::pair<DataInputNonConst, TypeInputNonConst>>;
+ using PairElements = std::pair<std::pair<DataInputNonConst, TypeInputNonConst>,
+ std::pair<DataInputNonConst, TypeInputNonConst>>;
using TupleElements = std::vector<std::pair<DataInputNonConst, TypeInputNonConst>>;
virtual MapElements getObjectElements(DataInput&, TypeInput&) = 0;
@@ -94,41 +101,47 @@ namespace armarx::aron::data
virtual PairElements getPairElements(DataInput&, TypeInput&) = 0;
virtual TupleElements getTupleElements(DataInput&, TypeInput&) = 0;
- virtual void visitObjectOnEnter(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitObjectOnExit(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitDictOnEnter(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitDictOnExit(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitPairOnEnter(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitPairOnExit(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitTupleOnEnter(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitTupleOnExit(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitListOnEnter(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitListOnExit(DataInput& elementData, TypeInput& elementType) {};
+ virtual void visitObjectOnEnter(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitObjectOnExit(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitDictOnEnter(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitDictOnExit(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitPairOnEnter(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitPairOnExit(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitTupleOnEnter(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitTupleOnExit(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitListOnEnter(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitListOnExit(DataInput& elementData, TypeInput& elementType){};
- virtual void visitMatrix(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitNDArray(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitQuaternion(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitImage(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitPointCloud(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitIntEnum(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitInt(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitLong(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitFloat(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitDouble(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitBool(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitString(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitAnyObject(DataInput& elementData, TypeInput& elementType) {};
- virtual void visitUnknown(DataInput& elementData, TypeInput& elementType)
+ virtual void visitMatrix(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitNDArray(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitQuaternion(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitImage(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitPointCloud(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitIntEnum(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitInt(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitLong(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitFloat(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitDouble(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitBool(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitString(DataInput& elementData, TypeInput& elementType){};
+ virtual void visitAnyObject(DataInput& elementData, TypeInput& elementType){};
+
+ virtual void
+ visitUnknown(DataInput& elementData, TypeInput& elementType)
{
if (!elementData)
{
- throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor. The element was NULL.");
+ throw error::AronException(__PRETTY_FUNCTION__,
+ "Unknown type in visitor. The element was NULL.");
}
else
{
- throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor. The element was set but descriptor was undefined.");
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Unknown type in visitor. The element was set but descriptor was undefined.");
}
}
+
virtual ~RecursiveTypedVisitor() = default;
};
@@ -142,8 +155,11 @@ namespace armarx::aron::data
* @see type/visitor/RecursiveVisitor.h
*/
template <class RecursiveVisitorImplementation>
- requires isRecursiveVisitor<RecursiveVisitorImplementation, typename RecursiveVisitorImplementation::Input>
- void visitRecursive(RecursiveVisitorImplementation& v, typename RecursiveVisitorImplementation::Input& o)
+ requires isRecursiveVisitor<RecursiveVisitorImplementation,
+ typename RecursiveVisitorImplementation::Input>
+ void
+ visitRecursive(RecursiveVisitorImplementation& v,
+ typename RecursiveVisitorImplementation::Input& o)
{
data::Descriptor descriptor = v.getDescriptor(o);
switch (descriptor)
@@ -194,8 +210,13 @@ namespace armarx::aron::data
* @see data/visitor/Visitor.h
*/
template <class RecursiveVisitorImplementation>
- requires isRecursiveTypedVisitor<RecursiveVisitorImplementation, typename RecursiveVisitorImplementation::DataInput, typename RecursiveVisitorImplementation::TypeInput>
- void visitRecursive(RecursiveVisitorImplementation& v, typename RecursiveVisitorImplementation::DataInput& o, typename RecursiveVisitorImplementation::TypeInput& t)
+ requires isRecursiveTypedVisitor<RecursiveVisitorImplementation,
+ typename RecursiveVisitorImplementation::DataInput,
+ typename RecursiveVisitorImplementation::TypeInput>
+ void
+ visitRecursive(RecursiveVisitorImplementation& v,
+ typename RecursiveVisitorImplementation::DataInput& o,
+ typename RecursiveVisitorImplementation::TypeInput& t)
{
type::Descriptor descriptor = v.getDescriptor(o, t);
switch (descriptor)
@@ -237,13 +258,13 @@ namespace armarx::aron::data
}
case type::Descriptor::DICT:
{
- v.visitDictOnEnter(o, t);
- for (auto& [key, pair] : v.getDictElements(o, t))
- {
- visitRecursive(v, pair.first, pair.second);
- }
- v.visitDictOnExit(o, t);
- return;
+ v.visitDictOnEnter(o, t);
+ for (auto& [key, pair] : v.getDictElements(o, t))
+ {
+ visitRecursive(v, pair.first, pair.second);
+ }
+ v.visitDictOnExit(o, t);
+ return;
}
case type::Descriptor::OBJECT:
{
@@ -285,4 +306,4 @@ namespace armarx::aron::data
return v.visitUnknown(o, t);
}
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/Visitor.h b/source/RobotAPI/libraries/aron/core/data/visitor/Visitor.h
index d7c62f104cbc3b97b9162b37b08d59cc3740f425..cc6603ed226768d5d5ee466c44a5b6c4d42fe9d5 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/Visitor.h
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/Visitor.h
@@ -65,16 +65,22 @@ namespace armarx::aron::data
{
using Input = typename VisitorBase<T>::Input;
- virtual void visitDict(Input&) {};
- virtual void visitList(Input&) {};
- virtual void visitNDArray(Input&) {};
- virtual void visitInt(Input&) {};
- virtual void visitLong(Input&) {};
- virtual void visitFloat(Input&) {};
- virtual void visitDouble(Input&) {};
- virtual void visitBool(Input&) {};
- virtual void visitString(Input&) {};
- virtual void visitUnknown(Input&) { throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor."); }
+ virtual void visitDict(Input&){};
+ virtual void visitList(Input&){};
+ virtual void visitNDArray(Input&){};
+ virtual void visitInt(Input&){};
+ virtual void visitLong(Input&){};
+ virtual void visitFloat(Input&){};
+ virtual void visitDouble(Input&){};
+ virtual void visitBool(Input&){};
+ virtual void visitString(Input&){};
+
+ virtual void
+ visitUnknown(Input&)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor.");
+ }
+
virtual ~Visitor() = default;
};
@@ -87,25 +93,31 @@ namespace armarx::aron::data
using DataInput = typename TypedVisitorBase<T1, T2>::DataInput;
using TypeInput = typename TypedVisitorBase<T1, T2>::TypeInput;
- virtual void visitObject(DataInput&, TypeInput&) {};
- virtual void visitDict(DataInput&, TypeInput&) {};
- virtual void visitPair(DataInput&, TypeInput&) {};
- virtual void visitTuple(DataInput&, TypeInput&) {};
- virtual void visitList(DataInput&, TypeInput&) {};
- virtual void visitMatrix(DataInput&, TypeInput&) {};
- virtual void visitNDArray(DataInput&, TypeInput&) {};
- virtual void visitQuaternion(DataInput&, TypeInput&) {};
- virtual void visitImage(DataInput&, TypeInput&) {};
- virtual void visitPointCloud(DataInput&, TypeInput&) {};
- virtual void visitIntEnum(DataInput&, TypeInput&) {};
- virtual void visitInt(DataInput&, TypeInput&) {};
- virtual void visitLong(DataInput&, TypeInput&) {};
- virtual void visitFloat(DataInput&, TypeInput&) {};
- virtual void visitDouble(DataInput&, TypeInput&) {};
- virtual void visitBool(DataInput&, TypeInput&) {};
- virtual void visitString(DataInput&, TypeInput&) {};
- virtual void visitAnyObject(DataInput&, TypeInput&) {};
- virtual void visitUnknown(DataInput&, TypeInput&) { throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor."); }
+ virtual void visitObject(DataInput&, TypeInput&){};
+ virtual void visitDict(DataInput&, TypeInput&){};
+ virtual void visitPair(DataInput&, TypeInput&){};
+ virtual void visitTuple(DataInput&, TypeInput&){};
+ virtual void visitList(DataInput&, TypeInput&){};
+ virtual void visitMatrix(DataInput&, TypeInput&){};
+ virtual void visitNDArray(DataInput&, TypeInput&){};
+ virtual void visitQuaternion(DataInput&, TypeInput&){};
+ virtual void visitImage(DataInput&, TypeInput&){};
+ virtual void visitPointCloud(DataInput&, TypeInput&){};
+ virtual void visitIntEnum(DataInput&, TypeInput&){};
+ virtual void visitInt(DataInput&, TypeInput&){};
+ virtual void visitLong(DataInput&, TypeInput&){};
+ virtual void visitFloat(DataInput&, TypeInput&){};
+ virtual void visitDouble(DataInput&, TypeInput&){};
+ virtual void visitBool(DataInput&, TypeInput&){};
+ virtual void visitString(DataInput&, TypeInput&){};
+ virtual void visitAnyObject(DataInput&, TypeInput&){};
+
+ virtual void
+ visitUnknown(DataInput&, TypeInput&)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor.");
+ }
+
virtual ~TypedVisitor() = default;
};
@@ -115,13 +127,13 @@ namespace armarx::aron::data
template <class T, class Data, class Type>
concept isTypedVisitor = std::is_base_of<TypedVisitor<Data, Type>, T>::value;
-
/**
* @see type/visitor/Visitor.h
*/
template <class VisitorImplementation>
- requires isVisitor<VisitorImplementation, typename VisitorImplementation::Input>
- void visit(VisitorImplementation& v, typename VisitorImplementation::Input& o)
+ requires isVisitor<VisitorImplementation, typename VisitorImplementation::Input>
+ void
+ visit(VisitorImplementation& v, typename VisitorImplementation::Input& o)
{
auto descriptor = v.getDescriptor(o);
switch (descriptor)
@@ -155,50 +167,55 @@ namespace armarx::aron::data
* Does NOT check if the data and type representation match!
*/
template <class VisitorImplementation>
- requires isTypedVisitor<VisitorImplementation, typename VisitorImplementation::DataInput, typename VisitorImplementation::TypeInput>
- void visit(VisitorImplementation& v, typename VisitorImplementation::DataInput& o, typename VisitorImplementation::TypeInput& t)
+ requires isTypedVisitor<VisitorImplementation,
+ typename VisitorImplementation::DataInput,
+ typename VisitorImplementation::TypeInput>
+ void
+ visit(VisitorImplementation& v,
+ typename VisitorImplementation::DataInput& o,
+ typename VisitorImplementation::TypeInput& t)
{
auto descriptor = v.getDescriptor(o, t);
switch (descriptor)
{
- case type::Descriptor::OBJECT:
- return v.visitObject(o, t);
- case type::Descriptor::LIST:
- return v.visitList(o, t);
- case type::Descriptor::DICT:
- return v.visitDict(o, t);
- case type::Descriptor::PAIR:
- return v.visitPair(o, t);
- case type::Descriptor::TUPLE:
- return v.visitTuple(o, t);
- case type::Descriptor::NDARRAY:
- return v.visitNDArray(o, t);
- case type::Descriptor::MATRIX:
- return v.visitMatrix(o, t);
- case type::Descriptor::IMAGE:
- return v.visitImage(o, t);
- case type::Descriptor::POINTCLOUD:
- return v.visitPointCloud(o, t);
- case type::Descriptor::QUATERNION:
- return v.visitQuaternion(o, t);
- case type::Descriptor::INT:
- return v.visitInt(o, t);
- case type::Descriptor::LONG:
- return v.visitLong(o, t);
- case type::Descriptor::FLOAT:
- return v.visitFloat(o, t);
- case type::Descriptor::DOUBLE:
- return v.visitDouble(o, t);
- case type::Descriptor::STRING:
- return v.visitString(o, t);
- case type::Descriptor::BOOL:
- return v.visitBool(o, t);
- case type::Descriptor::ANY_OBJECT:
- return v.visitAnyObject(o, t);
- case type::Descriptor::INT_ENUM:
- return v.visitIntEnum(o, t);
- case type::Descriptor::UNKNOWN:
- return v.visitUnknown(o, t);
+ case type::Descriptor::OBJECT:
+ return v.visitObject(o, t);
+ case type::Descriptor::LIST:
+ return v.visitList(o, t);
+ case type::Descriptor::DICT:
+ return v.visitDict(o, t);
+ case type::Descriptor::PAIR:
+ return v.visitPair(o, t);
+ case type::Descriptor::TUPLE:
+ return v.visitTuple(o, t);
+ case type::Descriptor::NDARRAY:
+ return v.visitNDArray(o, t);
+ case type::Descriptor::MATRIX:
+ return v.visitMatrix(o, t);
+ case type::Descriptor::IMAGE:
+ return v.visitImage(o, t);
+ case type::Descriptor::POINTCLOUD:
+ return v.visitPointCloud(o, t);
+ case type::Descriptor::QUATERNION:
+ return v.visitQuaternion(o, t);
+ case type::Descriptor::INT:
+ return v.visitInt(o, t);
+ case type::Descriptor::LONG:
+ return v.visitLong(o, t);
+ case type::Descriptor::FLOAT:
+ return v.visitFloat(o, t);
+ case type::Descriptor::DOUBLE:
+ return v.visitDouble(o, t);
+ case type::Descriptor::STRING:
+ return v.visitString(o, t);
+ case type::Descriptor::BOOL:
+ return v.visitBool(o, t);
+ case type::Descriptor::ANY_OBJECT:
+ return v.visitAnyObject(o, t);
+ case type::Descriptor::INT_ENUM:
+ return v.visitIntEnum(o, t);
+ case type::Descriptor::UNKNOWN:
+ return v.visitUnknown(o, t);
}
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
index e80e7d37e071585f8b6f3ae0e7d0f81d280b49e4..c9f23e01435d3edbe21e187a934f1cbd56058783 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
@@ -26,7 +26,8 @@
namespace armarx::aron::data
{
- data::Descriptor ConstNlohmannJSONVisitor::GetDescriptor(Input& n)
+ data::Descriptor
+ ConstNlohmannJSONVisitor::GetDescriptor(Input& n)
{
if (n == nlohmann::json())
{
@@ -37,8 +38,9 @@ namespace armarx::aron::data
return armarx::aron::data::rw::json::conversion::String2Descriptor.at(t);
}
- data::Descriptor ConstNlohmannJSONVisitor::getDescriptor(Input& n)
+ data::Descriptor
+ ConstNlohmannJSONVisitor::getDescriptor(Input& n)
{
return GetDescriptor(n);
}
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h b/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h
index 1b83bc3ca472d490c894d1dd9f94e50beec723e7..10c4ae96de322430689fe84d0992b6adf99f6884 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/nlohmannJSON/NlohmannJSONVisitor.h
@@ -30,8 +30,8 @@
// SImox
#include <SimoxUtility/json.h>
-#include "../Visitor.h"
#include "../../rw/json/Data.h"
+#include "../Visitor.h"
namespace armarx::aron::data
{
@@ -46,4 +46,4 @@ namespace armarx::aron::data
};
// TODO
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
index d060c70606a439a9b7abe8ec03f89c565aefaa17..9688a04e1e8ebcc6cb16c503b5c52d504f711ba2 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.cpp
@@ -24,15 +24,16 @@
// Header
#include "VariantVisitor.h"
-#include "../../variant/All.h"
#include "../../../type/visitor/variant/VariantVisitor.h"
+#include "../../variant/All.h"
namespace armarx::aron::data
{
/****************************************************************************
* ConstVariantVisitor
***************************************************************************/
- data::Descriptor ConstVariantVisitor::GetDescriptor(Input& n)
+ data::Descriptor
+ ConstVariantVisitor::GetDescriptor(Input& n)
{
if (!n)
{
@@ -41,80 +42,125 @@ namespace armarx::aron::data
return n->getDescriptor();
}
- data::Descriptor ConstVariantVisitor::getDescriptor(Input& n)
+ data::Descriptor
+ ConstVariantVisitor::getDescriptor(Input& n)
{
return GetDescriptor(n);
}
- void ConstVariantVisitor::visitDict(Input& i)
+ void
+ ConstVariantVisitor::visitDict(Input& i)
{
auto aron = data::Dict::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void ConstVariantVisitor::visitList(Input& i)
+ void
+ ConstVariantVisitor::visitList(Input& i)
{
auto aron = data::List::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void ConstVariantVisitor::visitNDArray(Input& i)
+ void
+ ConstVariantVisitor::visitNDArray(Input& i)
{
auto aron = data::NDArray::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void ConstVariantVisitor::visitInt(Input& i)
+ void
+ ConstVariantVisitor::visitInt(Input& i)
{
auto aron = data::Int::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void ConstVariantVisitor::visitLong(Input& i)
+ void
+ ConstVariantVisitor::visitLong(Input& i)
{
auto aron = data::Long::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void ConstVariantVisitor::visitFloat(Input& i)
+ void
+ ConstVariantVisitor::visitFloat(Input& i)
{
auto aron = data::Float::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void ConstVariantVisitor::visitDouble(Input& i)
+ void
+ ConstVariantVisitor::visitDouble(Input& i)
{
auto aron = data::Double::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void ConstVariantVisitor::visitBool(Input& i)
+ void
+ ConstVariantVisitor::visitBool(Input& i)
{
auto aron = data::Bool::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void ConstVariantVisitor::visitString(Input& i)
+ void
+ ConstVariantVisitor::visitString(Input& i)
{
auto aron = data::String::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void ConstVariantVisitor::visitAronVariant(const data::DictPtr&) {}
- void ConstVariantVisitor::visitAronVariant(const data::ListPtr&) {}
- void ConstVariantVisitor::visitAronVariant(const data::NDArrayPtr&) {}
- void ConstVariantVisitor::visitAronVariant(const data::IntPtr&) {}
- void ConstVariantVisitor::visitAronVariant(const data::LongPtr&) {}
- void ConstVariantVisitor::visitAronVariant(const data::FloatPtr&) {}
- void ConstVariantVisitor::visitAronVariant(const data::DoublePtr&) {}
- void ConstVariantVisitor::visitAronVariant(const data::BoolPtr&) {}
- void ConstVariantVisitor::visitAronVariant(const data::StringPtr&) {}
+ void
+ ConstVariantVisitor::visitAronVariant(const data::DictPtr&)
+ {
+ }
+
+ void
+ ConstVariantVisitor::visitAronVariant(const data::ListPtr&)
+ {
+ }
+
+ void
+ ConstVariantVisitor::visitAronVariant(const data::NDArrayPtr&)
+ {
+ }
+
+ void
+ ConstVariantVisitor::visitAronVariant(const data::IntPtr&)
+ {
+ }
+
+ void
+ ConstVariantVisitor::visitAronVariant(const data::LongPtr&)
+ {
+ }
+ void
+ ConstVariantVisitor::visitAronVariant(const data::FloatPtr&)
+ {
+ }
+
+ void
+ ConstVariantVisitor::visitAronVariant(const data::DoublePtr&)
+ {
+ }
+
+ void
+ ConstVariantVisitor::visitAronVariant(const data::BoolPtr&)
+ {
+ }
+
+ void
+ ConstVariantVisitor::visitAronVariant(const data::StringPtr&)
+ {
+ }
/****************************************************************************
* ConstTypedVariantVisitor
***************************************************************************/
- type::Descriptor ConstTypedVariantVisitor::GetDescriptor(DataInput& i, TypeInput& j)
+ type::Descriptor
+ ConstTypedVariantVisitor::GetDescriptor(DataInput& i, TypeInput& j)
{
auto t_desc = type::ConstVariantVisitor::GetDescriptor(j);
if (t_desc == type::Descriptor::UNKNOWN)
@@ -125,361 +171,558 @@ namespace armarx::aron::data
return t_desc;
}
- type::Descriptor ConstTypedVariantVisitor::getDescriptor(DataInput& i, TypeInput& n)
+ type::Descriptor
+ ConstTypedVariantVisitor::getDescriptor(DataInput& i, TypeInput& n)
{
return GetDescriptor(i, n);
}
- void ConstTypedVariantVisitor::visitObject(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitObject(DataInput& i, TypeInput& j)
{
auto d = data::Dict::DynamicCastAndCheck(i);
auto t = type::Object::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitDict(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitDict(DataInput& i, TypeInput& j)
{
auto d = data::Dict::DynamicCastAndCheck(i);
auto t = type::Dict::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitPair(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitPair(DataInput& i, TypeInput& j)
{
auto d = data::List::DynamicCastAndCheck(i);
auto t = type::Pair::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitTuple(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitTuple(DataInput& i, TypeInput& j)
{
auto d = data::List::DynamicCastAndCheck(i);
auto t = type::Tuple::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitList(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitList(DataInput& i, TypeInput& j)
{
auto d = data::List::DynamicCastAndCheck(i);
auto t = type::List::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitMatrix(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitMatrix(DataInput& i, TypeInput& j)
{
auto d = data::NDArray::DynamicCastAndCheck(i);
auto t = type::Matrix::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitNDArray(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitNDArray(DataInput& i, TypeInput& j)
{
auto d = data::NDArray::DynamicCastAndCheck(i);
auto t = type::NDArray::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitQuaternion(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitQuaternion(DataInput& i, TypeInput& j)
{
auto d = data::NDArray::DynamicCastAndCheck(i);
auto t = type::Quaternion::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitImage(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitImage(DataInput& i, TypeInput& j)
{
auto d = data::NDArray::DynamicCastAndCheck(i);
auto t = type::Image::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitPointCloud(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitPointCloud(DataInput& i, TypeInput& j)
{
auto d = data::NDArray::DynamicCastAndCheck(i);
auto t = type::PointCloud::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitIntEnum(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitIntEnum(DataInput& i, TypeInput& j)
{
auto d = data::Int::DynamicCastAndCheck(i);
auto t = type::IntEnum::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitInt(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitInt(DataInput& i, TypeInput& j)
{
auto d = data::Int::DynamicCastAndCheck(i);
auto t = type::Int::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitLong(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitLong(DataInput& i, TypeInput& j)
{
auto d = data::Long::DynamicCastAndCheck(i);
auto t = type::Long::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitFloat(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitFloat(DataInput& i, TypeInput& j)
{
auto d = data::Float::DynamicCastAndCheck(i);
auto t = type::Float::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitDouble(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitDouble(DataInput& i, TypeInput& j)
{
auto d = data::Double::DynamicCastAndCheck(i);
auto t = type::Double::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitBool(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitBool(DataInput& i, TypeInput& j)
{
auto d = data::Bool::DynamicCastAndCheck(i);
auto t = type::Bool::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitString(DataInput& i, TypeInput& j)
+ void
+ ConstTypedVariantVisitor::visitString(DataInput& i, TypeInput& j)
{
auto d = data::String::DynamicCastAndCheck(i);
auto t = type::String::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void ConstTypedVariantVisitor::visitAronVariant(const data::DictPtr&, const type::ObjectPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::DictPtr&, const type::DictPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::ListPtr&, const type::ListPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::ListPtr&, const type::PairPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::ListPtr&, const type::TuplePtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::MatrixPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::NDArrayPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::QuaternionPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::PointCloudPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::ImagePtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::IntPtr&, const type::IntEnumPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::IntPtr&, const type::IntPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::LongPtr&, const type::LongPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::FloatPtr&, const type::FloatPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::DoublePtr&, const type::DoublePtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::BoolPtr&, const type::BoolPtr&) {}
- void ConstTypedVariantVisitor::visitAronVariant(const data::StringPtr&, const type::StringPtr&) {}
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::DictPtr&, const type::ObjectPtr&)
+ {
+ }
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::DictPtr&, const type::DictPtr&)
+ {
+ }
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::ListPtr&, const type::ListPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::ListPtr&, const type::PairPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::ListPtr&, const type::TuplePtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::MatrixPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::NDArrayPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::QuaternionPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::PointCloudPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::ImagePtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::IntPtr&, const type::IntEnumPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::IntPtr&, const type::IntPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::LongPtr&, const type::LongPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::FloatPtr&, const type::FloatPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::DoublePtr&, const type::DoublePtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::BoolPtr&, const type::BoolPtr&)
+ {
+ }
+
+ void
+ ConstTypedVariantVisitor::visitAronVariant(const data::StringPtr&, const type::StringPtr&)
+ {
+ }
/****************************************************************************
* RecursiveConstVariantVisitor
***************************************************************************/
- data::Descriptor RecursiveConstVariantVisitor::getDescriptor(Input& n)
+ data::Descriptor
+ RecursiveConstVariantVisitor::getDescriptor(Input& n)
{
return ConstVariantVisitor::GetDescriptor(n);
}
- RecursiveConstVariantVisitor::MapElements RecursiveConstVariantVisitor::GetDictElements(Input& n)
+ RecursiveConstVariantVisitor::MapElements
+ RecursiveConstVariantVisitor::GetDictElements(Input& n)
{
auto x = data::Dict::DynamicCastAndCheck(n);
return x->getElements();
}
- RecursiveConstVariantVisitor::MapElements RecursiveConstVariantVisitor::getDictElements(Input& n)
+ RecursiveConstVariantVisitor::MapElements
+ RecursiveConstVariantVisitor::getDictElements(Input& n)
{
return GetDictElements(n);
}
- RecursiveConstVariantVisitor::ListElements RecursiveConstVariantVisitor::GetListElements(Input& n)
+ RecursiveConstVariantVisitor::ListElements
+ RecursiveConstVariantVisitor::GetListElements(Input& n)
{
auto x = data::List::DynamicCastAndCheck(n);
return x->getElements();
}
- RecursiveConstVariantVisitor::ListElements RecursiveConstVariantVisitor::getListElements(Input& n)
+ RecursiveConstVariantVisitor::ListElements
+ RecursiveConstVariantVisitor::getListElements(Input& n)
{
return GetListElements(n);
}
- void RecursiveConstVariantVisitor::visitDictOnEnter(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitDictOnEnter(Input& i)
{
auto aron = data::Dict::DynamicCastAndCheck(i);
visitAronVariantOnEnter(aron);
}
- void RecursiveConstVariantVisitor::visitDictOnExit(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitDictOnExit(Input& i)
{
auto aron = data::Dict::DynamicCastAndCheck(i);
visitAronVariantOnExit(aron);
}
- void RecursiveConstVariantVisitor::visitListOnEnter(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitListOnEnter(Input& i)
{
auto aron = data::List::DynamicCastAndCheck(i);
visitAronVariantOnEnter(aron);
}
- void RecursiveConstVariantVisitor::visitListOnExit(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitListOnExit(Input& i)
{
auto aron = data::List::DynamicCastAndCheck(i);
visitAronVariantOnExit(aron);
}
- void RecursiveConstVariantVisitor::visitNDArray(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitNDArray(Input& i)
{
auto aron = data::NDArray::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveConstVariantVisitor::visitInt(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitInt(Input& i)
{
auto aron = data::Int::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveConstVariantVisitor::visitLong(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitLong(Input& i)
{
auto aron = data::Long::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveConstVariantVisitor::visitFloat(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitFloat(Input& i)
{
auto aron = data::Float::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveConstVariantVisitor::visitDouble(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitDouble(Input& i)
{
auto aron = data::Double::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveConstVariantVisitor::visitBool(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitBool(Input& i)
{
auto aron = data::Bool::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveConstVariantVisitor::visitString(Input& i)
+ void
+ RecursiveConstVariantVisitor::visitString(Input& i)
{
auto aron = data::String::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveConstVariantVisitor::visitAronVariantOnEnter(const data::DictPtr&) {}
- void RecursiveConstVariantVisitor::visitAronVariantOnEnter(const data::ListPtr&) {}
- void RecursiveConstVariantVisitor::visitAronVariantOnExit(const data::DictPtr&) {}
- void RecursiveConstVariantVisitor::visitAronVariantOnExit(const data::ListPtr&) {}
- void RecursiveConstVariantVisitor::visitAronVariant(const data::NDArrayPtr&) {}
- void RecursiveConstVariantVisitor::visitAronVariant(const data::IntPtr&) {}
- void RecursiveConstVariantVisitor::visitAronVariant(const data::LongPtr&) {}
- void RecursiveConstVariantVisitor::visitAronVariant(const data::FloatPtr&) {}
- void RecursiveConstVariantVisitor::visitAronVariant(const data::DoublePtr&) {}
- void RecursiveConstVariantVisitor::visitAronVariant(const data::BoolPtr&) {}
- void RecursiveConstVariantVisitor::visitAronVariant(const data::StringPtr&) {}
+ void
+ RecursiveConstVariantVisitor::visitAronVariantOnEnter(const data::DictPtr&)
+ {
+ }
+
+ void
+ RecursiveConstVariantVisitor::visitAronVariantOnEnter(const data::ListPtr&)
+ {
+ }
+
+ void
+ RecursiveConstVariantVisitor::visitAronVariantOnExit(const data::DictPtr&)
+ {
+ }
+
+ void
+ RecursiveConstVariantVisitor::visitAronVariantOnExit(const data::ListPtr&)
+ {
+ }
+
+ void
+ RecursiveConstVariantVisitor::visitAronVariant(const data::NDArrayPtr&)
+ {
+ }
+
+ void
+ RecursiveConstVariantVisitor::visitAronVariant(const data::IntPtr&)
+ {
+ }
+
+ void
+ RecursiveConstVariantVisitor::visitAronVariant(const data::LongPtr&)
+ {
+ }
+
+ void
+ RecursiveConstVariantVisitor::visitAronVariant(const data::FloatPtr&)
+ {
+ }
+
+ void
+ RecursiveConstVariantVisitor::visitAronVariant(const data::DoublePtr&)
+ {
+ }
+ void
+ RecursiveConstVariantVisitor::visitAronVariant(const data::BoolPtr&)
+ {
+ }
+ void
+ RecursiveConstVariantVisitor::visitAronVariant(const data::StringPtr&)
+ {
+ }
/****************************************************************************
* RecursiveVariantVisitor
***************************************************************************/
- data::Descriptor RecursiveVariantVisitor::getDescriptor(Input& n)
+ data::Descriptor
+ RecursiveVariantVisitor::getDescriptor(Input& n)
{
return ConstVariantVisitor::GetDescriptor(n);
}
- std::map<std::string, RecursiveVariantVisitor::InputNonConst> RecursiveVariantVisitor::getDictElements(Input& n)
+ std::map<std::string, RecursiveVariantVisitor::InputNonConst>
+ RecursiveVariantVisitor::getDictElements(Input& n)
{
return RecursiveConstVariantVisitor::GetDictElements(n);
}
- std::vector<RecursiveVariantVisitor::InputNonConst> RecursiveVariantVisitor::getListElements(Input& n)
+ std::vector<RecursiveVariantVisitor::InputNonConst>
+ RecursiveVariantVisitor::getListElements(Input& n)
{
return RecursiveConstVariantVisitor::GetListElements(n);
}
- void RecursiveVariantVisitor::visitDictOnEnter(Input& i)
+ void
+ RecursiveVariantVisitor::visitDictOnEnter(Input& i)
{
auto aron = data::Dict::DynamicCastAndCheck(i);
visitAronVariantOnEnter(aron);
}
- void RecursiveVariantVisitor::visitDictOnExit(Input& i)
+ void
+ RecursiveVariantVisitor::visitDictOnExit(Input& i)
{
auto aron = data::Dict::DynamicCastAndCheck(i);
visitAronVariantOnExit(aron);
}
- void RecursiveVariantVisitor::visitListOnEnter(Input& i)
+ void
+ RecursiveVariantVisitor::visitListOnEnter(Input& i)
{
auto aron = data::List::DynamicCastAndCheck(i);
visitAronVariantOnEnter(aron);
}
- void RecursiveVariantVisitor::visitListOnExit(Input& i)
+ void
+ RecursiveVariantVisitor::visitListOnExit(Input& i)
{
auto aron = data::List::DynamicCastAndCheck(i);
visitAronVariantOnExit(aron);
}
- void RecursiveVariantVisitor::visitNDArray(Input& i)
+ void
+ RecursiveVariantVisitor::visitNDArray(Input& i)
{
auto aron = data::NDArray::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveVariantVisitor::visitInt(Input& i)
+ void
+ RecursiveVariantVisitor::visitInt(Input& i)
{
auto aron = data::Int::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveVariantVisitor::visitLong(Input& i)
+ void
+ RecursiveVariantVisitor::visitLong(Input& i)
{
auto aron = data::Long::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveVariantVisitor::visitFloat(Input& i)
+ void
+ RecursiveVariantVisitor::visitFloat(Input& i)
{
auto aron = data::Float::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveVariantVisitor::visitDouble(Input& i)
+ void
+ RecursiveVariantVisitor::visitDouble(Input& i)
{
auto aron = data::Double::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveVariantVisitor::visitBool(Input& i)
+ void
+ RecursiveVariantVisitor::visitBool(Input& i)
{
auto aron = data::Bool::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveVariantVisitor::visitString(Input& i)
+ void
+ RecursiveVariantVisitor::visitString(Input& i)
{
auto aron = data::String::DynamicCastAndCheck(i);
visitAronVariant(aron);
}
- void RecursiveVariantVisitor::visitAronVariantOnEnter(data::DictPtr&) {}
- void RecursiveVariantVisitor::visitAronVariantOnEnter(data::ListPtr&) {}
- void RecursiveVariantVisitor::visitAronVariantOnExit(data::DictPtr&) {}
- void RecursiveVariantVisitor::visitAronVariantOnExit(data::ListPtr&) {}
- void RecursiveVariantVisitor::visitAronVariant(data::NDArrayPtr&) {}
- void RecursiveVariantVisitor::visitAronVariant(data::IntPtr&) {}
- void RecursiveVariantVisitor::visitAronVariant(data::LongPtr&) {}
- void RecursiveVariantVisitor::visitAronVariant(data::FloatPtr&) {}
- void RecursiveVariantVisitor::visitAronVariant(data::DoublePtr&) {}
- void RecursiveVariantVisitor::visitAronVariant(data::BoolPtr&) {}
- void RecursiveVariantVisitor::visitAronVariant(data::StringPtr&) {}
+ void
+ RecursiveVariantVisitor::visitAronVariantOnEnter(data::DictPtr&)
+ {
+ }
+
+ void
+ RecursiveVariantVisitor::visitAronVariantOnEnter(data::ListPtr&)
+ {
+ }
+
+ void
+ RecursiveVariantVisitor::visitAronVariantOnExit(data::DictPtr&)
+ {
+ }
+
+ void
+ RecursiveVariantVisitor::visitAronVariantOnExit(data::ListPtr&)
+ {
+ }
+ void
+ RecursiveVariantVisitor::visitAronVariant(data::NDArrayPtr&)
+ {
+ }
+
+ void
+ RecursiveVariantVisitor::visitAronVariant(data::IntPtr&)
+ {
+ }
+
+ void
+ RecursiveVariantVisitor::visitAronVariant(data::LongPtr&)
+ {
+ }
+
+ void
+ RecursiveVariantVisitor::visitAronVariant(data::FloatPtr&)
+ {
+ }
+
+ void
+ RecursiveVariantVisitor::visitAronVariant(data::DoublePtr&)
+ {
+ }
+
+ void
+ RecursiveVariantVisitor::visitAronVariant(data::BoolPtr&)
+ {
+ }
+
+ void
+ RecursiveVariantVisitor::visitAronVariant(data::StringPtr&)
+ {
+ }
/****************************************************************************
* RecursiveConstTypedVariantVisitor
***************************************************************************/
- type::Descriptor RecursiveConstTypedVariantVisitor::getDescriptor(DataInput& o, TypeInput& n)
+ type::Descriptor
+ RecursiveConstTypedVariantVisitor::getDescriptor(DataInput& o, TypeInput& n)
{
return ConstTypedVariantVisitor::GetDescriptor(o, n);
}
@@ -637,182 +880,309 @@ namespace armarx::aron::data
return GetTupleElements(o, t);
}
- void RecursiveConstTypedVariantVisitor::visitObjectOnEnter(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitObjectOnEnter(DataInput& i, TypeInput& j)
{
auto d = data::Dict::DynamicCastAndCheck(i);
auto t = type::Object::DynamicCastAndCheck(j);
visitAronVariantOnEnter(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitDictOnEnter(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitDictOnEnter(DataInput& i, TypeInput& j)
{
auto d = data::Dict::DynamicCastAndCheck(i);
auto t = type::Dict::DynamicCastAndCheck(j);
visitAronVariantOnEnter(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitPairOnEnter(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitPairOnEnter(DataInput& i, TypeInput& j)
{
auto d = data::List::DynamicCastAndCheck(i);
auto t = type::Pair::DynamicCastAndCheck(j);
visitAronVariantOnEnter(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitTupleOnEnter(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitTupleOnEnter(DataInput& i, TypeInput& j)
{
auto d = data::List::DynamicCastAndCheck(i);
auto t = type::Tuple::DynamicCastAndCheck(j);
visitAronVariantOnEnter(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitListOnEnter(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitListOnEnter(DataInput& i, TypeInput& j)
{
auto d = data::List::DynamicCastAndCheck(i);
auto t = type::List::DynamicCastAndCheck(j);
visitAronVariantOnEnter(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitObjectOnExit(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitObjectOnExit(DataInput& i, TypeInput& j)
{
auto d = data::Dict::DynamicCastAndCheck(i);
auto t = type::Object::DynamicCastAndCheck(j);
visitAronVariantOnExit(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitDictOnExit(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitDictOnExit(DataInput& i, TypeInput& j)
{
auto d = data::Dict::DynamicCastAndCheck(i);
auto t = type::Dict::DynamicCastAndCheck(j);
visitAronVariantOnExit(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitPairOnExit(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitPairOnExit(DataInput& i, TypeInput& j)
{
auto d = data::List::DynamicCastAndCheck(i);
auto t = type::Pair::DynamicCastAndCheck(j);
visitAronVariantOnExit(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitTupleOnExit(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitTupleOnExit(DataInput& i, TypeInput& j)
{
auto d = data::List::DynamicCastAndCheck(i);
auto t = type::Tuple::DynamicCastAndCheck(j);
visitAronVariantOnExit(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitListOnExit(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitListOnExit(DataInput& i, TypeInput& j)
{
auto d = data::List::DynamicCastAndCheck(i);
auto t = type::List::DynamicCastAndCheck(j);
visitAronVariantOnExit(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitMatrix(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitMatrix(DataInput& i, TypeInput& j)
{
auto d = data::NDArray::DynamicCastAndCheck(i);
auto t = type::Matrix::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitNDArray(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitNDArray(DataInput& i, TypeInput& j)
{
auto d = data::NDArray::DynamicCastAndCheck(i);
auto t = type::NDArray::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitQuaternion(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitQuaternion(DataInput& i, TypeInput& j)
{
auto d = data::NDArray::DynamicCastAndCheck(i);
auto t = type::Quaternion::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitImage(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitImage(DataInput& i, TypeInput& j)
{
auto d = data::NDArray::DynamicCastAndCheck(i);
auto t = type::Image::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitPointCloud(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitPointCloud(DataInput& i, TypeInput& j)
{
auto d = data::NDArray::DynamicCastAndCheck(i);
auto t = type::PointCloud::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitIntEnum(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitIntEnum(DataInput& i, TypeInput& j)
{
auto d = data::Int::DynamicCastAndCheck(i);
auto t = type::IntEnum::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitInt(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitInt(DataInput& i, TypeInput& j)
{
auto d = data::Int::DynamicCastAndCheck(i);
auto t = type::Int::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitLong(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitLong(DataInput& i, TypeInput& j)
{
auto d = data::Long::DynamicCastAndCheck(i);
auto t = type::Long::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitFloat(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitFloat(DataInput& i, TypeInput& j)
{
auto d = data::Float::DynamicCastAndCheck(i);
auto t = type::Float::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitDouble(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitDouble(DataInput& i, TypeInput& j)
{
auto d = data::Double::DynamicCastAndCheck(i);
auto t = type::Double::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitBool(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitBool(DataInput& i, TypeInput& j)
{
auto d = data::Bool::DynamicCastAndCheck(i);
auto t = type::Bool::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
- void RecursiveConstTypedVariantVisitor::visitString(DataInput& i, TypeInput& j)
+ void
+ RecursiveConstTypedVariantVisitor::visitString(DataInput& i, TypeInput& j)
{
auto d = data::String::DynamicCastAndCheck(i);
auto t = type::String::DynamicCastAndCheck(j);
visitAronVariant(d, t);
}
-
// see above
- void RecursiveConstTypedVariantVisitor::visitAronVariantOnEnter(const data::DictPtr&, const type::ObjectPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariantOnEnter(const data::DictPtr&, const type::DictPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariantOnEnter(const data::ListPtr&, const type::ListPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariantOnEnter(const data::ListPtr&, const type::PairPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariantOnEnter(const data::ListPtr&, const type::TuplePtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariantOnExit(const data::DictPtr&, const type::ObjectPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariantOnExit(const data::DictPtr&, const type::DictPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariantOnExit(const data::ListPtr&, const type::ListPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariantOnExit(const data::ListPtr&, const type::PairPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariantOnExit(const data::ListPtr&, const type::TuplePtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::MatrixPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::NDArrayPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::QuaternionPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::PointCloudPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&, const type::ImagePtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::IntPtr&, const type::IntEnumPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::IntPtr&, const type::IntPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::LongPtr&, const type::LongPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::FloatPtr&, const type::FloatPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::DoublePtr&, const type::DoublePtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::BoolPtr&, const type::BoolPtr&) {}
- void RecursiveConstTypedVariantVisitor::visitAronVariant(const data::StringPtr&, const type::StringPtr&) {}
-}
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariantOnEnter(const data::DictPtr&,
+ const type::ObjectPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariantOnEnter(const data::DictPtr&,
+ const type::DictPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariantOnEnter(const data::ListPtr&,
+ const type::ListPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariantOnEnter(const data::ListPtr&,
+ const type::PairPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariantOnEnter(const data::ListPtr&,
+ const type::TuplePtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariantOnExit(const data::DictPtr&,
+ const type::ObjectPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariantOnExit(const data::DictPtr&,
+ const type::DictPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariantOnExit(const data::ListPtr&,
+ const type::ListPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariantOnExit(const data::ListPtr&,
+ const type::PairPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariantOnExit(const data::ListPtr&,
+ const type::TuplePtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&,
+ const type::MatrixPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&,
+ const type::NDArrayPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&,
+ const type::QuaternionPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&,
+ const type::PointCloudPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::NDArrayPtr&,
+ const type::ImagePtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::IntPtr&,
+ const type::IntEnumPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::IntPtr&, const type::IntPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::LongPtr&, const type::LongPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::FloatPtr&,
+ const type::FloatPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::DoublePtr&,
+ const type::DoublePtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::BoolPtr&, const type::BoolPtr&)
+ {
+ }
+
+ void
+ RecursiveConstTypedVariantVisitor::visitAronVariant(const data::StringPtr&,
+ const type::StringPtr&)
+ {
+ }
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h
index 726adfd346fbdec56d945d16eac4059e57c4f90b..a6abd21b590d768b9ca12345a8fd688eb80b2cd6 100644
--- a/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h
@@ -27,9 +27,9 @@
#include <string>
#include <vector>
-#include "../RecursiveVisitor.h"
-#include "../../variant/forward_declarations.h"
#include "../../../type/variant/forward_declarations.h"
+#include "../../variant/forward_declarations.h"
+#include "../RecursiveVisitor.h"
namespace armarx::aron::data
{
@@ -69,7 +69,8 @@ namespace armarx::aron::data
/**
* @see type/visitor/variant/VariantVisitor.h
*/
- struct ConstTypedVariantVisitor : virtual public TypedVisitor<const data::VariantPtr, const type::VariantPtr>
+ struct ConstTypedVariantVisitor :
+ virtual public TypedVisitor<const data::VariantPtr, const type::VariantPtr>
{
static type::Descriptor GetDescriptor(DataInput& i, TypeInput& j);
type::Descriptor getDescriptor(DataInput& i, TypeInput& j) override;
@@ -189,7 +190,8 @@ namespace armarx::aron::data
/**
* @see type/visitor/variant/VariantVisitor.h
*/
- struct RecursiveConstTypedVariantVisitor : virtual public RecursiveTypedVisitor<const data::VariantPtr, const type::VariantPtr>
+ struct RecursiveConstTypedVariantVisitor :
+ virtual public RecursiveTypedVisitor<const data::VariantPtr, const type::VariantPtr>
{
type::Descriptor getDescriptor(DataInput& o, TypeInput& n) override;
static MapElements GetObjectElements(DataInput& o, TypeInput& t);
@@ -259,4 +261,4 @@ namespace armarx::aron::data
virtual void visitAronVariant(const data::BoolPtr&, const type::BoolPtr&);
virtual void visitAronVariant(const data::StringPtr&, const type::StringPtr&);
};
-}
+} // namespace armarx::aron::data
diff --git a/source/RobotAPI/libraries/aron/core/rw.h b/source/RobotAPI/libraries/aron/core/rw.h
index 02673b3e8be2d01cb75618d8ca6bb66b0d10de38..ccb61620c648a35e5fc4c4c5c69b263645a0680b 100644
--- a/source/RobotAPI/libraries/aron/core/rw.h
+++ b/source/RobotAPI/libraries/aron/core/rw.h
@@ -7,32 +7,31 @@ namespace armarx::aron
{
template <class ReaderT, class T>
- requires(data::isReader<ReaderT>&& armarx::aron::cpp::isAronGeneratedClass<T>) inline void read(
- ReaderT& aron_r,
- typename ReaderT::InputType& input,
- T& ret)
+ requires(data::isReader<ReaderT> && armarx::aron::cpp::isAronGeneratedClass<T>)
+ inline void
+ read(ReaderT& aron_r, typename ReaderT::InputType& input, T& ret)
{
ret.read(aron_r, input);
}
template <class WriterT, class T>
- requires(data::isWriter<WriterT>&& armarx::aron::cpp::isAronGeneratedClass<
- T>) inline void write(WriterT& aron_w,
- const T& input,
- typename WriterT::ReturnType& ret,
- const armarx::aron::Path& aron_p = armarx::aron::Path())
+ requires(data::isWriter<WriterT> && armarx::aron::cpp::isAronGeneratedClass<T>)
+ inline void
+ write(WriterT& aron_w,
+ const T& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p = armarx::aron::Path())
{
ret = input.write(aron_w, aron_p);
}
template <class ReaderT, class DtoT, class BoT>
- requires(
- data::isReader<ReaderT>&& armarx::aron::cpp::isAronGeneratedClass<DtoT> &&
- !detail::DtoAndBoAreSame<DtoT, BoT>) inline void read(ReaderT& aron_r,
- typename ReaderT::InputType& input,
- BoT& ret)
+ requires(data::isReader<ReaderT> && armarx::aron::cpp::isAronGeneratedClass<DtoT> &&
+ !detail::DtoAndBoAreSame<DtoT, BoT>)
+ inline void
+ read(ReaderT& aron_r, typename ReaderT::InputType& input, BoT& ret)
{
DtoT aron;
aron.read(aron_r, input);
@@ -42,13 +41,13 @@ namespace armarx::aron
template <class WriterT, class DtoT, class BoT>
- requires(
- data::isWriter<WriterT>&& armarx::aron::cpp::isAronGeneratedClass<DtoT> &&
- !detail::DtoAndBoAreSame<DtoT, BoT>) inline void write(WriterT& aron_w,
- const BoT& input,
- typename WriterT::ReturnType& ret,
- const armarx::aron::Path& aron_p =
- armarx::aron::Path())
+ requires(data::isWriter<WriterT> && armarx::aron::cpp::isAronGeneratedClass<DtoT> &&
+ !detail::DtoAndBoAreSame<DtoT, BoT>)
+ inline void
+ write(WriterT& aron_w,
+ const BoT& input,
+ typename WriterT::ReturnType& ret,
+ const armarx::aron::Path& aron_p = armarx::aron::Path())
{
DtoT aron;
armarx::toAron(aron, input);
diff --git a/source/RobotAPI/libraries/aron/core/type/converter/Converter.h b/source/RobotAPI/libraries/aron/core/type/converter/Converter.h
index d714842d4bfd1a1281ad60d2dc4e059c67f3279b..00a53876f7e30bbf415ab979a0311593f7d9034d 100644
--- a/source/RobotAPI/libraries/aron/core/type/converter/Converter.h
+++ b/source/RobotAPI/libraries/aron/core/type/converter/Converter.h
@@ -31,7 +31,7 @@ namespace armarx::aron::type
{
// prototypes
template <class ReaderImplementation, class WriterImplementation, class DerivedT>
- requires isReader<ReaderImplementation> && isWriter<WriterImplementation>
+ requires isReader<ReaderImplementation> && isWriter<WriterImplementation>
struct Converter;
template <class T>
@@ -40,14 +40,14 @@ namespace armarx::aron::type
T>::value;
template <class ConverterImplementation>
- requires isConverter<ConverterImplementation>
+ requires isConverter<ConverterImplementation>
typename ConverterImplementation::WriterReturnType
readAndWrite(typename ConverterImplementation::ReaderInputType& o);
/// Converter struct providing the needed methods.
/// WriterImplementation is a writer class, TODO: add concepts
template <class ReaderImplementation, class WriterImplementation, class DerivedT>
- requires isReader<ReaderImplementation> && isWriter<WriterImplementation>
+ requires isReader<ReaderImplementation> && isWriter<WriterImplementation>
struct Converter : virtual public Visitor<typename ReaderImplementation::InputType>
{
@@ -110,7 +110,7 @@ namespace armarx::aron::type
void
visitList(ReaderInputType& o) final
- {
+ {
//ARMARX_INFO << "convert visitList";
ReaderInputTypeNonConst acceptedType;
@@ -331,7 +331,7 @@ namespace armarx::aron::type
/// the function to read from a variant and write to a writer T
/// returns the returntype of T
template <class ConverterImplementation>
- requires isConverter<ConverterImplementation>
+ requires isConverter<ConverterImplementation>
typename ConverterImplementation::WriterReturnType
readAndWrite(typename ConverterImplementation::ReaderInputType& o)
diff --git a/source/RobotAPI/libraries/aron/core/type/converter/nlohmannJSON/NlohmannJSONConverter.h b/source/RobotAPI/libraries/aron/core/type/converter/nlohmannJSON/NlohmannJSONConverter.h
index ba64ac905ff814165e1cdb4f07c0ad2931433e59..846474ced359bb2c9fac47959977a418db9f5518 100644
--- a/source/RobotAPI/libraries/aron/core/type/converter/nlohmannJSON/NlohmannJSONConverter.h
+++ b/source/RobotAPI/libraries/aron/core/type/converter/nlohmannJSON/NlohmannJSONConverter.h
@@ -23,28 +23,32 @@
#pragma once
-#include "../Converter.h"
-#include "../../visitor/nlohmannJSON/NlohmannJSONVisitor.h"
#include "../../rw/reader/nlohmannJSON/NlohmannJSONReader.h"
#include "../../rw/writer/nlohmannJSON/NlohmannJSONWriter.h"
+#include "../../visitor/nlohmannJSON/NlohmannJSONVisitor.h"
+#include "../Converter.h"
namespace armarx::aron::type
{
// WriterImplementation is a writer class
template <class WriterImplementation>
- requires isWriter<WriterImplementation>
+ requires isWriter<WriterImplementation>
struct FromNlohmannJSONConverter :
- virtual public Converter<aron::type::reader::NlohmannJSONReader, WriterImplementation, FromNlohmannJSONConverter<WriterImplementation>>
+ virtual public Converter<aron::type::reader::NlohmannJSONReader,
+ WriterImplementation,
+ FromNlohmannJSONConverter<WriterImplementation>>
{
virtual ~FromNlohmannJSONConverter() = default;
};
// WriterImplementation is a reader class
template <class ReaderImplementation>
- requires isReader<ReaderImplementation>
+ requires isReader<ReaderImplementation>
struct ToNlohmannJSONConverter :
- virtual public Converter<ReaderImplementation, aron::type::writer::NlohmannJSONWriter, ToNlohmannJSONConverter<ReaderImplementation>>
+ virtual public Converter<ReaderImplementation,
+ aron::type::writer::NlohmannJSONWriter,
+ ToNlohmannJSONConverter<ReaderImplementation>>
{
virtual ~ToNlohmannJSONConverter() = default;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/converter/variant/VariantConverter.h b/source/RobotAPI/libraries/aron/core/type/converter/variant/VariantConverter.h
index bd81fa777318871d7c74f6e595f2dd55eb955a7c..87c7ee4184218482ca079ea30394af86b01058e1 100644
--- a/source/RobotAPI/libraries/aron/core/type/converter/variant/VariantConverter.h
+++ b/source/RobotAPI/libraries/aron/core/type/converter/variant/VariantConverter.h
@@ -23,30 +23,33 @@
#pragma once
-#include "../Converter.h"
-#include "../../visitor/variant/VariantVisitor.h"
#include "../../rw/reader/variant/VariantReader.h"
#include "../../rw/writer/variant/VariantWriter.h"
-
+#include "../../visitor/variant/VariantVisitor.h"
+#include "../Converter.h"
namespace armarx::aron::type
{
// WriterImplementation is a writer class
template <class WriterImplementation>
- requires isWriter<WriterImplementation>
+ requires isWriter<WriterImplementation>
struct FromVariantConverter :
- virtual public Converter<aron::type::reader::VariantReader, WriterImplementation, FromVariantConverter<WriterImplementation>>
+ virtual public Converter<aron::type::reader::VariantReader,
+ WriterImplementation,
+ FromVariantConverter<WriterImplementation>>
{
virtual ~FromVariantConverter() = default;
};
// WriterImplementation is a reader class
template <class ReaderImplementation>
- requires isReader<ReaderImplementation>
+ requires isReader<ReaderImplementation>
struct ToVariantConverter :
- virtual public Converter<ReaderImplementation, aron::type::writer::VariantWriter, ToVariantConverter<ReaderImplementation>>
+ virtual public Converter<ReaderImplementation,
+ aron::type::writer::VariantWriter,
+ ToVariantConverter<ReaderImplementation>>
{
virtual ~ToVariantConverter() = default;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
index 3421d385498b104cc145612e86ffb5bbf19fe6a7..a8cc8ba840b00ec012585e733e2ccc2f9772d7f0 100644
--- a/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/rw/reader/nlohmannJSON/NlohmannJSONReader.cpp
@@ -258,7 +258,8 @@ namespace armarx::aron::type::reader
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- if (input.count(rw::json::constantes::DEFAULT_SLUG) && not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
+ if (input.count(rw::json::constantes::DEFAULT_SLUG) &&
+ not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
{
if (std::string d = input[rw::json::constantes::DEFAULT_SLUG]; not d.empty())
{
@@ -277,7 +278,8 @@ namespace armarx::aron::type::reader
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- if (input.count(rw::json::constantes::DEFAULT_SLUG) && not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
+ if (input.count(rw::json::constantes::DEFAULT_SLUG) &&
+ not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
{
if (std::string d = input[rw::json::constantes::DEFAULT_SLUG]; not d.empty())
{
@@ -296,7 +298,8 @@ namespace armarx::aron::type::reader
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- if (input.count(rw::json::constantes::DEFAULT_SLUG) && not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
+ if (input.count(rw::json::constantes::DEFAULT_SLUG) &&
+ not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
{
if (std::string d = input[rw::json::constantes::DEFAULT_SLUG]; not d.empty())
{
@@ -315,7 +318,8 @@ namespace armarx::aron::type::reader
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- if (input.count(rw::json::constantes::DEFAULT_SLUG) && not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
+ if (input.count(rw::json::constantes::DEFAULT_SLUG) &&
+ not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
{
if (std::string d = input[rw::json::constantes::DEFAULT_SLUG]; not d.empty())
{
@@ -334,7 +338,8 @@ namespace armarx::aron::type::reader
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- if (input.count(rw::json::constantes::DEFAULT_SLUG) && not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
+ if (input.count(rw::json::constantes::DEFAULT_SLUG) &&
+ not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
{
if (std::string d = input[rw::json::constantes::DEFAULT_SLUG]; not d.empty())
{
@@ -353,7 +358,8 @@ namespace armarx::aron::type::reader
maybe = rw::json::conversion::String2Maybe.at(input[rw::json::constantes::MAYBE_SLUG]);
- if (input.count(rw::json::constantes::DEFAULT_SLUG) && not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
+ if (input.count(rw::json::constantes::DEFAULT_SLUG) &&
+ not input.at(rw::json::constantes::DEFAULT_SLUG).is_null())
{
if (std::string d = input[rw::json::constantes::DEFAULT_SLUG]; not d.empty())
{
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/All.h b/source/RobotAPI/libraries/aron/core/type/variant/All.h
index 46934fb072efe7e58992d22477234785569d5e80..140fb6222d0ab075751a589cd40b42ece48dc65a 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/All.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/All.h
@@ -1,10 +1,10 @@
#pragma once
+#include "any/All.h"
#include "container/All.h"
-#include "ndarray/All.h"
#include "enum/All.h"
+#include "ndarray/All.h"
#include "primitive/All.h"
-#include "any/All.h"
/**
* A convenience header to include all aron files (full include, not forward declared)
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
index 3c32ade9fdb74f97252250d21acca0f00558acb1..3eceac2ccb5622b7a5245ba9283779564cebe6b2 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Factory.cpp
@@ -25,6 +25,7 @@
// Header
#include "Factory.h"
+
#include <ArmarXCore/core/logging/Logging.h>
// ArmarX
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Factory.h b/source/RobotAPI/libraries/aron/core/type/variant/Factory.h
index 678e3ac70734ecc8f3504419f4424ae989f1343f..b2455f4e64a41771f25d0e0da5fdaff294c8a1be 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Factory.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Factory.h
@@ -25,8 +25,8 @@
#include <memory>
-#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/libraries/aron/core/Descriptor.h>
+#include <RobotAPI/libraries/aron/core/Path.h>
#include <RobotAPI/libraries/aron/core/type/variant/forward_declarations.h>
namespace armarx::aron::type
@@ -44,4 +44,4 @@ namespace armarx::aron::type
virtual ~VariantFactory() = default;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp b/source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp
index dc90127ad6fe3664bbcf09ca918238c956c2bee1..013207625c3b02b49be4ac6a85db98d6afcb952c 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/Variant.cpp
@@ -36,7 +36,8 @@ namespace armarx::aron::type
// static data members
const VariantFactoryPtr Variant::FACTORY = VariantFactoryPtr(new VariantFactory());
- VariantPtr Variant::FromAronDTO(const type::dto::GenericType& a, const Path& path)
+ VariantPtr
+ Variant::FromAronDTO(const type::dto::GenericType& a, const Path& path)
{
return FACTORY->create(a, path);
}
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/any/AnyObject.cpp b/source/RobotAPI/libraries/aron/core/type/variant/any/AnyObject.cpp
index 78a17d080b78cd1d4989a38ee8e888d66ab63d84..2744d597a1581c31a7f5daeed34a2d4bc68abc0e 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/any/AnyObject.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/variant/any/AnyObject.cpp
@@ -23,8 +23,8 @@
// STD/STL
-#include <string>
#include <map>
+#include <string>
// Header
#include "AnyObject.h"
@@ -37,25 +37,28 @@ namespace armarx::aron::type
{
}
- AnyObject::AnyObject(const type::dto::AnyObject&o, const Path& path) :
+ AnyObject::AnyObject(const type::dto::AnyObject& o, const Path& path) :
detail::AnyVariant<type::dto::AnyObject, AnyObject>(o, type::Descriptor::ANY_OBJECT, path)
{
}
/* public member functions */
- type::dto::AnyObjectPtr AnyObject::toAnyObjectDTO() const
+ type::dto::AnyObjectPtr
+ AnyObject::toAnyObjectDTO() const
{
return aron;
}
/* virtual implementations */
- std::string AnyObject::getShortName() const
+ std::string
+ AnyObject::getShortName() const
{
return "AnyObject";
}
- std::string AnyObject::getFullName() const
+ std::string
+ AnyObject::getFullName() const
{
return "armarx::aron::type::AnyObject";
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/any/AnyObject.h b/source/RobotAPI/libraries/aron/core/type/variant/any/AnyObject.h
index db153869ba81c4a47f777cc97bc3fe306ede0f16..17894e60b41ac7b097f2fe0e2a21ce657f5b0b25 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/any/AnyObject.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/any/AnyObject.h
@@ -34,8 +34,7 @@ namespace armarx::aron::type
/**
* @brief The AnyObject class. It represents the any object type
*/
- class AnyObject :
- public detail::AnyVariant<type::dto::AnyObject, AnyObject>
+ class AnyObject : public detail::AnyVariant<type::dto::AnyObject, AnyObject>
{
public:
/* constructors */
@@ -48,4 +47,4 @@ namespace armarx::aron::type
virtual std::string getShortName() const override;
virtual std::string getFullName() const override;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/AnyVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/AnyVariant.h
index eb9be13f27f8691afc7a2cc5168b63a770589e22..6b680806a51b6ba943e77af96b0dab09cebad497 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/AnyVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/AnyVariant.h
@@ -31,9 +31,8 @@
namespace armarx::aron::type::detail
{
- template<typename AronTypeT, typename DerivedT>
- class AnyVariant :
- public SpecializedVariantBase<AronTypeT, DerivedT>
+ template <typename AronTypeT, typename DerivedT>
+ class AnyVariant : public SpecializedVariantBase<AronTypeT, DerivedT>
{
public:
using SpecializedVariantBase<AronTypeT, DerivedT>::SpecializedVariantBase;
@@ -41,19 +40,26 @@ namespace armarx::aron::type::detail
virtual ~AnyVariant() = default;
/* virtual implementations */
- VariantPtr navigateAbsolute(const Path &path) const override
+ VariantPtr
+ navigateAbsolute(const Path& path) const override
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through an any navigator. The input path was: " + path.toString(), Variant::getPath());
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Could not navigate through an any navigator. The input path was: " +
+ path.toString(),
+ Variant::getPath());
}
- std::vector<VariantPtr> getChildren() const override
+ std::vector<VariantPtr>
+ getChildren() const override
{
return {};
}
- size_t childrenSize() const override
+ size_t
+ childrenSize() const override
{
return 0;
}
};
-}
+} // namespace armarx::aron::type::detail
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h
index 5e61cfac4e1e98943c0a548ff0ca8d684d1700cb..622929211e70cc209ca4371a5f0c09474b4db6ca 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/ContainerVariant.h
@@ -31,12 +31,11 @@
namespace armarx::aron::type::detail
{
- template<typename AronTypeT, typename DerivedT>
- class ContainerVariant :
- public SpecializedVariantBase<AronTypeT, DerivedT>
+ template <typename AronTypeT, typename DerivedT>
+ class ContainerVariant : public SpecializedVariantBase<AronTypeT, DerivedT>
{
public:
using SpecializedVariantBase<AronTypeT, DerivedT>::SpecializedVariantBase;
virtual ~ContainerVariant() = default;
};
-}
+} // namespace armarx::aron::type::detail
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/DtoVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/DtoVariant.h
index da38cbed4981b64c7ec68b89c42c92fef619693f..bf49c292d51cce24643de2e6d57d6da38af404f0 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/DtoVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/DtoVariant.h
@@ -30,9 +30,8 @@
namespace armarx::aron::type::detail
{
- template<typename AronTypeT, typename DerivedT>
- class DtoVariant :
- public SpecializedVariantBase<AronTypeT, DerivedT>
+ template <typename AronTypeT, typename DerivedT>
+ class DtoVariant : public SpecializedVariantBase<AronTypeT, DerivedT>
{
public:
using SpecializedVariantBase<AronTypeT, DerivedT>::SpecializedVariantBase;
@@ -40,19 +39,26 @@ namespace armarx::aron::type::detail
virtual ~DtoVariant() = default;
/* virtual implementations */
- VariantPtr navigateAbsolute(const Path &path) const override
+ VariantPtr
+ navigateAbsolute(const Path& path) const override
{
- throw error::AronException(__PRETTY_FUNCTION__, "Could not navigate through an dto navigator. The input path was: " + path.toString(), Variant::getPath());
+ throw error::AronException(
+ __PRETTY_FUNCTION__,
+ "Could not navigate through an dto navigator. The input path was: " +
+ path.toString(),
+ Variant::getPath());
}
- std::vector<VariantPtr> getChildren() const override
+ std::vector<VariantPtr>
+ getChildren() const override
{
return {};
}
- size_t childrenSize() const override
+ size_t
+ childrenSize() const override
{
return 0;
}
};
-}
+} // namespace armarx::aron::type::detail
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
index f084df0a9202d7da4609853cbffe8f85936618ab..767a590517caacd6f4cae998a1203caa2dac0c02 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/PrimitiveVariant.h
@@ -27,7 +27,6 @@
#include "SpecializedVariant.h"
-
namespace armarx::aron::type::detail
{
template <typename AronTypeT, typename DerivedT, typename ValueT>
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h b/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
index fdc1bcf17ce912687112c9a9beab080df68d9a59..4bf0084837cae510b133f16fb8ac482d581212f4 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/detail/SpecializedVariant.h
@@ -29,7 +29,6 @@
#include "../Variant.h"
-
namespace armarx::aron::type::detail
{
template <typename AronTypeT, typename DerivedT>
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/All.h b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/All.h
index feab5f11257587eae2b81b6104f7a876132c0531..4b44add211b47a8af4d94178fc3d8f3b58606eb0 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/ndarray/All.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/ndarray/All.h
@@ -1,10 +1,10 @@
#pragma once
-#include "NDArray.h"
-#include "Matrix.h"
-#include "Quaternion.h"
#include "Image.h"
+#include "Matrix.h"
+#include "NDArray.h"
#include "PointCloud.h"
+#include "Quaternion.h"
/**
* A convenience header to include all ndarray aron files (full include, not forward declared)
diff --git a/source/RobotAPI/libraries/aron/core/type/variant/primitive/All.h b/source/RobotAPI/libraries/aron/core/type/variant/primitive/All.h
index 2b739568ee4a7174a683df8f93ecb77efd41a369..79a84faa7e9eaec6b218e508bac7fb14548a15d5 100644
--- a/source/RobotAPI/libraries/aron/core/type/variant/primitive/All.h
+++ b/source/RobotAPI/libraries/aron/core/type/variant/primitive/All.h
@@ -1,11 +1,11 @@
#pragma once
+#include "Bool.h"
+#include "Double.h"
+#include "Float.h"
#include "Int.h"
#include "Long.h"
-#include "Float.h"
-#include "Double.h"
#include "String.h"
-#include "Bool.h"
/**
* A convenience header to include all primitive aron files (full include, not forward declared)
diff --git a/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h b/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h
index e8368a7dcc988aa38a4ebd0931001c5a52b02dcd..38d1b878361a23d9f2d2877c37186d6f7e4b3a0f 100644
--- a/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/Visitor.h
@@ -35,49 +35,50 @@ namespace armarx::aron::type
* If you want to call a visitX recursively, please refer to "RecursiveVisitor.h"
*/
template <class VisitorImplementation>
- void visit(VisitorImplementation& v, typename VisitorImplementation::Input& t)
+ void
+ visit(VisitorImplementation& v, typename VisitorImplementation::Input& t)
{
auto descriptor = v.getDescriptor(t);
switch (descriptor)
{
- case type::Descriptor::OBJECT:
- return v.visitObject(t);
- case type::Descriptor::LIST:
- return v.visitList(t);
- case type::Descriptor::DICT:
- return v.visitDict(t);
- case type::Descriptor::PAIR:
- return v.visitPair(t);
- case type::Descriptor::TUPLE:
- return v.visitTuple(t);
- case type::Descriptor::NDARRAY:
- return v.visitNDArray(t);
- case type::Descriptor::MATRIX:
- return v.visitMatrix(t);
- case type::Descriptor::IMAGE:
- return v.visitImage(t);
- case type::Descriptor::POINTCLOUD:
- return v.visitPointCloud(t);
- case type::Descriptor::QUATERNION:
- return v.visitQuaternion(t);
- case type::Descriptor::INT:
- return v.visitInt(t);
- case type::Descriptor::LONG:
- return v.visitLong(t);
- case type::Descriptor::FLOAT:
- return v.visitFloat(t);
- case type::Descriptor::DOUBLE:
- return v.visitDouble(t);
- case type::Descriptor::STRING:
- return v.visitString(t);
- case type::Descriptor::BOOL:
- return v.visitBool(t);
- case type::Descriptor::ANY_OBJECT:
- return v.visitAnyObject(t);
- case type::Descriptor::INT_ENUM:
- return v.visitIntEnum(t);
- case type::Descriptor::UNKNOWN:
- return v.visitUnknown(t);
+ case type::Descriptor::OBJECT:
+ return v.visitObject(t);
+ case type::Descriptor::LIST:
+ return v.visitList(t);
+ case type::Descriptor::DICT:
+ return v.visitDict(t);
+ case type::Descriptor::PAIR:
+ return v.visitPair(t);
+ case type::Descriptor::TUPLE:
+ return v.visitTuple(t);
+ case type::Descriptor::NDARRAY:
+ return v.visitNDArray(t);
+ case type::Descriptor::MATRIX:
+ return v.visitMatrix(t);
+ case type::Descriptor::IMAGE:
+ return v.visitImage(t);
+ case type::Descriptor::POINTCLOUD:
+ return v.visitPointCloud(t);
+ case type::Descriptor::QUATERNION:
+ return v.visitQuaternion(t);
+ case type::Descriptor::INT:
+ return v.visitInt(t);
+ case type::Descriptor::LONG:
+ return v.visitLong(t);
+ case type::Descriptor::FLOAT:
+ return v.visitFloat(t);
+ case type::Descriptor::DOUBLE:
+ return v.visitDouble(t);
+ case type::Descriptor::STRING:
+ return v.visitString(t);
+ case type::Descriptor::BOOL:
+ return v.visitBool(t);
+ case type::Descriptor::ANY_OBJECT:
+ return v.visitAnyObject(t);
+ case type::Descriptor::INT_ENUM:
+ return v.visitIntEnum(t);
+ case type::Descriptor::UNKNOWN:
+ return v.visitUnknown(t);
}
}
@@ -100,28 +101,34 @@ namespace armarx::aron::type
struct Visitor : virtual public VisitorBase<T>
{
using Input = typename VisitorBase<T>::Input;
- virtual void visitObject(Input&) {};
- virtual void visitDict(Input&) {};
- virtual void visitPair(Input&) {};
- virtual void visitTuple(Input&) {};
- virtual void visitList(Input&) {};
- virtual void visitMatrix(Input&) {};
- virtual void visitNDArray(Input&) {};
- virtual void visitQuaternion(Input&) {};
- virtual void visitOrientation(Input&) {};
- virtual void visitPosition(Input&) {};
- virtual void visitPose(Input&) {};
- virtual void visitImage(Input&) {};
- virtual void visitPointCloud(Input&) {};
- virtual void visitIntEnum(Input&) {};
- virtual void visitInt(Input&) {};
- virtual void visitLong(Input&) {};
- virtual void visitFloat(Input&) {};
- virtual void visitDouble(Input&) {};
- virtual void visitBool(Input&) {};
- virtual void visitString(Input&) {};
- virtual void visitAnyObject(Input&) {};
- virtual void visitUnknown(Input&) { throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor."); }
+ virtual void visitObject(Input&){};
+ virtual void visitDict(Input&){};
+ virtual void visitPair(Input&){};
+ virtual void visitTuple(Input&){};
+ virtual void visitList(Input&){};
+ virtual void visitMatrix(Input&){};
+ virtual void visitNDArray(Input&){};
+ virtual void visitQuaternion(Input&){};
+ virtual void visitOrientation(Input&){};
+ virtual void visitPosition(Input&){};
+ virtual void visitPose(Input&){};
+ virtual void visitImage(Input&){};
+ virtual void visitPointCloud(Input&){};
+ virtual void visitIntEnum(Input&){};
+ virtual void visitInt(Input&){};
+ virtual void visitLong(Input&){};
+ virtual void visitFloat(Input&){};
+ virtual void visitDouble(Input&){};
+ virtual void visitBool(Input&){};
+ virtual void visitString(Input&){};
+ virtual void visitAnyObject(Input&){};
+
+ virtual void
+ visitUnknown(Input&)
+ {
+ throw error::AronException(__PRETTY_FUNCTION__, "Unknown type in visitor.");
+ }
+
virtual ~Visitor() = default;
};
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp b/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
index 528fe3577256ca175f1fd0e5c9256fd767d7b410..5445f99e2631d246f7aec3de22edf912581ac5bd 100644
--- a/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.cpp
@@ -26,14 +26,16 @@
namespace armarx::aron::type
{
- type::Descriptor ConstNlohmannJSONVisitor::GetDescriptor(Input& n)
+ type::Descriptor
+ ConstNlohmannJSONVisitor::GetDescriptor(Input& n)
{
std::string t = n[armarx::aron::type::rw::json::constantes::TYPE_SLUG];
return armarx::aron::type::rw::json::conversion::String2Descriptor.at(t);
}
- type::Descriptor ConstNlohmannJSONVisitor::getDescriptor(Input& n)
+ type::Descriptor
+ ConstNlohmannJSONVisitor::getDescriptor(Input& n)
{
return GetDescriptor(n);
}
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.h b/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.h
index b56b4ab1ccaa29cc43a5a65575c4e349a342c3fb..26ff48d325868e2fa32a48fb69b2c8b5acbbc652 100644
--- a/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.h
+++ b/source/RobotAPI/libraries/aron/core/type/visitor/nlohmannJSON/NlohmannJSONVisitor.h
@@ -30,8 +30,8 @@
// Simox
#include <SimoxUtility/json.h>
-#include "../Visitor.h"
#include "../../rw/json/Data.h"
+#include "../Visitor.h"
namespace armarx::aron::type
{
@@ -46,4 +46,4 @@ namespace armarx::aron::type
};
// TODO
-}
+} // namespace armarx::aron::type
diff --git a/source/RobotAPI/libraries/aron/filter/data/WhitelistFilter.h b/source/RobotAPI/libraries/aron/filter/data/WhitelistFilter.h
index 88a8c1f3bf1f7fa09bab560b4547838a59c0a13f..1a2a65ff458446e388401fb06b00def12811baa0 100644
--- a/source/RobotAPI/libraries/aron/filter/data/WhitelistFilter.h
+++ b/source/RobotAPI/libraries/aron/filter/data/WhitelistFilter.h
@@ -24,6 +24,7 @@
#include <algorithm>
#include <map>
#include <vector>
+
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
diff --git a/source/RobotAPI/libraries/aron/similarity/cosine.cpp b/source/RobotAPI/libraries/aron/similarity/cosine.cpp
index ac9e22cf073bc56969ae6c37b58fff01f1ec9ef0..4a4c32b95acf1de0aacb833068b82e36665f53f1 100644
--- a/source/RobotAPI/libraries/aron/similarity/cosine.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/cosine.cpp
@@ -1,8 +1,10 @@
#include "cosine.h"
+#include <cmath>
+
#include <SimoxUtility/algorithm/string.h>
+
#include <ArmarXCore/core/logging/Logging.h>
-#include <cmath>
namespace armarx::aron::similarity
{
@@ -14,8 +16,10 @@ namespace armarx::aron::similarity
int height = p1->getShape().at(1);
int dimensions = p1->getShape().at(2);
ARMARX_CHECK(dimensions == p2->getShape().at(2));
- if(dimensions > 3){
- ARMARX_INFO << "Trying to calculate cosine similarity for more than 3 channels, only first three channels will be looked at";
+ if (dimensions > 3)
+ {
+ ARMARX_INFO << "Trying to calculate cosine similarity for more than 3 channels, only "
+ "first three channels will be looked at";
}
Eigen::MatrixXf m1r(width, height);
Eigen::MatrixXf m2r(width, height);
@@ -28,8 +32,10 @@ namespace armarx::aron::similarity
auto image2 = p2->getDataAsVector();
- for(int x = 0; x < width; x++){
- for(int y = 0; y < height; y++){
+ for (int x = 0; x < width; x++)
+ {
+ for (int y = 0; y < height; y++)
+ {
//R-value matices:
int index = x + width * (y + 0 * height);
double element1 = image1.at(index);
@@ -67,7 +73,7 @@ namespace armarx::aron::similarity
double cosineB = 1.0 - (dotProductB / normProductB);
//ARMARX_INFO << VAROUT(cosineB);
- return (cosineR + cosineG + cosineB)/ 3;
+ return (cosineR + cosineG + cosineB) / 3;
}
-}
+} // namespace armarx::aron::similarity
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.cpp
index 2724dc3402316b4bb0c261dd67de788dd6fd1b6d..a96ac2fae8ff725e2f081df52dd78aba51a3e886 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.cpp
@@ -1,33 +1,41 @@
#include "FloatSimilarity.h"
+
#include <cmath>
-#include "ArmarXCore/core/logging/Logging.h"
-namespace armarx::aron::similarity{
+#include "ArmarXCore/core/logging/Logging.h"
-double FloatSimilarity::calculate_similarity(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2, Type t)
+namespace armarx::aron::similarity
{
- switch (t) {
- case Type::MAE:
- return calculateMAE(f1, f2);
- case Type::MSE:
- return calculateMSE(f1, f2);
- default:
- ARMARX_INFO << "Trying to calculate similarity with unknown similarity type";
- return -1;
- }
-}
-double FloatSimilarity::calculateMAE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2)
-{
+ double
+ FloatSimilarity::calculate_similarity(armarx::aron::data::FloatPtr f1,
+ armarx::aron::data::FloatPtr f2,
+ Type t)
+ {
+ switch (t)
+ {
+ case Type::MAE:
+ return calculateMAE(f1, f2);
+ case Type::MSE:
+ return calculateMSE(f1, f2);
+ default:
+ ARMARX_INFO << "Trying to calculate similarity with unknown similarity type";
+ return -1;
+ }
+ }
- return std::abs(f1->getValue() - f2->getValue());
-}
+ double
+ FloatSimilarity::calculateMAE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2)
+ {
-double FloatSimilarity::calculateMSE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2)
-{
- return std::pow(f1->getValue() - f2->getValue(), 2);
-}
+ return std::abs(f1->getValue() - f2->getValue());
+ }
+ double
+ FloatSimilarity::calculateMSE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2)
+ {
+ return std::pow(f1->getValue() - f2->getValue(), 2);
+ }
-}
+} // namespace armarx::aron::similarity
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h
index d325c6d61f2a5dc77ad53eb78a456da2dc07acf2..d847e80aeaea05961fed8a8446654a743f5a43ea 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/FloatSimilarity.h
@@ -1,22 +1,28 @@
#pragma once
#include <vector>
+
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Float.h"
-namespace armarx::aron::similarity::FloatSimilarity{
+namespace armarx::aron::similarity::FloatSimilarity
+{
- enum Type {
+ enum Type
+ {
MSE,
MAE,
NONE
};
- double calculate_similarity(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2, Type t);
+ double
+ calculate_similarity(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2, Type t);
- double calculate_similarity_multi(std::vector<armarx::aron::data::FloatPtr>& images, armarx::aron::data::FloatPtr p, Type type);
+ double calculate_similarity_multi(std::vector<armarx::aron::data::FloatPtr>& images,
+ armarx::aron::data::FloatPtr p,
+ Type type);
double calculateMAE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2);
double calculateMSE(armarx::aron::data::FloatPtr f1, armarx::aron::data::FloatPtr f2);
-}
+} // namespace armarx::aron::similarity::FloatSimilarity
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
index 467c9f1da493c54fe0de113cfa1127a6d082f37d..5b68317648f4636acbae01db6dfca643b3fb46b7 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.cpp
@@ -1,18 +1,23 @@
#include "NDArraySimilarity.h"
#include <cmath>
+
#include <ArmarXCore/core/logging/Logging.h>
-#include "mse.h"
-#include "mae.h"
-#include "chernoff.h"
+
#include "../../cosine.h"
+#include "chernoff.h"
+#include "mae.h"
+#include "mse.h"
-namespace armarx::aron::similarity{
+namespace armarx::aron::similarity
+{
- double NDArraySimilarity::calculate_similarity(data::NDArrayPtr p1, data::NDArrayPtr p2, Type type)
+ double
+ NDArraySimilarity::calculate_similarity(data::NDArrayPtr p1, data::NDArrayPtr p2, Type type)
{
- switch(type){
+ switch (type)
+ {
case Type::MSE:
//ARMARX_INFO << "Calculate MSE";
return armarx::aron::similarity::mse::compute_similarity(p1, p2);
@@ -23,14 +28,17 @@ namespace armarx::aron::similarity{
case Type::COSINE:
return armarx::aron::similarity::cosine::compute_similarity(p1, p2);
default:
- ARMARX_WARNING << "Trying to calculate similarity with unspecified similarity measurement";
+ ARMARX_WARNING
+ << "Trying to calculate similarity with unspecified similarity measurement";
return -1;
}
}
- std::string NDArraySimilarity::to_string(Type t)
+ std::string
+ NDArraySimilarity::to_string(Type t)
{
- switch(t){
+ switch (t)
+ {
case MSE:
return "MSE";
case MAE:
@@ -44,15 +52,21 @@ namespace armarx::aron::similarity{
}
}
- double NDArraySimilarity::calculate_similarity_multi(std::vector<data::NDArrayPtr> images, armarx::aron::data::NDArrayPtr p, Type type)
+ double
+ NDArraySimilarity::calculate_similarity_multi(std::vector<data::NDArrayPtr> images,
+ armarx::aron::data::NDArrayPtr p,
+ Type type)
{
double sim = 0;
- for(auto& image: images){
+ for (auto& image : images)
+ {
//ARMARX_INFO << "Before calculation";
sim += calculate_similarity(image, p, type);
//ARMARX_INFO << "Sim is currently: " << sim;
}
- return sim / (images.size() + 1); // to average it over the distances, makes it easier to find good parameters
+ return sim /
+ (images.size() +
+ 1); // to average it over the distances, makes it easier to find good parameters
}
-}
+} // namespace armarx::aron::similarity
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h
index 70413392f2cc8f2ee50a2c78d019607c3376feef..26d19e18bc7eb1f1fd387f789f306e076ba8fc39 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/NDArraySimilarity.h
@@ -1,11 +1,14 @@
#pragma once
#include <vector>
+
#include "RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h"
-namespace armarx::aron::similarity::NDArraySimilarity{
+namespace armarx::aron::similarity::NDArraySimilarity
+{
- enum Type {
+ enum Type
+ {
MSE,
MAE,
CHERNOFF,
@@ -15,7 +18,9 @@ namespace armarx::aron::similarity::NDArraySimilarity{
std::string to_string(Type t);
- double calculate_similarity(armarx::aron::data::NDArrayPtr p1, armarx::aron::data::NDArrayPtr p2, Type type);
+ double calculate_similarity(armarx::aron::data::NDArrayPtr p1,
+ armarx::aron::data::NDArrayPtr p2,
+ Type type);
/**
* @brief calculate_similarity_multi compares the image p with all images from the images vector, the dissimilarity values are simply summed up
@@ -24,6 +29,8 @@ namespace armarx::aron::similarity::NDArraySimilarity{
* @param type Type of dissimilarity measure used
* @return dissimilarity
*/
- double calculate_similarity_multi(std::vector<armarx::aron::data::NDArrayPtr> images, armarx::aron::data::NDArrayPtr p, Type type);
+ double calculate_similarity_multi(std::vector<armarx::aron::data::NDArrayPtr> images,
+ armarx::aron::data::NDArrayPtr p,
+ Type type);
-}
+} // namespace armarx::aron::similarity::NDArraySimilarity
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
index f91feb06aa18d66b7eb31800327d4dedd939938e..9a4c451cafbac8734c57b330d603da01598d73b5 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.cpp
@@ -1,14 +1,18 @@
#include "chernoff.h"
#include <cmath>
+
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
+
#include <ArmarXCore/core/logging/Logging.h>
namespace armarx::aron::similarity::chernoff
{
-
- double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2){
+
+ double
+ compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2)
+ {
//TODO: there seems to be an error that leads to the mean vectors always having the same
//value, which leads to the diff being zero which leads to distance being 0/inf/nan
//aron::data::NDArray image1 = *p1;
@@ -29,11 +33,12 @@ namespace armarx::aron::similarity::chernoff
std::vector<std::vector<double>> cov_two = calculate_covariance_matrix(s, mean_two);
//then calculate the Bhattacharyya distance and return
double distance = calculate_bhattacharyya_distance(mean_one, mean_two, cov_one, cov_two);
- ARMARX_INFO << "Chernoff distance: " << std::to_string(distance);
+ ARMARX_INFO << "Chernoff distance: " << std::to_string(distance);
return distance;
}
- std::vector<double> calculate_mean_values(data::NDArray array)
+ std::vector<double>
+ calculate_mean_values(data::NDArray array)
{
std::vector<double> mean(3, 0.0);
int width = array.getShape().at(0);
@@ -41,8 +46,10 @@ namespace armarx::aron::similarity::chernoff
int colors = array.getShape().at(2);
auto data = array.getDataAsVector();
- for (int row = 0; row < height; row++){
- for (int col = 0; col < width; col++){
+ for (int row = 0; row < height; row++)
+ {
+ for (int col = 0; col < width; col++)
+ {
double red = data.at(row * width * colors + col * colors);
double green = data.at(row * width * colors + col * colors + 1);
double blue = data.at(row * width * colors + col * colors + 2);
@@ -60,22 +67,27 @@ namespace armarx::aron::similarity::chernoff
return mean;
}
- data::NDArray normalize_ndarray(data::NDArrayPtr array, int j)
+ data::NDArray
+ normalize_ndarray(data::NDArrayPtr array, int j)
{
std::vector<unsigned char> data = array->getDataAsVector();
std::vector<unsigned char> new_data(data.size());
- for(int i = 0; i < data.size(); i++){
+ for (int i = 0; i < data.size(); i++)
+ {
double a = data.at(i);
- double n = a/j;
+ double n = a / j;
new_data.at(i) = n;
}
- armarx::aron::data::NDArray n(array->getShape(), array->getType(),new_data, array->getPath());
+ armarx::aron::data::NDArray n(
+ array->getShape(), array->getType(), new_data, array->getPath());
return n;
}
- std::vector<std::vector<double> > calculate_covariance_matrix(data::NDArray array, std::vector<double> mean = std::vector<double>())
+ std::vector<std::vector<double>>
+ calculate_covariance_matrix(data::NDArray array,
+ std::vector<double> mean = std::vector<double>())
{
int width = array.getShape().at(0);
int height = array.getShape().at(1);
@@ -83,9 +95,12 @@ namespace armarx::aron::similarity::chernoff
auto data = array.getDataAsVector();
std::vector<double> mean_vec(3, 0.0);
- if(mean.empty()){
+ if (mean.empty())
+ {
mean_vec = calculate_mean_values(array);
- } else {
+ }
+ else
+ {
mean_vec = mean;
}
@@ -93,8 +108,10 @@ namespace armarx::aron::similarity::chernoff
std::vector<std::vector<double>> covariance(3, std::vector<double>(3, 0.0));
- for(int row = 0; row < height; row++){
- for(int col = 0; col < width; col++){
+ for (int row = 0; row < height; row++)
+ {
+ for (int col = 0; col < width; col++)
+ {
double red = data.at(row * width * colors + col * colors);
double green = data.at(row * width * colors + col * colors + 1);
double blue = data.at(row * width * colors + col * colors + 2);
@@ -112,8 +129,10 @@ namespace armarx::aron::similarity::chernoff
}
}
- for(int i = 0; i < 3; i++){
- for (int j = 0; j < i; j++){
+ for (int i = 0; i < 3; i++)
+ {
+ for (int j = 0; j < i; j++)
+ {
covariance[i][j] /= numPixels;
covariance[j][i] = covariance[i][j];
}
@@ -123,7 +142,11 @@ namespace armarx::aron::similarity::chernoff
return covariance;
}
- double calculate_bhattacharyya_distance(std::vector<double> mean_one, std::vector<double> mean_two, std::vector<std::vector<double> > covariance_one, std::vector<std::vector<double> > covariance_two)
+ double
+ calculate_bhattacharyya_distance(std::vector<double> mean_one,
+ std::vector<double> mean_two,
+ std::vector<std::vector<double>> covariance_one,
+ std::vector<std::vector<double>> covariance_two)
{
//first make mean vectors and covariance matrices in Eigen:: objects
Eigen::VectorXd meanOne(3);
@@ -141,13 +164,13 @@ namespace armarx::aron::similarity::chernoff
Eigen::Matrix3d cov_one;
cov_one << covariance_one[0][0], covariance_one[0][1], covariance_one[0][2],
- covariance_one[1][0], covariance_one[1][1], covariance_one[1][2],
- covariance_one[2][0], covariance_one[2][1], covariance_one[2][2];
+ covariance_one[1][0], covariance_one[1][1], covariance_one[1][2], covariance_one[2][0],
+ covariance_one[2][1], covariance_one[2][2];
Eigen::Matrix3d cov_two;
cov_two << covariance_two[0][0], covariance_two[0][1], covariance_two[0][2],
- covariance_two[1][0], covariance_two[1][1], covariance_two[1][2],
- covariance_two[2][0], covariance_two[2][1], covariance_two[2][2];
+ covariance_two[1][0], covariance_two[1][1], covariance_two[1][2], covariance_two[2][0],
+ covariance_two[2][1], covariance_two[2][2];
//ARMARX_INFO << VAROUT(cov_one);
@@ -163,7 +186,7 @@ namespace armarx::aron::similarity::chernoff
double det_sigma = sigma.determinant();
//formula from https://www.kaggle.com/code/debanga/statistical-distances
- double bigger = meanDiff.transpose() * (sigma_inverse) * meanDiff;
+ double bigger = meanDiff.transpose() * (sigma_inverse)*meanDiff;
//ARMARX_INFO << "Bigger: " << std::to_string(bigger);
double term_one = 0.125 * bigger;
double sqr = std::sqrt(det_cov_one * det_cov_two);
@@ -177,5 +200,5 @@ namespace armarx::aron::similarity::chernoff
return distance;
}
-
-}
+
+} // namespace armarx::aron::similarity::chernoff
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.h b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.h
index 9c8dc173af489379e00435d9f1c98dda5e2e5f69..39d8ac5425cdb8ae8bab6ddbe22f677d290fb3ff 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.h
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/chernoff.h
@@ -30,7 +30,7 @@
namespace armarx::aron::similarity::chernoff
{
double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2);
-
+
armarx::aron::data::NDArray normalize_ndarray(armarx::aron::data::NDArrayPtr array, int i);
std::vector<double> calculate_mean_values(armarx::aron::data::NDArray array);
@@ -38,7 +38,8 @@ namespace armarx::aron::similarity::chernoff
std::vector<std::vector<double>> calculate_covariance_matrix(armarx::aron::data::NDArray array,
std::vector<double> mean);
- double calculate_bhattacharyya_distance(std::vector<double> mean_one, std::vector<double> mean_two,
+ double calculate_bhattacharyya_distance(std::vector<double> mean_one,
+ std::vector<double> mean_two,
std::vector<std::vector<double>> covariance_one,
std::vector<std::vector<double>> covariance_two);
-}
+} // namespace armarx::aron::similarity::chernoff
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mae.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/mae.cpp
index 3be0d5ef20e5a3325c90a69dbbca0083c7ebfb89..2463484ba20176a2c5cc7f1fc96aa0531cd0ba99 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/mae.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mae.cpp
@@ -3,7 +3,8 @@
namespace armarx::aron::similarity::mae
{
- double compute_similarity(const data::NDArrayPtr p1, const data::NDArrayPtr p2)
+ double
+ compute_similarity(const data::NDArrayPtr p1, const data::NDArrayPtr p2)
{
double sum = 0;
int width = p1->getShape().at(0);
@@ -15,9 +16,12 @@ namespace armarx::aron::similarity::mae
ARMARX_CHECK(first_image.size() == second_image.size());
- for (int w = 0; w < width; w++){
- for (int h = 0; h < height; h++){
- for (int c = 0; c < colors; c++){
+ for (int w = 0; w < width; w++)
+ {
+ for (int h = 0; h < height; h++)
+ {
+ for (int c = 0; c < colors; c++)
+ {
int k = h * width * colors + w * colors + c;
sum += std::abs(first_image.at(k) - second_image.at(k));
}
@@ -27,5 +31,4 @@ namespace armarx::aron::similarity::mae
}
-}
-
+} // namespace armarx::aron::similarity::mae
diff --git a/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp b/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
index 052cccff38bd7e9dd004a4d75efa578e01836604..de25ac45dd12bf682176730576ec4c611760f1b9 100644
--- a/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
+++ b/source/RobotAPI/libraries/aron/similarity/data/image/mse.cpp
@@ -1,8 +1,10 @@
#include "mse.h"
+#include <cmath>
+
#include <SimoxUtility/algorithm/string.h>
+
#include <ArmarXCore/core/logging/Logging.h>
-#include <cmath>
namespace armarx::aron::similarity
{
@@ -11,16 +13,22 @@ namespace armarx::aron::similarity
{
//ARMARX_INFO << "Begin MSE";
//auto start = std::chrono::high_resolution_clock::now();
- double sum = 0;
+ double sum = 0;
//TODO: check shapes have same size
//ARMARX_INFO << "One";
- if(p1 != nullptr && p2 != nullptr){
+ if (p1 != nullptr && p2 != nullptr)
+ {
p1->getShortName();
- } else {
- if(p1 == nullptr){
+ }
+ else
+ {
+ if (p1 == nullptr)
+ {
ARMARX_INFO << "P1 is nullptr";
- }else{
+ }
+ else
+ {
ARMARX_INFO << "P2 is Nullpointer";
}
}
@@ -39,7 +47,8 @@ namespace armarx::aron::similarity
//auto start = std::chrono::high_resolution_clock::now();
int rolling_number = 1; //TODO: make sure that no elements will be left over
- for(int i = 0; i < int(first_image.size()); i+= rolling_number){
+ for (int i = 0; i < int(first_image.size()); i += rolling_number)
+ {
//loop unrolling to shorten the needed computation time:
sum += std::pow(first_image.at(i) - second_image.at(i), 2);
/*
diff --git a/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp b/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp
index aac543a04b6ba1696dedc7a21c7d4ea125896e4e..a9be0e33a469a03ad921b1e952c5abd8cb39190f 100644
--- a/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp
+++ b/source/RobotAPI/libraries/aron/test/AronConversionTester.cpp
@@ -25,11 +25,11 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::aron::test
{
- AronConversionTester::AronConversionTester(dti::AronConversionTestInterfacePrx python) : interface(python)
+ AronConversionTester::AronConversionTester(dti::AronConversionTestInterfacePrx python) :
+ interface(python)
{
ARMARX_CHECK(python);
}
diff --git a/source/RobotAPI/libraries/aron/test/AronConversionTester.h b/source/RobotAPI/libraries/aron/test/AronConversionTester.h
index b7ad4b8dfdcf55a37f8efc3116c27440d925d428..e034f5ca2beb5848b905248f27a68768d0485f2b 100644
--- a/source/RobotAPI/libraries/aron/test/AronConversionTester.h
+++ b/source/RobotAPI/libraries/aron/test/AronConversionTester.h
@@ -28,7 +28,6 @@
#include <RobotAPI/interface/aron/test/AronConversionTestInterface.h>
-
namespace armarx::aron::test
{
@@ -100,7 +99,8 @@ namespace armarx::aron::test
}
else
{
- ARMARX_WARNING << ss.str() << "Conversion in Python component failed: \n" << res.errorMessage;
+ ARMARX_WARNING << ss.str() << "Conversion in Python component failed: \n"
+ << res.errorMessage;
}
}
diff --git a/source/RobotAPI/libraries/aron_component_config/ComponentPlugin.h b/source/RobotAPI/libraries/aron_component_config/ComponentPlugin.h
index 08e496d77a9d28f09ade6599f692a89ec06cdbc4..30a38c57696108b1bbddeeb2ab794185bb3b805f 100644
--- a/source/RobotAPI/libraries/aron_component_config/ComponentPlugin.h
+++ b/source/RobotAPI/libraries/aron_component_config/ComponentPlugin.h
@@ -65,7 +65,8 @@ namespace armarx::plugins
postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties) override
{
ARMARX_TRACE;
- armarx::aron::component_config::PropertyDefinitionSetterVisitor vis(properties, prefix());
+ armarx::aron::component_config::PropertyDefinitionSetterVisitor vis(properties,
+ prefix());
const auto& config = config_.getUpToDateReadBuffer();
armarx::aron::data::visitRecursive(vis, config.toAron(), config.ToAronType());
ComponentPlugin::postCreatePropertyDefinitions(properties);
diff --git a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp
index 1804cb7326160912cdf214aebb8f887c79f4d4c2..450e5f746b602f470b91d82554f2479dd96411d8 100644
--- a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.cpp
@@ -22,23 +22,28 @@
*/
#include "CartesianFeedForwardPositionController.h"
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/AbstractFunctionR1R6.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
namespace armarx
{
- CartesianFeedForwardPositionController::CartesianFeedForwardPositionController(const VirtualRobot::RobotNodePtr& tcp)
- : tcp(tcp)
+ CartesianFeedForwardPositionController::CartesianFeedForwardPositionController(
+ const VirtualRobot::RobotNodePtr& tcp) :
+ tcp(tcp)
{
-
}
- Eigen::VectorXf CartesianFeedForwardPositionController::calculate(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t, VirtualRobot::IKSolver::CartesianSelection mode)
+ Eigen::VectorXf
+ CartesianFeedForwardPositionController::calculate(
+ const ::math::AbstractFunctionR1R6Ptr& trajectory,
+ float t,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
@@ -48,7 +53,7 @@ namespace armarx
{
Eigen::Vector3f targetPos = trajectory->GetPosition(t);
Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
- Eigen::Vector3f posVel = errPos * KpPos;
+ Eigen::Vector3f posVel = errPos * KpPos;
if (enableFeedForward)
{
posVel += trajectory->GetPositionDerivative(t);
@@ -83,14 +88,20 @@ namespace armarx
return cartesianVel;
}
- float CartesianFeedForwardPositionController::getPositionError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+ float
+ CartesianFeedForwardPositionController::getPositionError(
+ const ::math::AbstractFunctionR1R6Ptr& trajectory,
+ float t)
{
Eigen::Vector3f targetPos = trajectory->GetPosition(t);
Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
return errPos.norm();
}
- float CartesianFeedForwardPositionController::getOrientationError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+ float
+ CartesianFeedForwardPositionController::getOrientationError(
+ const ::math::AbstractFunctionR1R6Ptr& trajectory,
+ float t)
{
Eigen::Matrix3f targetOri = trajectory->GetOrientation(t).toRotationMatrix();
Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
@@ -99,14 +110,19 @@ namespace armarx
return aa.angle();
}
- Eigen::Vector3f CartesianFeedForwardPositionController::getPositionDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+ Eigen::Vector3f
+ CartesianFeedForwardPositionController::getPositionDiff(
+ const ::math::AbstractFunctionR1R6Ptr& trajectory,
+ float t)
{
Eigen::Vector3f targetPos = trajectory->GetPosition(t);
return targetPos - tcp->getPositionInRootFrame();
-
}
- Eigen::Vector3f CartesianFeedForwardPositionController::getOrientationDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t)
+ Eigen::Vector3f
+ CartesianFeedForwardPositionController::getOrientationDiff(
+ const ::math::AbstractFunctionR1R6Ptr& trajectory,
+ float t)
{
Eigen::Matrix3f targetOri = trajectory->GetOrientation(t).toRotationMatrix();
Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
@@ -114,4 +130,4 @@ namespace armarx
Eigen::AngleAxisf aa(oriDir);
return aa.axis() * aa.angle();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h
index c40a394645c3b8c188315720b9e672e4b4e5ea00..81f063d07c7a378fd1e958ebbd8ba716696d4990 100644
--- a/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianFeedForwardPositionController.h
@@ -23,27 +23,31 @@
#pragma once
+#include <Eigen/Dense>
+
+#include <VirtualRobot/IK/IKSolver.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/math/MathForwardDefinitions.h>
-#include <VirtualRobot/IK/IKSolver.h>
-
-#include <Eigen/Dense>
namespace armarx
{
class CartesianFeedForwardPositionController;
- using CartesianFeedForwardPositionControllerPtr = std::shared_ptr<CartesianFeedForwardPositionController>;
+ using CartesianFeedForwardPositionControllerPtr =
+ std::shared_ptr<CartesianFeedForwardPositionController>;
class CartesianFeedForwardPositionController
{
public:
CartesianFeedForwardPositionController(const VirtualRobot::RobotNodePtr& tcp);
- Eigen::VectorXf calculate(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t, VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const ::math::AbstractFunctionR1R6Ptr& trajectory,
+ float t,
+ VirtualRobot::IKSolver::CartesianSelection mode);
float getPositionError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t);
float getOrientationError(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t);
Eigen::Vector3f getPositionDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t);
- Eigen::Vector3f getOrientationDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory, float t);
+ Eigen::Vector3f getOrientationDiff(const ::math::AbstractFunctionR1R6Ptr& trajectory,
+ float t);
float KpPos = 1;
float KpOri = 1;
@@ -54,4 +58,4 @@ namespace armarx
private:
VirtualRobot::RobotNodePtr tcp;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
index 5c45b8873d933c7d53728603c703af3e4ed0141a..085a9ea5a91fe54f60e317da1f0eb1897b23c8d9 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
@@ -22,26 +22,27 @@
*/
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
#include "CartesianNaturalPositionController.h"
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/math/Helpers.h>
-#include <VirtualRobot/MathTools.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
namespace armarx
{
- CartesianNaturalPositionController::CartesianNaturalPositionController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& elbow,
- float maxPositionAcceleration,
- float maxOrientationAcceleration,
- float maxNullspaceAcceleration,
- const VirtualRobot::RobotNodePtr& tcp)
- :
+ CartesianNaturalPositionController::CartesianNaturalPositionController(
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& elbow,
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const VirtualRobot::RobotNodePtr& tcp) :
vcTcp(rns,
Eigen::VectorXf::Constant(rns->getSize(), 0.f),
VirtualRobot::IKSolver::CartesianSelection::All,
@@ -59,7 +60,9 @@ namespace armarx
clearNullspaceTarget();
}
- Eigen::VectorXf CartesianNaturalPositionController::LimitInfNormTo(const Eigen::VectorXf& vec, const std::vector<float>& maxValue)
+ Eigen::VectorXf
+ CartesianNaturalPositionController::LimitInfNormTo(const Eigen::VectorXf& vec,
+ const std::vector<float>& maxValue)
{
if (maxValue.size() == 0)
{
@@ -81,7 +84,8 @@ namespace armarx
return vec * scale;
}
- const Eigen::VectorXf CartesianNaturalPositionController::calculateNullspaceTargetDifference()
+ const Eigen::VectorXf
+ CartesianNaturalPositionController::calculateNullspaceTargetDifference()
{
Eigen::VectorXf jvNT = Eigen::VectorXf(rns->getSize());
for (size_t i = 0; i < rns->getSize(); i++)
@@ -103,7 +107,9 @@ namespace armarx
return jvNT;
}
- const Eigen::VectorXf CartesianNaturalPositionController::calculate(float dt, VirtualRobot::IKSolver::CartesianSelection mode)
+ const Eigen::VectorXf
+ CartesianNaturalPositionController::calculate(float dt,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
//int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
@@ -131,9 +137,11 @@ namespace armarx
if (nullSpaceControlEnabled)
{
Eigen::VectorXf jnv = Eigen::VectorXf::Zero(rns->getSize());
- Eigen::VectorXf jvElb = vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
+ Eigen::VectorXf jvElb =
+ vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
- Eigen::VectorXf jvLA = jointLimitAvoidanceKp * vcTcp.controller.calculateJointLimitAvoidance();
+ Eigen::VectorXf jvLA =
+ jointLimitAvoidanceKp * vcTcp.controller.calculateJointLimitAvoidance();
Eigen::VectorXf jvNT = nullspaceTargetKp * calculateNullspaceTargetDifference();
//ARMARX_IMPORTANT << deactivateSpam(1) << VAROUT(jvNT);
@@ -162,30 +170,39 @@ namespace armarx
return jvClamped;
}
-
- void CartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget)
+ void
+ CartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::Vector3f& elbowTarget)
{
this->tcpTarget = tcpTarget;
this->elbowTarget = elbowTarget;
}
- void CartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
+ void
+ CartesianNaturalPositionController::setFeedForwardVelocity(
+ const Eigen::Vector6f& feedForwardVelocity)
{
this->feedForwardVelocity = feedForwardVelocity;
}
- void CartesianNaturalPositionController::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
+ void
+ CartesianNaturalPositionController::setFeedForwardVelocity(
+ const Eigen::Vector3f& feedForwardVelocityPos,
+ const Eigen::Vector3f& feedForwardVelocityOri)
{
feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
}
- void CartesianNaturalPositionController::setNullspaceTarget(const Eigen::VectorXf& nullspaceTarget)
+ void
+ CartesianNaturalPositionController::setNullspaceTarget(const Eigen::VectorXf& nullspaceTarget)
{
this->nullspaceTarget = nullspaceTarget;
}
- void CartesianNaturalPositionController::setNullspaceTarget(const std::vector<float>& nullspaceTarget)
+ void
+ CartesianNaturalPositionController::setNullspaceTarget(
+ const std::vector<float>& nullspaceTarget)
{
ARMARX_CHECK_EXPRESSION(rns->getSize() == nullspaceTarget.size());
for (size_t i = 0; i < rns->getSize(); i++)
@@ -194,57 +211,70 @@ namespace armarx
}
}
- void CartesianNaturalPositionController::clearNullspaceTarget()
+ void
+ CartesianNaturalPositionController::clearNullspaceTarget()
{
this->nullspaceTarget = Eigen::VectorXf::Constant(rns->getSize(), std::nanf(""));
}
- void CartesianNaturalPositionController::clearFeedForwardVelocity()
+ void
+ CartesianNaturalPositionController::clearFeedForwardVelocity()
{
feedForwardVelocity = Eigen::Vector6f::Zero();
}
- float CartesianNaturalPositionController::getPositionError() const
+ float
+ CartesianNaturalPositionController::getPositionError() const
{
return pcTcp.getPositionError(getCurrentTarget());
}
- float CartesianNaturalPositionController::getOrientationError() const
+ float
+ CartesianNaturalPositionController::getOrientationError() const
{
return pcTcp.getOrientationError(getCurrentTarget());
}
- bool CartesianNaturalPositionController::isTargetReached() const
+ bool
+ CartesianNaturalPositionController::isTargetReached() const
{
- return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+ return getPositionError() < thresholdPositionReached &&
+ getOrientationError() < thresholdOrientationReached;
}
- bool CartesianNaturalPositionController::isTargetNear() const
+ bool
+ CartesianNaturalPositionController::isTargetNear() const
{
- return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+ return getPositionError() < thresholdPositionNear &&
+ getOrientationError() < thresholdOrientationNear;
}
- const Eigen::Matrix4f& CartesianNaturalPositionController::getCurrentTarget() const
+ const Eigen::Matrix4f&
+ CartesianNaturalPositionController::getCurrentTarget() const
{
return tcpTarget;
}
- const Eigen::Vector3f CartesianNaturalPositionController::getCurrentTargetPosition() const
+ const Eigen::Vector3f
+ CartesianNaturalPositionController::getCurrentTargetPosition() const
{
return ::math::Helpers::GetPosition(tcpTarget);
}
- const Eigen::Vector3f& CartesianNaturalPositionController::getCurrentElbowTarget() const
+ const Eigen::Vector3f&
+ CartesianNaturalPositionController::getCurrentElbowTarget() const
{
return elbowTarget;
}
- const Eigen::VectorXf& CartesianNaturalPositionController::getCurrentNullspaceTarget() const
+ const Eigen::VectorXf&
+ CartesianNaturalPositionController::getCurrentNullspaceTarget() const
{
return nullspaceTarget;
}
- bool CartesianNaturalPositionController::hasNullspaceTarget() const
+ bool
+ CartesianNaturalPositionController::hasNullspaceTarget() const
{
for (int i = 0; i < nullspaceTarget.rows(); i++)
{
@@ -256,58 +286,63 @@ namespace armarx
return false;
}
- void CartesianNaturalPositionController::setNullSpaceControl(bool enabled)
+ void
+ CartesianNaturalPositionController::setNullSpaceControl(bool enabled)
{
nullSpaceControlEnabled = enabled;
}
- std::string CartesianNaturalPositionController::getStatusText()
+ std::string
+ CartesianNaturalPositionController::getStatusText()
{
std::stringstream ss;
ss.precision(2);
- ss << std::fixed << "distance: " << getPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
+ ss << std::fixed << "distance: " << getPositionError() << " mm "
+ << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
return ss.str();
}
- void CartesianNaturalPositionController::setConfig(const CartesianNaturalPositionControllerConfig& cfg)
+ void
+ CartesianNaturalPositionController::setConfig(
+ const CartesianNaturalPositionControllerConfig& cfg)
{
- jointLimitAvoidanceKp = cfg.jointLimitAvoidanceKp;
-
- thresholdOrientationNear = cfg.thresholdOrientationNear;
- thresholdOrientationReached = cfg.thresholdOrientationReached;
- thresholdPositionNear = cfg.thresholdPositionNear;
- thresholdPositionReached = cfg.thresholdPositionReached;
-
- maxJointVelocity = cfg.maxJointVelocity;
- maxNullspaceVelocity = cfg.maxNullspaceVelocity;
-
- nullspaceTargetKp = cfg.nullspaceTargetKp;
-
- pcTcp.maxPosVel = cfg.maxTcpPosVel;
- pcTcp.maxOriVel = cfg.maxTcpOriVel;
- pcTcp.KpOri = cfg.KpOri;
- pcTcp.KpPos = cfg.KpPos;
- pcElb.maxPosVel = cfg.maxElbPosVel;
- pcElb.KpPos = cfg.elbowKp;
-
- vcTcp.setMaxAccelerations(
- cfg.maxPositionAcceleration,
- cfg.maxOrientationAcceleration,
- cfg.maxNullspaceAcceleration
- );
+ jointLimitAvoidanceKp = cfg.jointLimitAvoidanceKp;
+
+ thresholdOrientationNear = cfg.thresholdOrientationNear;
+ thresholdOrientationReached = cfg.thresholdOrientationReached;
+ thresholdPositionNear = cfg.thresholdPositionNear;
+ thresholdPositionReached = cfg.thresholdPositionReached;
+
+ maxJointVelocity = cfg.maxJointVelocity;
+ maxNullspaceVelocity = cfg.maxNullspaceVelocity;
+
+ nullspaceTargetKp = cfg.nullspaceTargetKp;
+
+ pcTcp.maxPosVel = cfg.maxTcpPosVel;
+ pcTcp.maxOriVel = cfg.maxTcpOriVel;
+ pcTcp.KpOri = cfg.KpOri;
+ pcTcp.KpPos = cfg.KpPos;
+ pcElb.maxPosVel = cfg.maxElbPosVel;
+ pcElb.KpPos = cfg.elbowKp;
+
+ vcTcp.setMaxAccelerations(cfg.maxPositionAcceleration,
+ cfg.maxOrientationAcceleration,
+ cfg.maxNullspaceAcceleration);
if (cfg.jointCosts.size() > 0)
{
vcTcp.controller.setJointCosts(cfg.jointCosts);
}
}
- Eigen::VectorXf CartesianNaturalPositionController::actualTcpVel(const Eigen::VectorXf& jointVel)
+ Eigen::VectorXf
+ CartesianNaturalPositionController::actualTcpVel(const Eigen::VectorXf& jointVel)
{
return (vcTcp.controller.ik->getJacobianMatrix(VirtualRobot::IKSolver::All) * jointVel);
}
- Eigen::VectorXf CartesianNaturalPositionController::actualElbVel(const Eigen::VectorXf& jointVel)
+ Eigen::VectorXf
+ CartesianNaturalPositionController::actualElbVel(const Eigen::VectorXf& jointVel)
{
return (vcElb.jacobi * jointVel);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
index 81c51f351c69f315cb48d3efc8fd26f46a037625..16a28453ea6f839d0961f3f6c2713140bd06d951 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
@@ -29,9 +29,9 @@
#include <VirtualRobot/Robot.h>
+#include <RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.h>
#include "CartesianVelocityControllerWithRamp.h"
@@ -40,11 +40,11 @@ namespace Eigen
using Vector6f = Matrix<float, 6, 1>;
}
-
namespace armarx
{
class CartesianNaturalPositionController;
- using CartesianNaturalPositionControllerPtr = std::shared_ptr<CartesianNaturalPositionController>;
+ using CartesianNaturalPositionControllerPtr =
+ std::shared_ptr<CartesianNaturalPositionController>;
class CartesianNaturalPositionController
{
@@ -54,17 +54,20 @@ namespace armarx
float maxPositionAcceleration,
float maxOrientationAcceleration,
float maxNullspaceAcceleration,
- const VirtualRobot::RobotNodePtr& tcp = nullptr
- );
+ const VirtualRobot::RobotNodePtr& tcp = nullptr);
- static Eigen::VectorXf LimitInfNormTo(const Eigen::VectorXf& vec, const std::vector<float>& maxValue);
+ static Eigen::VectorXf LimitInfNormTo(const Eigen::VectorXf& vec,
+ const std::vector<float>& maxValue);
const Eigen::VectorXf calculateNullspaceTargetDifference();
- const Eigen::VectorXf calculate(float dt, VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All);
+ const Eigen::VectorXf
+ calculate(float dt,
+ VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All);
void setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget);
void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
- void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
+ void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos,
+ const Eigen::Vector3f& feedForwardVelocityOri);
void setNullspaceTarget(const Eigen::VectorXf& nullspaceTarget);
void setNullspaceTarget(const std::vector<float>& nullspaceTarget);
void clearNullspaceTarget();
@@ -93,7 +96,7 @@ namespace armarx
Eigen::VectorXf actualTcpVel(const Eigen::VectorXf& jointVel);
Eigen::VectorXf actualElbVel(const Eigen::VectorXf& jointVel);
- CartesianVelocityControllerWithRamp vcTcp;
+ CartesianVelocityControllerWithRamp vcTcp;
CartesianPositionController pcTcp;
CartesianVelocityController vcElb;
CartesianPositionController pcElb;
@@ -103,10 +106,10 @@ namespace armarx
Eigen::Vector3f elbowTarget;
Eigen::VectorXf nullspaceTarget = Eigen::VectorXf(0);
- float thresholdPositionReached = 0.f;
- float thresholdOrientationReached = 0.f;
- float thresholdPositionNear = 0.f;
- float thresholdOrientationNear = 0.f;
+ float thresholdPositionReached = 0.f;
+ float thresholdOrientationReached = 0.f;
+ float thresholdPositionNear = 0.f;
+ float thresholdOrientationNear = 0.f;
std::vector<float> maxJointVelocity;
std::vector<float> maxNullspaceVelocity;
@@ -114,15 +117,15 @@ namespace armarx
Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
Eigen::Vector6f cartesianVelocity = Eigen::Vector6f::Zero();
- bool autoClearFeedForward = true;
+ bool autoClearFeedForward = true;
- bool nullSpaceControlEnabled = true;
- float jointLimitAvoidanceScale = 0.f;
- float jointLimitAvoidanceKp = 0.f;
- float nullspaceTargetKp = 0;
+ bool nullSpaceControlEnabled = true;
+ float jointLimitAvoidanceScale = 0.f;
+ float jointLimitAvoidanceKp = 0.f;
+ float nullspaceTargetKp = 0;
private:
VirtualRobot::RobotNodeSetPtr rns;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index a2d75227f2cf8739a62bdd34a7301a2baca5c6e8..09863ef8198fa11b90ffd0524522539c742eee39 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -23,24 +23,27 @@
#include "CartesianPositionController.h"
-
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
namespace armarx
{
- CartesianPositionController::CartesianPositionController(const VirtualRobot::RobotNodePtr& tcp,
- const VirtualRobot::RobotNodePtr& referenceFrame)
- : tcp(tcp), referenceFrame(referenceFrame ? referenceFrame : tcp->getRobot()->getRootNode())
+ CartesianPositionController::CartesianPositionController(
+ const VirtualRobot::RobotNodePtr& tcp,
+ const VirtualRobot::RobotNodePtr& referenceFrame) :
+ tcp(tcp), referenceFrame(referenceFrame ? referenceFrame : tcp->getRobot()->getRootNode())
{
}
- Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
+ Eigen::VectorXf
+ CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose,
+ VirtualRobot::IKSolver::CartesianSelection mode) const
{
ARMARX_TRACE;
int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
@@ -53,7 +56,7 @@ namespace armarx
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
Eigen::Vector3f currentPos = tcp->getPositionInFrame(referenceFrame);
Eigen::Vector3f errPos = targetPos - currentPos;
- Eigen::Vector3f posVel = errPos * KpPos;
+ Eigen::Vector3f posVel = errPos * KpPos;
if (maxPosVel > 0)
{
posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
@@ -80,13 +83,14 @@ namespace armarx
return cartesianVel;
}
- Eigen::VectorXf CartesianPositionController::calculatePos(const Eigen::Vector3f& targetPos) const
+ Eigen::VectorXf
+ CartesianPositionController::calculatePos(const Eigen::Vector3f& targetPos) const
{
ARMARX_TRACE;
Eigen::VectorXf cartesianVel(3);
Eigen::Vector3f currentPos = tcp->getPositionInFrame(referenceFrame);
Eigen::Vector3f errPos = targetPos - currentPos;
- Eigen::Vector3f posVel = errPos * KpPos;
+ Eigen::Vector3f posVel = errPos * KpPos;
if (maxPosVel > 0)
{
ARMARX_TRACE;
@@ -96,52 +100,65 @@ namespace armarx
return cartesianVel;
}
- float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose) const
+ float
+ CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose) const
{
return GetPositionError(targetPose, tcp, referenceFrame);
}
- float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose) const
+ float
+ CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose) const
{
return GetOrientationError(targetPose, tcp, referenceFrame);
}
- float CartesianPositionController::GetPositionError(const Eigen::Matrix4f& targetPose,
- const VirtualRobot::RobotNodePtr& tcp,
- const VirtualRobot::RobotNodePtr& referenceFrame)
+ float
+ CartesianPositionController::GetPositionError(const Eigen::Matrix4f& targetPose,
+ const VirtualRobot::RobotNodePtr& tcp,
+ const VirtualRobot::RobotNodePtr& referenceFrame)
{
ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
- Eigen::Vector3f tcpPos = referenceFrame ? tcp->getPositionInFrame(referenceFrame) : tcp->getPositionInRootFrame();
+ Eigen::Vector3f tcpPos = referenceFrame ? tcp->getPositionInFrame(referenceFrame)
+ : tcp->getPositionInRootFrame();
Eigen::Vector3f errPos = targetPos - tcpPos;
return errPos.norm();
}
- float CartesianPositionController::GetOrientationError(const Eigen::Matrix4f& targetPose,
- const VirtualRobot::RobotNodePtr& tcp,
- const VirtualRobot::RobotNodePtr& referenceFrame)
+ float
+ CartesianPositionController::GetOrientationError(
+ const Eigen::Matrix4f& targetPose,
+ const VirtualRobot::RobotNodePtr& tcp,
+ const VirtualRobot::RobotNodePtr& referenceFrame)
{
ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
- Eigen::Matrix4f tcpPose = referenceFrame ? tcp->getPoseInFrame(referenceFrame) : tcp->getPoseInRootFrame();
+ Eigen::Matrix4f tcpPose =
+ referenceFrame ? tcp->getPoseInFrame(referenceFrame) : tcp->getPoseInRootFrame();
Eigen::Matrix3f tcpOri = tcpPose.block<3, 3>(0, 0);
Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
Eigen::AngleAxisf aa(oriDir);
return aa.angle();
}
- bool CartesianPositionController::Reached(const Eigen::Matrix4f& targetPose,
- const VirtualRobot::RobotNodePtr& tcp,
- bool checkOri,
- float thresholdPosReached,
- float thresholdOriReached,
- const VirtualRobot::RobotNodePtr& referenceFrame)
+ bool
+ CartesianPositionController::Reached(const Eigen::Matrix4f& targetPose,
+ const VirtualRobot::RobotNodePtr& tcp,
+ bool checkOri,
+ float thresholdPosReached,
+ float thresholdOriReached,
+ const VirtualRobot::RobotNodePtr& referenceFrame)
{
- return GetPositionError(targetPose, tcp, referenceFrame) < thresholdPosReached
- && (!checkOri || GetOrientationError(targetPose, tcp, referenceFrame) < thresholdOriReached);
+ return GetPositionError(targetPose, tcp, referenceFrame) < thresholdPosReached &&
+ (!checkOri ||
+ GetOrientationError(targetPose, tcp, referenceFrame) < thresholdOriReached);
}
- bool CartesianPositionController::reached(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode, float thresholdPosReached, float thresholdOriReached)
+ bool
+ CartesianPositionController::reached(const Eigen::Matrix4f& targetPose,
+ VirtualRobot::IKSolver::CartesianSelection mode,
+ float thresholdPosReached,
+ float thresholdOriReached)
{
ARMARX_TRACE;
if (mode & VirtualRobot::IKSolver::Position)
@@ -161,19 +178,22 @@ namespace armarx
return true;
}
- Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
+ Eigen::Vector3f
+ CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
{
ARMARX_TRACE;
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
return targetPos - tcp->getPositionInFrame(referenceFrame);
}
- Eigen::Vector3f CartesianPositionController::getPositionDiffVec3(const Eigen::Vector3f& targetPosition) const
+ Eigen::Vector3f
+ CartesianPositionController::getPositionDiffVec3(const Eigen::Vector3f& targetPosition) const
{
return targetPosition - tcp->getPositionInFrame(referenceFrame);
}
- Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
+ Eigen::Vector3f
+ CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
{
ARMARX_TRACE;
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
@@ -183,13 +203,12 @@ namespace armarx
return aa.axis() * aa.angle();
}
- VirtualRobot::RobotNodePtr CartesianPositionController::getTcp() const
+ VirtualRobot::RobotNodePtr
+ CartesianPositionController::getTcp() const
{
return tcp;
}
-
-
#define SS_OUT(x) ss << #x << " = " << x << "\n"
#define SET_FLT(x) obj->setFloat(#x, x)
#define GET_FLT(x) x = obj->getFloat(#x);
@@ -198,7 +217,8 @@ namespace armarx
{
}
- std::string CartesianPositionControllerConfig::output(const Ice::Current&) const
+ std::string
+ CartesianPositionControllerConfig::output(const Ice::Current&) const
{
std::stringstream ss;
@@ -214,7 +234,9 @@ namespace armarx
return ss.str();
}
- void CartesianPositionControllerConfig::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+ void
+ CartesianPositionControllerConfig::serialize(const ObjectSerializerBasePtr& serializer,
+ const Ice::Current&) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -226,10 +248,11 @@ namespace armarx
SET_FLT(thresholdOrientationReached);
SET_FLT(thresholdPositionNear);
SET_FLT(thresholdPositionReached);
-
}
- void CartesianPositionControllerConfig::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
+ void
+ CartesianPositionControllerConfig::deserialize(const ObjectSerializerBasePtr& serializer,
+ const Ice::Current&)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -241,6 +264,5 @@ namespace armarx
GET_FLT(thresholdOrientationReached);
GET_FLT(thresholdPositionNear);
GET_FLT(thresholdPositionReached);
-
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index df4272087876ef431d2c49ed7800bc1a57d3992d..8d8ce0d1cb4ee90c573fa8a5454aaeae5a6edd76 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -23,14 +23,15 @@
#pragma once
-#include <VirtualRobot/VirtualRobot.h>
+#include <Eigen/Dense>
+
#include <VirtualRobot/IK/IKSolver.h>
+#include <VirtualRobot/VirtualRobot.h>
-#include <Eigen/Dense>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include <ArmarXCore/observers/variant/Variant.h>
#include <RobotAPI/interface/core/CartesianPositionControllerConfig.h>
-#include <ArmarXCore/observers/variant/Variant.h>
-#include <ArmarXCore/observers/AbstractObjectSerializer.h>
namespace armarx
{
@@ -47,7 +48,8 @@ namespace armarx
CartesianPositionController(CartesianPositionController&&) = default;
CartesianPositionController& operator=(CartesianPositionController&&) = default;
- Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const;
+ Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose,
+ VirtualRobot::IKSolver::CartesianSelection mode) const;
Eigen::VectorXf calculatePos(const Eigen::Vector3f& targetPos) const;
float getPositionError(const Eigen::Matrix4f& targetPose) const;
@@ -55,16 +57,20 @@ namespace armarx
static float GetPositionError(const Eigen::Matrix4f& targetPose,
const VirtualRobot::RobotNodePtr& tcp,
const VirtualRobot::RobotNodePtr& referenceFrame = nullptr);
- static float GetOrientationError(const Eigen::Matrix4f& targetPose,
- const VirtualRobot::RobotNodePtr& tcp,
- const VirtualRobot::RobotNodePtr& referenceFrame = nullptr);
+ static float
+ GetOrientationError(const Eigen::Matrix4f& targetPose,
+ const VirtualRobot::RobotNodePtr& tcp,
+ const VirtualRobot::RobotNodePtr& referenceFrame = nullptr);
static bool Reached(const Eigen::Matrix4f& targetPose,
const VirtualRobot::RobotNodePtr& tcp,
bool checkOri,
float thresholdPosReached,
float thresholdOriReached,
const VirtualRobot::RobotNodePtr& referenceFrame = nullptr);
- bool reached(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode, float thresholdPosReached, float thresholdOriReached);
+ bool reached(const Eigen::Matrix4f& targetPose,
+ VirtualRobot::IKSolver::CartesianSelection mode,
+ float thresholdPosReached,
+ float thresholdOriReached);
Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
Eigen::Vector3f getPositionDiffVec3(const Eigen::Vector3f& targetPosition) const;
@@ -81,12 +87,14 @@ namespace armarx
VirtualRobot::RobotNodePtr tcp;
VirtualRobot::RobotNodePtr referenceFrame;
};
-}
+} // namespace armarx
+
namespace armarx::VariantType
{
// variant types
- const VariantTypeId CartesianPositionControllerConfig = Variant::addTypeName("::armarx::CartesianPositionControllerConfig");
-}
+ const VariantTypeId CartesianPositionControllerConfig =
+ Variant::addTypeName("::armarx::CartesianPositionControllerConfig");
+} // namespace armarx::VariantType
namespace armarx
{
@@ -96,35 +104,47 @@ namespace armarx
CartesianPositionControllerConfig();
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
+ Ice::ObjectPtr
+ ice_clone() const override
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& = ::Ice::Current()) const override
+
+ VariantDataClassPtr
+ clone(const Ice::Current& = ::Ice::Current()) const override
{
return new CartesianPositionControllerConfig(*this);
}
+
std::string output(const Ice::Current& c = ::Ice::Current()) const override;
- VariantTypeId getType(const Ice::Current& = ::Ice::Current()) const override
+
+ VariantTypeId
+ getType(const Ice::Current& = ::Ice::Current()) const override
{
return VariantType::CartesianPositionControllerConfig;
}
- bool validate(const Ice::Current& = ::Ice::Current()) override
+
+ bool
+ validate(const Ice::Current& = ::Ice::Current()) override
{
return true;
}
- friend std::ostream& operator<<(std::ostream& stream, const CartesianPositionControllerConfig& rhs)
+ friend std::ostream&
+ operator<<(std::ostream& stream, const CartesianPositionControllerConfig& rhs)
{
- stream << "CartesianPositionControllerConfig: " << std::endl << rhs.output() << std::endl;
+ stream << "CartesianPositionControllerConfig: " << std::endl
+ << rhs.output() << std::endl;
return stream;
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
-
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = ::Ice::Current()) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = ::Ice::Current()) override;
};
- typedef IceInternal::Handle<CartesianPositionControllerConfig> CartesianPositionControllerConfigPtr;
-}
+ typedef IceInternal::Handle<CartesianPositionControllerConfig>
+ CartesianPositionControllerConfigPtr;
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index 636104a3993a63b694445d19c1492303280e24bf..7e291751ed80f8397525c8617accbdb1de0b64b9 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -23,24 +23,27 @@
#include "CartesianVelocityController.h"
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <Eigen/Core>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <Eigen/Core>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/core/math/MathUtils.h>
using namespace armarx;
-CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod, bool considerJointLimits)
- : rns(rns),
- _considerJointLimits(considerJointLimits)
+CartesianVelocityController::CartesianVelocityController(
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& tcp,
+ const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod,
+ bool considerJointLimits) :
+ rns(rns), _considerJointLimits(considerJointLimits)
{
//ARMARX_IMPORTANT << VAROUT(rns->getRobot()->getRootNode());
ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), invJacMethod));
@@ -51,7 +54,8 @@ CartesianVelocityController::CartesianVelocityController(const VirtualRobot::Rob
_jointCosts = Eigen::VectorXf::Constant(rns->getSize(), 1);
}
-void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode)
+void
+CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode)
{
ARMARX_TRACE;
jacobi = ik->getJacobianMatrix(_tcp, mode);
@@ -68,10 +72,13 @@ void CartesianVelocityController::calculateJacobis(VirtualRobot::IKSolver::Carte
double minSigVal = sv(sv.rows() - 1, sv.cols() - 1);
double damping = minSigVal < 1e-2 ? 1e-2 : 1e-8;
ik->setDampedSvdLambda(damping);
- _inv = ik->computePseudoInverseJacobianMatrix(_jacobiWithCosts, ik->getJacobiRegularization(mode));
+ _inv =
+ ik->computePseudoInverseJacobianMatrix(_jacobiWithCosts, ik->getJacobiRegularization(mode));
}
-Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, VirtualRobot::IKSolver::CartesianSelection mode)
+Eigen::VectorXf
+CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
return calculate(cartesianVel, Eigen::VectorXf::Zero(0), mode);
/*calculateJacobis(mode);
@@ -96,13 +103,20 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
return jointVel;*/
}
-Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode)
+Eigen::VectorXf
+CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel,
+ float KpJointLimitAvoidanceScale,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
- Eigen::VectorXf nullspaceVel = calculateNullspaceVelocity(cartesianVel, KpJointLimitAvoidanceScale, mode);
+ Eigen::VectorXf nullspaceVel =
+ calculateNullspaceVelocity(cartesianVel, KpJointLimitAvoidanceScale, mode);
return calculate(cartesianVel, nullspaceVel, mode);
}
-Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode)
+Eigen::VectorXf
+CartesianVelocityController::calculate(const Eigen::VectorXf& cartesianVel,
+ const Eigen::VectorXf& nullspaceVel,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
ARMARX_TRACE;
calculateJacobis(mode);
@@ -125,7 +139,8 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
// Prevent division by zero
if (squaredNorm > 1.0e-32f)
{
- nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+ nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) /
+ nullspace.col(i).squaredNorm();
}
}
}
@@ -152,7 +167,8 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
return jointVel;
}
-Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidance()
+Eigen::VectorXf
+CartesianVelocityController::calculateJointLimitAvoidance()
{
Eigen::VectorXf r(rns->getSize());
for (size_t i = 0; i < rns->getSize(); i++)
@@ -164,7 +180,8 @@ Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidance()
}
else
{
- float f = math::MathUtils::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
+ float f = math::MathUtils::ILerp(
+ rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
r(i) = cos(f * M_PI);
//r(i) = math::MathUtils::Lerp(1.f, -1.f, f);
}
@@ -177,7 +194,8 @@ Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidance()
* @param margins Vector with same size as rns. Values between 0 (no margin) and 1 (50% low and 50% high margin).
* @return
*/
-Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidanceWithMargins(const Eigen::VectorXf& margins)
+Eigen::VectorXf
+CartesianVelocityController::calculateJointLimitAvoidanceWithMargins(const Eigen::VectorXf& margins)
{
Eigen::VectorXf r(rns->getSize());
for (size_t i = 0; i < rns->getSize(); i++)
@@ -194,14 +212,18 @@ Eigen::VectorXf CartesianVelocityController::calculateJointLimitAvoidanceWithMar
float hi = rn->getJointLimitHi();
float range = hi - lo;
float mrg = math::MathUtils::LimitMinMax(0.f, 1.f, margins(i) * range / 2);
- r(i) = math::MathUtils::ILerpClamp(lo + mrg, lo, rn->getJointValue())
- + math::MathUtils::ILerpClamp(hi, hi - mrg, rn->getJointValue());
+ r(i) = math::MathUtils::ILerpClamp(lo + mrg, lo, rn->getJointValue()) +
+ math::MathUtils::ILerpClamp(hi, hi - mrg, rn->getJointValue());
}
}
return r;
}
-Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel, float KpScale, VirtualRobot::IKSolver::CartesianSelection mode)
+Eigen::VectorXf
+CartesianVelocityController::calculateNullspaceVelocity(
+ const Eigen::VectorXf& cartesianVel,
+ float KpScale,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
Eigen::VectorXf regularization = calculateRegularization(mode);
// Eigen does not allow product between two column vectors (this fails in Debug mode)
@@ -210,16 +232,19 @@ Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Ei
Eigen::VectorXf vel = cartesianVel.cwiseProduct(regularization);
return KpScale * vel.norm() * calculateJointLimitAvoidance();
-
}
-void CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization)
+void
+CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization,
+ float cartesianRadianRegularization)
{
_cartesianMMRegularization = cartesianMMRegularization;
_cartesianRadianRegularization = cartesianRadianRegularization;
}
-Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode)
+Eigen::VectorXf
+CartesianVelocityController::calculateRegularization(
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
Eigen::VectorXf regularization(6);
@@ -249,7 +274,13 @@ Eigen::VectorXf CartesianVelocityController::calculateRegularization(VirtualRobo
return regularization.topRows(i);
}
-bool CartesianVelocityController::clampJacobiAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, Eigen::MatrixXf& jacobi, Eigen::MatrixXf& inv, float jointLimitCheckAccuracy)
+bool
+CartesianVelocityController::clampJacobiAtJointLimits(
+ VirtualRobot::IKSolver::CartesianSelection mode,
+ const Eigen::VectorXf& cartesianVel,
+ Eigen::MatrixXf& jacobi,
+ Eigen::MatrixXf& inv,
+ float jointLimitCheckAccuracy)
{
bool modifiedJacobi = false;
@@ -261,14 +292,17 @@ bool CartesianVelocityController::clampJacobiAtJointLimits(VirtualRobot::IKSolve
auto& node = rns->getNode(static_cast<int>(i));
if (node->isLimitless() || // limitless joint cannot be out of limits
- std::abs(jointVel(i)) < 0.001f // If it the jacobi doesnt want this joint to move anyway, there is no point in recalculating the inverse jacobi
- )
+ std::abs(jointVel(i)) <
+ 0.001f // If it the jacobi doesnt want this joint to move anyway, there is no point in recalculating the inverse jacobi
+ )
{
continue;
}
- if ((node->getJointValue() >= node->getJointLimitHigh() - jointLimitCheckAccuracy && jointVel(i) > 0)
- || (node->getJointValue() <= node->getJointLimitLow() + jointLimitCheckAccuracy && jointVel(i) < 0))
+ if ((node->getJointValue() >= node->getJointLimitHigh() - jointLimitCheckAccuracy &&
+ jointVel(i) > 0) ||
+ (node->getJointValue() <= node->getJointLimitLow() + jointLimitCheckAccuracy &&
+ jointVel(i) < 0))
{
for (int k = 0; k < jacobi.rows(); ++k) // memory allocation free resetting of column
{
@@ -291,17 +325,20 @@ bool CartesianVelocityController::clampJacobiAtJointLimits(VirtualRobot::IKSolve
return modifiedJacobi;
}
-bool CartesianVelocityController::getConsiderJointLimits() const
+bool
+CartesianVelocityController::getConsiderJointLimits() const
{
return _considerJointLimits;
}
-void CartesianVelocityController::setConsiderJointLimits(bool value)
+void
+CartesianVelocityController::setConsiderJointLimits(bool value)
{
_considerJointLimits = value;
}
-void CartesianVelocityController::setJointCosts(const std::vector<float>& jointCosts)
+void
+CartesianVelocityController::setJointCosts(const std::vector<float>& jointCosts)
{
ARMARX_CHECK((int)jointCosts.size() == _jointCosts.rows());
for (size_t i = 0; i < jointCosts.size(); i++)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.h b/source/RobotAPI/libraries/core/CartesianVelocityController.h
index 0454957fb9cbf3c765c9a794e8d401321251730c..bd4d0ea6cda37ba1186cd9c00e7b7fe01c1f567b 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.h
@@ -23,11 +23,11 @@
#pragma once
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/IK/JacobiProvider.h>
-
#include <Eigen/Core>
+#include <VirtualRobot/IK/JacobiProvider.h>
+#include <VirtualRobot/VirtualRobot.h>
+
namespace armarx
{
class CartesianVelocityController;
@@ -36,21 +36,31 @@ namespace armarx
class CartesianVelocityController
{
public:
- CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp = nullptr,
- const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped,
+ CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& tcp = nullptr,
+ const VirtualRobot::JacobiProvider::InverseJacobiMethod
+ invJacMethod = VirtualRobot::JacobiProvider::eSVDDamped,
bool _considerJointLimits = true);
CartesianVelocityController(CartesianVelocityController&&) = default;
CartesianVelocityController& operator=(CartesianVelocityController&&) = default;
- Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, VirtualRobot::IKSolver::CartesianSelection mode);
- Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float KpJointLimitAvoidanceScale, VirtualRobot::IKSolver::CartesianSelection mode);
- Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel,
+ VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel,
+ float KpJointLimitAvoidanceScale,
+ VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel,
+ const Eigen::VectorXf& nullspaceVel,
+ VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf calculateJointLimitAvoidance();
Eigen::VectorXf calculateJointLimitAvoidanceWithMargins(const Eigen::VectorXf& margins);
- Eigen::VectorXf calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel, float KpScale, VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculateNullspaceVelocity(const Eigen::VectorXf& cartesianVel,
+ float KpScale,
+ VirtualRobot::IKSolver::CartesianSelection mode);
- void setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization);
+ void setCartesianRegularization(float cartesianMMRegularization,
+ float cartesianRadianRegularization);
bool getConsiderJointLimits() const;
void setConsiderJointLimits(bool value);
@@ -68,10 +78,14 @@ namespace armarx
void calculateJacobis(VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::MatrixXf _jacobiWithCosts;
Eigen::MatrixXf _inv;
- bool clampJacobiAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode, const Eigen::VectorXf& cartesianVel, Eigen::MatrixXf& jacobi, Eigen::MatrixXf& _inv, float jointLimitCheckAccuracy = 0.001f);
+ bool clampJacobiAtJointLimits(VirtualRobot::IKSolver::CartesianSelection mode,
+ const Eigen::VectorXf& cartesianVel,
+ Eigen::MatrixXf& jacobi,
+ Eigen::MatrixXf& _inv,
+ float jointLimitCheckAccuracy = 0.001f);
bool _considerJointLimits = true;
float _cartesianMMRegularization;
float _cartesianRadianRegularization;
Eigen::VectorXf _jointCosts;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index 6c9d30d58dc25d3a6a58021ad7f763776041cd04..1551014feab583d889e5c209f5528023ee1a1466 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -24,25 +24,34 @@
#include "CartesianVelocityControllerWithRamp.h"
#include <VirtualRobot/IK/DifferentialIK.h>
-#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
#include <ArmarXCore/core/logging/Logging.h>
namespace armarx
{
- CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
- float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration, const VirtualRobot::RobotNodePtr& tcp)
- : controller(rns, tcp), mode(mode)
+ CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::VectorXf& currentJointVelocity,
+ VirtualRobot::IKSolver::CartesianSelection mode,
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const VirtualRobot::RobotNodePtr& tcp) :
+ controller(rns, tcp), mode(mode)
{
- setMaxAccelerations(maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
+ setMaxAccelerations(
+ maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
setCurrentJointVelocity(currentJointVelocity);
#pragma GCC diagnostic pop
}
- void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
+ void
+ CartesianVelocityControllerWithRamp::setCurrentJointVelocity(
+ const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
{
Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller._tcp, mode);
//Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
@@ -54,56 +63,74 @@ namespace armarx
for (int i = 0; i < nullspace.cols(); i++)
{
- nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
+ nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) /
+ nullspace.col(i).squaredNorm();
}
cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
nullSpaceVelocityRamp.setState(nsv);
-
}
- void CartesianVelocityControllerWithRamp::switchMode(const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode)
+ void
+ CartesianVelocityControllerWithRamp::switchMode(const Eigen::VectorXf& currentJointVelocity,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller._tcp, mode);
cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
this->mode = mode;
}
- VirtualRobot::IKSolver::CartesianSelection CartesianVelocityControllerWithRamp::getMode()
+ VirtualRobot::IKSolver::CartesianSelection
+ CartesianVelocityControllerWithRamp::getMode()
{
return mode;
}
- Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)
+ Eigen::VectorXf
+ CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel,
+ float jointLimitAvoidanceScale,
+ float dt)
{
- Eigen::VectorXf nullspaceVel = controller.calculateNullspaceVelocity(cartesianVel, jointLimitAvoidanceScale, mode);
+ Eigen::VectorXf nullspaceVel =
+ controller.calculateNullspaceVelocity(cartesianVel, jointLimitAvoidanceScale, mode);
return calculate(cartesianVel, nullspaceVel, dt);
}
- Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt)
+ Eigen::VectorXf
+ CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel,
+ const Eigen::VectorXf& nullspaceVel,
+ float dt)
{
ARMARX_TRACE;
- return controller.calculate(cartesianVelocityRamp.update(cartesianVel, dt), nullSpaceVelocityRamp.update(nullspaceVel, dt), mode);
+ return controller.calculate(cartesianVelocityRamp.update(cartesianVel, dt),
+ nullSpaceVelocityRamp.update(nullspaceVel, dt),
+ mode);
}
- void CartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration)
+ void
+ CartesianVelocityControllerWithRamp::setMaxAccelerations(float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration)
{
cartesianVelocityRamp.setMaxOrientationAcceleration(maxOrientationAcceleration);
cartesianVelocityRamp.setMaxPositionAcceleration(maxPositionAcceleration);
nullSpaceVelocityRamp.setMaxAccelaration(maxNullspaceAcceleration);
}
- float CartesianVelocityControllerWithRamp::getMaxOrientationAcceleration()
+ float
+ CartesianVelocityControllerWithRamp::getMaxOrientationAcceleration()
{
return cartesianVelocityRamp.getMaxOrientationAcceleration();
}
- float CartesianVelocityControllerWithRamp::getMaxPositionAcceleration()
+ float
+ CartesianVelocityControllerWithRamp::getMaxPositionAcceleration()
{
return cartesianVelocityRamp.getMaxPositionAcceleration();
}
- float CartesianVelocityControllerWithRamp::getMaxNullspaceAcceleration()
+ float
+ CartesianVelocityControllerWithRamp::getMaxNullspaceAcceleration()
{
return nullSpaceVelocityRamp.getMaxAccelaration();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
index 3b6b93dc9ba6336acb6e1df39d9c14c6abf59f35..d8b33b0fabab916bc9d9c087413308265a63cd28 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
@@ -23,41 +23,53 @@
#pragma once
-#include "CartesianVelocityController.h"
-#include "CartesianVelocityRamp.h"
-#include "JointVelocityRamp.h"
+#include <Eigen/Dense>
-#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/IK/IKSolver.h>
+#include <VirtualRobot/RobotNodeSet.h>
-#include <Eigen/Dense>
-
+#include "CartesianVelocityController.h"
+#include "CartesianVelocityRamp.h"
+#include "JointVelocityRamp.h"
namespace armarx
{
class CartesianVelocityControllerWithRamp;
- using CartesianVelocityControllerWithRampPtr = std::shared_ptr<CartesianVelocityControllerWithRamp>;
+ using CartesianVelocityControllerWithRampPtr =
+ std::shared_ptr<CartesianVelocityControllerWithRamp>;
class CartesianVelocityControllerWithRamp
{
public:
- CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
- float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration,
+ CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::VectorXf& currentJointVelocity,
+ VirtualRobot::IKSolver::CartesianSelection mode,
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
const VirtualRobot::RobotNodePtr& tcp = nullptr);
CartesianVelocityControllerWithRamp(CartesianVelocityControllerWithRamp&&) = default;
- CartesianVelocityControllerWithRamp& operator=(CartesianVelocityControllerWithRamp&&) = default;
+ CartesianVelocityControllerWithRamp&
+ operator=(CartesianVelocityControllerWithRamp&&) = default;
- [[deprecated("computed null space velocity does not match pseudo inverse svd method in simox. never use this function.")]]
- void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
+ [[deprecated("computed null space velocity does not match pseudo inverse svd method in "
+ "simox. never use this function.")]] void
+ setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
- void switchMode(const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode);
+ void switchMode(const Eigen::VectorXf& currentJointVelocity,
+ VirtualRobot::IKSolver::CartesianSelection mode);
VirtualRobot::IKSolver::CartesianSelection getMode();
- Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt);
- Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt);
+ Eigen::VectorXf
+ calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt);
+ Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel,
+ const Eigen::VectorXf& nullspaceVel,
+ float dt);
- void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration, float maxNullspaceAcceleration);
+ void setMaxAccelerations(float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration);
float getMaxOrientationAcceleration();
float getMaxPositionAcceleration();
@@ -65,9 +77,10 @@ namespace armarx
CartesianVelocityController controller;
+
private:
VirtualRobot::IKSolver::CartesianSelection mode;
CartesianVelocityRamp cartesianVelocityRamp;
JointVelocityRamp nullSpaceVelocityRamp;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
index eb5646dac877f1f3632c94b9f4bf64c1d441a765..012108505e67de23ee7af2d9e8a0cc488133718f 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
@@ -23,23 +23,27 @@
#include "CartesianVelocityRamp.h"
-#include <ArmarXCore/core/logging/Logging.h>
-
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Robot.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx
{
CartesianVelocityRamp::CartesianVelocityRamp()
- { }
+ {
+ }
- void CartesianVelocityRamp::setState(const Eigen::VectorXf& state, VirtualRobot::IKSolver::CartesianSelection mode)
+ void
+ CartesianVelocityRamp::setState(const Eigen::VectorXf& state,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
this->state = state;
this->mode = mode;
}
- Eigen::VectorXf CartesianVelocityRamp::update(const Eigen::VectorXf& target, float dt)
+ Eigen::VectorXf
+ CartesianVelocityRamp::update(const Eigen::VectorXf& target, float dt)
{
if (dt <= 0)
{
@@ -47,7 +51,7 @@ namespace armarx
}
Eigen::VectorXf delta = target - state;
int i = 0;
- float factor = 1 ;
+ float factor = 1;
if (mode & VirtualRobot::IKSolver::Position)
{
@@ -69,25 +73,27 @@ namespace armarx
return state;
}
- void CartesianVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
+ void
+ CartesianVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
{
this->maxPositionAcceleration = maxPositionAcceleration;
-
}
- void CartesianVelocityRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
+ void
+ CartesianVelocityRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
{
this->maxOrientationAcceleration = maxOrientationAcceleration;
-
}
- float CartesianVelocityRamp::getMaxPositionAcceleration()
+ float
+ CartesianVelocityRamp::getMaxPositionAcceleration()
{
return maxPositionAcceleration;
}
- float CartesianVelocityRamp::getMaxOrientationAcceleration()
+ float
+ CartesianVelocityRamp::getMaxOrientationAcceleration()
{
return maxOrientationAcceleration;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
index 573dfe2b293450fb5dd98be649a982aef81d3842..c52647cc620ac393079080bd93231abff1cfd895 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.h
@@ -23,11 +23,11 @@
#pragma once
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/IK/IKSolver.h>
-
#include <Eigen/Dense>
+#include <VirtualRobot/IK/IKSolver.h>
+#include <VirtualRobot/VirtualRobot.h>
+
namespace armarx
{
class CartesianVelocityRamp;
@@ -37,7 +37,8 @@ namespace armarx
{
public:
CartesianVelocityRamp();
- void setState(const Eigen::VectorXf& state, VirtualRobot::IKSolver::CartesianSelection mode);
+ void setState(const Eigen::VectorXf& state,
+ VirtualRobot::IKSolver::CartesianSelection mode);
Eigen::VectorXf update(const Eigen::VectorXf& target, float dt);
@@ -52,8 +53,7 @@ namespace armarx
float maxOrientationAcceleration = 0;
Eigen::VectorXf state = Eigen::VectorXf::Zero(6);
- VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::CartesianSelection::All;
-
-
+ VirtualRobot::IKSolver::CartesianSelection mode =
+ VirtualRobot::IKSolver::CartesianSelection::All;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
index 34c1902c05f8cbc63435dcb1cd35a1735b4e2c39..a2cf6e82dc5520b53a4aa4e8f061c5c08b0033bc 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -22,19 +22,20 @@
*/
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-
#include "CartesianWaypointController.h"
-#include <VirtualRobot/math/Helpers.h>
+#include <cfloat>
+
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
-#include <cfloat>
+#include <VirtualRobot/math/Helpers.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
namespace armarx
{
CartesianWaypointController::CartesianWaypointController(
@@ -44,27 +45,24 @@ namespace armarx
float maxOrientationAcceleration,
float maxNullspaceAcceleration,
const VirtualRobot::RobotNodePtr& tcp,
- const VirtualRobot::RobotNodePtr& referenceFrame
- ) :
- ctrlCartesianVelWithRamps
- {
- rns,
- currentJointVelocity,
- VirtualRobot::IKSolver::CartesianSelection::All,
- maxPositionAcceleration,
- maxOrientationAcceleration,
- maxNullspaceAcceleration,
- tcp ? tcp : rns->getTCP()
- },
- ctrlCartesianPos2Vel(tcp ? tcp : rns->getTCP(), referenceFrame),
- currentWaypointIndex(0)
+ const VirtualRobot::RobotNodePtr& referenceFrame) :
+ ctrlCartesianVelWithRamps{rns,
+ currentJointVelocity,
+ VirtualRobot::IKSolver::CartesianSelection::All,
+ maxPositionAcceleration,
+ maxOrientationAcceleration,
+ maxNullspaceAcceleration,
+ tcp ? tcp : rns->getTCP()},
+ ctrlCartesianPos2Vel(tcp ? tcp : rns->getTCP(), referenceFrame),
+ currentWaypointIndex(0)
{
ARMARX_CHECK_NOT_NULL(rns);
_out = Eigen::VectorXf::Zero(rns->getSize());
_jnv = Eigen::VectorXf::Zero(rns->getSize());
}
- const Eigen::VectorXf& CartesianWaypointController::calculate(float dt)
+ const Eigen::VectorXf&
+ CartesianWaypointController::calculate(float dt)
{
ARMARX_TRACE;
//calculate cartesian velocity + some management stuff
@@ -73,7 +71,9 @@ namespace armarx
ARMARX_TRACE;
currentWaypointIndex++;
}
- cartesianVelocity = ctrlCartesianPos2Vel.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All) + feedForwardVelocity;
+ cartesianVelocity =
+ ctrlCartesianPos2Vel.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All) +
+ feedForwardVelocity;
if (autoClearFeedForward)
{
@@ -86,12 +86,10 @@ namespace armarx
{
ARMARX_TRACE;
//avoid joint limits
- _jnv = KpJointLimitAvoidance * ctrlCartesianVelWithRamps.controller.calculateJointLimitAvoidance() +
+ _jnv = KpJointLimitAvoidance *
+ ctrlCartesianVelWithRamps.controller.calculateJointLimitAvoidance() +
ctrlCartesianVelWithRamps.controller.calculateNullspaceVelocity(
- cartesianVelocity,
- jointLimitAvoidanceScale,
- VirtualRobot::IKSolver::All
- );
+ cartesianVelocity, jointLimitAvoidanceScale, VirtualRobot::IKSolver::All);
}
else
{
@@ -104,93 +102,114 @@ namespace armarx
return _out;
}
- void CartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
+ void
+ CartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
{
this->waypoints = waypoints;
currentWaypointIndex = 0;
}
- void CartesianWaypointController::swapWaypoints(std::vector<Eigen::Matrix4f>& waypoints)
+ void
+ CartesianWaypointController::swapWaypoints(std::vector<Eigen::Matrix4f>& waypoints)
{
std::swap(this->waypoints, waypoints);
currentWaypointIndex = 0;
}
- void CartesianWaypointController::addWaypoint(const Eigen::Matrix4f& waypoint)
+ void
+ CartesianWaypointController::addWaypoint(const Eigen::Matrix4f& waypoint)
{
this->waypoints.push_back(waypoint);
}
- void CartesianWaypointController::setTarget(const Eigen::Matrix4f& target)
+ void
+ CartesianWaypointController::setTarget(const Eigen::Matrix4f& target)
{
waypoints.clear();
waypoints.push_back(target);
currentWaypointIndex = 0;
}
- void CartesianWaypointController::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
+ void
+ CartesianWaypointController::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
{
this->feedForwardVelocity = feedForwardVelocity;
}
- void CartesianWaypointController::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
+ void
+ CartesianWaypointController::setFeedForwardVelocity(
+ const Eigen::Vector3f& feedForwardVelocityPos,
+ const Eigen::Vector3f& feedForwardVelocityOri)
{
feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
}
- void CartesianWaypointController::clearFeedForwardVelocity()
+ void
+ CartesianWaypointController::clearFeedForwardVelocity()
{
feedForwardVelocity = Eigen::Vector6f::Zero();
}
- float CartesianWaypointController::getPositionError() const
+ float
+ CartesianWaypointController::getPositionError() const
{
return ctrlCartesianPos2Vel.getPositionError(getCurrentTarget());
}
- float CartesianWaypointController::getOrientationError() const
+ float
+ CartesianWaypointController::getOrientationError() const
{
return ctrlCartesianPos2Vel.getOrientationError(getCurrentTarget());
}
- bool CartesianWaypointController::isCurrentTargetReached() const
+ bool
+ CartesianWaypointController::isCurrentTargetReached() const
{
- return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+ return getPositionError() < thresholdPositionReached &&
+ getOrientationError() < thresholdOrientationReached;
}
- bool CartesianWaypointController::isCurrentTargetNear() const
+ bool
+ CartesianWaypointController::isCurrentTargetNear() const
{
- return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+ return getPositionError() < thresholdPositionNear &&
+ getOrientationError() < thresholdOrientationNear;
}
- bool CartesianWaypointController::isFinalTargetReached() const
+ bool
+ CartesianWaypointController::isFinalTargetReached() const
{
return isLastWaypoint() && isCurrentTargetReached();
}
- bool CartesianWaypointController::isFinalTargetNear() const
+ bool
+ CartesianWaypointController::isFinalTargetNear() const
{
return isLastWaypoint() && isCurrentTargetNear();
}
- bool CartesianWaypointController::isLastWaypoint() const
+ bool
+ CartesianWaypointController::isLastWaypoint() const
{
return currentWaypointIndex + 1 >= waypoints.size();
}
- const Eigen::Matrix4f& CartesianWaypointController::getCurrentTarget() const
+ const Eigen::Matrix4f&
+ CartesianWaypointController::getCurrentTarget() const
{
return waypoints.at(currentWaypointIndex);
}
- const Eigen::Vector3f CartesianWaypointController::getCurrentTargetPosition() const
+ const Eigen::Vector3f
+ CartesianWaypointController::getCurrentTargetPosition() const
{
ARMARX_TRACE;
return ::math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
}
- size_t CartesianWaypointController::skipToClosestWaypoint(float rad2mmFactor)
+ size_t
+ CartesianWaypointController::skipToClosestWaypoint(float rad2mmFactor)
{
ARMARX_TRACE;
float dist = FLT_MAX;
@@ -210,45 +229,51 @@ namespace armarx
return currentWaypointIndex;
}
- void CartesianWaypointController::setNullSpaceControl(bool enabled)
+ void
+ CartesianWaypointController::setNullSpaceControl(bool enabled)
{
nullSpaceControlEnabled = enabled;
}
- std::string CartesianWaypointController::getStatusText()
+ std::string
+ CartesianWaypointController::getStatusText()
{
std::stringstream ss;
ss.precision(2);
- ss << std::fixed << "Waypoint: " << (currentWaypointIndex + 1) << "/" << waypoints.size() << " distance: " << getPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
+ ss << std::fixed << "Waypoint: " << (currentWaypointIndex + 1) << "/" << waypoints.size()
+ << " distance: " << getPositionError() << " mm "
+ << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
return ss.str();
}
- void CartesianWaypointController::setConfig(const CartesianWaypointControllerConfig& cfg)
+ void
+ CartesianWaypointController::setConfig(const CartesianWaypointControllerConfig& cfg)
{
- KpJointLimitAvoidance = cfg.kpJointLimitAvoidance;
- jointLimitAvoidanceScale = cfg.jointLimitAvoidanceScale;
-
- thresholdOrientationNear = cfg.thresholdOrientationNear;
- thresholdOrientationReached = cfg.thresholdOrientationReached;
- thresholdPositionNear = cfg.thresholdPositionNear;
- thresholdPositionReached = cfg.thresholdPositionReached;
-
- ctrlCartesianPos2Vel.maxOriVel = cfg.maxOriVel;
- ctrlCartesianPos2Vel.maxPosVel = cfg.maxPosVel;
- ctrlCartesianPos2Vel.KpOri = cfg.kpOri;
- ctrlCartesianPos2Vel.KpPos = cfg.kpPos;
-
- ctrlCartesianVelWithRamps.setMaxAccelerations(
- cfg.maxPositionAcceleration,
- cfg.maxOrientationAcceleration,
- cfg.maxNullspaceAcceleration
- );
+ KpJointLimitAvoidance = cfg.kpJointLimitAvoidance;
+ jointLimitAvoidanceScale = cfg.jointLimitAvoidanceScale;
+
+ thresholdOrientationNear = cfg.thresholdOrientationNear;
+ thresholdOrientationReached = cfg.thresholdOrientationReached;
+ thresholdPositionNear = cfg.thresholdPositionNear;
+ thresholdPositionReached = cfg.thresholdPositionReached;
+
+ ctrlCartesianPos2Vel.maxOriVel = cfg.maxOriVel;
+ ctrlCartesianPos2Vel.maxPosVel = cfg.maxPosVel;
+ ctrlCartesianPos2Vel.KpOri = cfg.kpOri;
+ ctrlCartesianPos2Vel.KpPos = cfg.kpPos;
+
+ ctrlCartesianVelWithRamps.setMaxAccelerations(cfg.maxPositionAcceleration,
+ cfg.maxOrientationAcceleration,
+ cfg.maxNullspaceAcceleration);
}
- void CartesianWaypointController::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
+
+ void
+ CartesianWaypointController::setCurrentJointVelocity(
+ const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
ctrlCartesianVelWithRamps.setCurrentJointVelocity(currentJointVelocity);
#pragma GCC diagnostic pop
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.h b/source/RobotAPI/libraries/core/CartesianWaypointController.h
index d6fb2f7bc2038ac64dfc260346662b1441baaddc..e9cf96d53bb7d3f4b9fa1b10c42b6c931a06f563 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.h
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.h
@@ -29,9 +29,9 @@
#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/interface/core/CartesianWaypointControllerConfig.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/interface/core/CartesianWaypointControllerConfig.h>
#include "CartesianVelocityControllerWithRamp.h"
@@ -40,7 +40,6 @@ namespace Eigen
using Vector6f = Matrix<float, 6, 1>;
}
-
namespace armarx
{
class CartesianWaypointController;
@@ -55,12 +54,12 @@ namespace armarx
float maxOrientationAcceleration,
float maxNullspaceAcceleration,
const VirtualRobot::RobotNodePtr& tcp = nullptr,
- const VirtualRobot::RobotNodePtr& referenceFrame = nullptr
- );
+ const VirtualRobot::RobotNodePtr& referenceFrame = nullptr);
- [[deprecated("compued null space velocity does not match pseudo inverse svd method in simox. never use this function.")]]
- void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
+ [[deprecated("compued null space velocity does not match pseudo inverse svd method in "
+ "simox. never use this function.")]] void
+ setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
const Eigen::VectorXf& calculate(float dt);
@@ -69,7 +68,8 @@ namespace armarx
void addWaypoint(const Eigen::Matrix4f& waypoint);
void setTarget(const Eigen::Matrix4f& target);
void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
- void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
+ void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos,
+ const Eigen::Vector3f& feedForwardVelocityOri);
void clearFeedForwardVelocity();
float getPositionError() const;
@@ -94,28 +94,28 @@ namespace armarx
void setConfig(const CartesianWaypointControllerConfig& cfg);
- CartesianVelocityControllerWithRamp ctrlCartesianVelWithRamps;
+ CartesianVelocityControllerWithRamp ctrlCartesianVelWithRamps;
CartesianPositionController ctrlCartesianPos2Vel;
std::vector<Eigen::Matrix4f> waypoints;
- size_t currentWaypointIndex = 0.f;
+ size_t currentWaypointIndex = 0.f;
- float thresholdPositionReached = 0.f;
- float thresholdOrientationReached = 0.f;
- float thresholdPositionNear = 0.f;
- float thresholdOrientationNear = 0.f;
+ float thresholdPositionReached = 0.f;
+ float thresholdOrientationReached = 0.f;
+ float thresholdPositionNear = 0.f;
+ float thresholdOrientationNear = 0.f;
Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
Eigen::Vector6f cartesianVelocity = Eigen::Vector6f::Zero();
- bool autoClearFeedForward = true;
+ bool autoClearFeedForward = true;
- bool nullSpaceControlEnabled = true;
- float jointLimitAvoidanceScale = 0.f;
- float KpJointLimitAvoidance = 0.f;
+ bool nullSpaceControlEnabled = true;
+ float jointLimitAvoidanceScale = 0.f;
+ float KpJointLimitAvoidance = 0.f;
private:
Eigen::VectorXf _out;
Eigen::VectorXf _jnv;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/EigenHelpers.h b/source/RobotAPI/libraries/core/EigenHelpers.h
index ce933cc8e0f6ae706efbd821ff2d6a6f1ae2a196..c065da7ee464df89f91cb0e348ecaa88e48782f8 100644
--- a/source/RobotAPI/libraries/core/EigenHelpers.h
+++ b/source/RobotAPI/libraries/core/EigenHelpers.h
@@ -28,8 +28,9 @@
namespace armarx
{
- template<class ScalarType>
- inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> MakeVectorX(std::initializer_list<ScalarType> ilist)
+ template <class ScalarType>
+ inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>
+ MakeVectorX(std::initializer_list<ScalarType> ilist)
{
Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> r(ilist.size());
std::size_t i = 0;
@@ -39,28 +40,38 @@ namespace armarx
}
return r;
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(std::initializer_list<float> ilist)
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(std::initializer_list<float> ilist)
{
return MakeVectorX<float>(ilist);
}
- template<class ScalarType, class...Ts>
- inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> MakeVectorXWarnNarrowing(Ts&& ...ts)
+ template <class ScalarType, class... Ts>
+ inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>
+ MakeVectorXWarnNarrowing(Ts&&... ts)
{
- return MakeVectorX<ScalarType>(std::initializer_list<ScalarType> {std::forward<Ts>(ts)...});
+ return MakeVectorX<ScalarType>(std::initializer_list<ScalarType>{std::forward<Ts>(ts)...});
}
- template<class ScalarType, class...Ts>
- inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> MakeVectorXIgnoreNarrowing(Ts&& ...ts)
+
+ template <class ScalarType, class... Ts>
+ inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>
+ MakeVectorXIgnoreNarrowing(Ts&&... ts)
{
- return MakeVectorX<ScalarType>(std::initializer_list<ScalarType> {static_cast<ScalarType>(std::forward<Ts>(ts))...});
+ return MakeVectorX<ScalarType>(
+ std::initializer_list<ScalarType>{static_cast<ScalarType>(std::forward<Ts>(ts))...});
}
- template<class ScalarType, class...Ts>
- inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> MakeVectorX(Ts&& ...ts)
+
+ template <class ScalarType, class... Ts>
+ inline Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>
+ MakeVectorX(Ts&&... ts)
{
- return MakeVectorX<ScalarType>(std::initializer_list<ScalarType> {static_cast<ScalarType>(std::forward<Ts>(ts))...});
+ return MakeVectorX<ScalarType>(
+ std::initializer_list<ScalarType>{static_cast<ScalarType>(std::forward<Ts>(ts))...});
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(std::vector<float> vec)
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(std::vector<float> vec)
{
Eigen::Matrix<float, Eigen::Dynamic, 1> r(vec.size());
std::size_t i = 0;
@@ -72,46 +83,82 @@ namespace armarx
}
/* Qt-Creator does not support variable amount of parameters. The following methods are only for Qt-Creator. */
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1)
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(float f1)
{
return MakeVectorX<float>(f1);
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2)
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(float f1, float f2)
{
return MakeVectorX<float>(f1, f2);
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3)
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(float f1, float f2, float f3)
{
return MakeVectorX<float>(f1, f2, f3);
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4)
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(float f1, float f2, float f3, float f4)
{
return MakeVectorX<float>(f1, f2, f3, f4);
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5)
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(float f1, float f2, float f3, float f4, float f5)
{
return MakeVectorX<float>(f1, f2, f3, f4, f5);
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6)
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6)
{
return MakeVectorX<float>(f1, f2, f3, f4, f5, f6);
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6, float f7)
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6, float f7)
{
return MakeVectorX<float>(f1, f2, f3, f4, f5, f6, f7);
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6, float f7, float f8)
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6, float f7, float f8)
{
return MakeVectorX<float>(f1, f2, f3, f4, f5, f6, f7, f8);
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6, float f7, float f8, float f9)
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(float f1,
+ float f2,
+ float f3,
+ float f4,
+ float f5,
+ float f6,
+ float f7,
+ float f8,
+ float f9)
{
return MakeVectorX<float>(f1, f2, f3, f4, f5, f6, f7, f8, f9);
}
- inline Eigen::Matrix<float, Eigen::Dynamic, 1> MakeVectorXf(float f1, float f2, float f3, float f4, float f5, float f6, float f7, float f8, float f9, float f10)
+
+ inline Eigen::Matrix<float, Eigen::Dynamic, 1>
+ MakeVectorXf(float f1,
+ float f2,
+ float f3,
+ float f4,
+ float f5,
+ float f6,
+ float f7,
+ float f8,
+ float f9,
+ float f10)
{
return MakeVectorX<float>(f1, f2, f3, f4, f5, f6, f7, f8, f9, f10);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp b/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
index 9c42823f12c1bb88cc4581d67a5f8d88d50a385c..249c6d316cdcca9d94643b80702dc803f1cead36 100644
--- a/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
+++ b/source/RobotAPI/libraries/core/FramedOrientedPoint.cpp
@@ -20,16 +20,15 @@
#include "FramedOrientedPoint.h"
-#include <VirtualRobot/Robot.h>
#include <VirtualRobot/LinkedCoordinate.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
namespace armarx
{
- FramedOrientedPoint::FramedOrientedPoint()
- = default;
+ FramedOrientedPoint::FramedOrientedPoint() = default;
FramedOrientedPoint::FramedOrientedPoint(const FramedOrientedPoint& source) :
IceUtil::Shared(source),
@@ -40,35 +39,51 @@ namespace armarx
{
}
- FramedOrientedPoint::FramedOrientedPoint(const Eigen::Vector3f& position, const Eigen::Vector3f& normal, const std::string& frame, const std::string& agent) :
+ FramedOrientedPoint::FramedOrientedPoint(const Eigen::Vector3f& position,
+ const Eigen::Vector3f& normal,
+ const std::string& frame,
+ const std::string& agent) :
OrientedPoint(position, normal)
{
this->frame = frame;
this->agent = agent;
-
}
- FramedOrientedPoint::FramedOrientedPoint(const OrientedPoint& pointWithNormal, const std::string& frame, const std::string& agent) :
- FramedOrientedPoint(pointWithNormal.positionToEigen(), pointWithNormal.normalToEigen(), frame, agent)
+ FramedOrientedPoint::FramedOrientedPoint(const OrientedPoint& pointWithNormal,
+ const std::string& frame,
+ const std::string& agent) :
+ FramedOrientedPoint(pointWithNormal.positionToEigen(),
+ pointWithNormal.normalToEigen(),
+ frame,
+ agent)
{
}
- FramedOrientedPoint::FramedOrientedPoint(Ice::Float px, ::Ice::Float py, ::Ice::Float pz, Ice::Float nx, ::Ice::Float ny, ::Ice::Float nz, const std::string& frame, const std::string& agent) :
+ FramedOrientedPoint::FramedOrientedPoint(Ice::Float px,
+ ::Ice::Float py,
+ ::Ice::Float pz,
+ Ice::Float nx,
+ ::Ice::Float ny,
+ ::Ice::Float nz,
+ const std::string& frame,
+ const std::string& agent) :
OrientedPoint(px, py, pz, nx, ny, nz)
{
this->frame = frame;
this->agent = agent;
}
- std::string FramedOrientedPoint::getFrame() const
+ std::string
+ FramedOrientedPoint::getFrame() const
{
return frame;
}
-
- void FramedOrientedPoint::changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame)
+ void
+ FramedOrientedPoint::changeFrame(const VirtualRobot::RobotPtr& robot,
+ const std::string& newFrame)
{
- FramedPosition framedPos(positionToEigen(), frame, agent);
+ FramedPosition framedPos(positionToEigen(), frame, agent);
FramedDirection framedDir(normalToEigen(), frame, agent);
framedPos.changeFrame(robot, newFrame);
@@ -82,97 +97,114 @@ namespace armarx
this->nz = framedDir.z;
}
- void FramedOrientedPoint::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
+ void
+ FramedOrientedPoint::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeFrame(sharedRobot, GlobalFrame);
}
- void FramedOrientedPoint::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
+ void
+ FramedOrientedPoint::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
{
changeFrame(referenceRobot, GlobalFrame);
}
- Eigen::Vector3f FramedOrientedPoint::positionToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Vector3f
+ FramedOrientedPoint::positionToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
return getFramedPosition().toGlobalEigen(referenceRobot);
}
- Eigen::Vector3f FramedOrientedPoint::normalToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Vector3f
+ FramedOrientedPoint::normalToGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
return getFramedNormal().toGlobalEigen(referenceRobot);
}
- Eigen::Vector3f FramedOrientedPoint::positionToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ Eigen::Vector3f
+ FramedOrientedPoint::positionToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
return getFramedPosition().toGlobalEigen(referenceRobot);
}
- Eigen::Vector3f FramedOrientedPoint::normalToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ Eigen::Vector3f
+ FramedOrientedPoint::normalToGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
return getFramedNormal().toGlobalEigen(referenceRobot);
}
-
- FramedOrientedPointPtr FramedOrientedPoint::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+ FramedOrientedPointPtr
+ FramedOrientedPoint::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
{
- return new FramedOrientedPoint(positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
+ return new FramedOrientedPoint(
+ positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
}
- FramedOrientedPointPtr FramedOrientedPoint::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+ FramedOrientedPointPtr
+ FramedOrientedPoint::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
{
- return new FramedOrientedPoint(positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
+ return new FramedOrientedPoint(
+ positionToRootEigen(referenceRobot), normalToRootEigen(referenceRobot), frame, agent);
}
- Eigen::Vector3f FramedOrientedPoint::positionToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Vector3f
+ FramedOrientedPoint::positionToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
return getFramedPosition().toRootEigen(referenceRobot);
}
- Eigen::Vector3f FramedOrientedPoint::normalToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Vector3f
+ FramedOrientedPoint::normalToRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
return getFramedNormal().toRootEigen(referenceRobot);
}
- Eigen::Vector3f FramedOrientedPoint::positionToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ Eigen::Vector3f
+ FramedOrientedPoint::positionToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
return getFramedPosition().toRootEigen(referenceRobot);
}
- Eigen::Vector3f FramedOrientedPoint::normalToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+ Eigen::Vector3f
+ FramedOrientedPoint::normalToRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
return getFramedNormal().toRootEigen(referenceRobot);
}
- FramedPosition FramedOrientedPoint::getFramedPosition() const
+ FramedPosition
+ FramedOrientedPoint::getFramedPosition() const
{
return FramedPosition(positionToEigen(), frame, agent);
}
- FramedDirection FramedOrientedPoint::getFramedNormal() const
+ FramedDirection
+ FramedOrientedPoint::getFramedNormal() const
{
return FramedDirection(normalToEigen(), frame, agent);
}
-
- std::string FramedOrientedPoint::output(const Ice::Current& c) const
+ std::string
+ FramedOrientedPoint::output(const Ice::Current& c) const
{
std::stringstream s;
- s << OrientedPoint::output(c) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << OrientedPoint::output(c) << std::endl
+ << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
-
- void FramedOrientedPoint::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+ void
+ FramedOrientedPoint::serialize(const ObjectSerializerBasePtr& serializer,
+ const Ice::Current&) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
OrientedPoint::serialize(serializer);
obj->setString("frame", frame);
obj->setString("agent", agent);
-
}
- void FramedOrientedPoint::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
+ void
+ FramedOrientedPoint::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
OrientedPoint::deserialize(serializer);
@@ -183,4 +215,4 @@ namespace armarx
agent = obj->getString("agent");
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/FramedOrientedPoint.h b/source/RobotAPI/libraries/core/FramedOrientedPoint.h
index 22aff2d36db3a88dc92b0237534eb1a867439ebf..cf94c318379c418ef333d017f8c3ce944292023b 100644
--- a/source/RobotAPI/libraries/core/FramedOrientedPoint.h
+++ b/source/RobotAPI/libraries/core/FramedOrientedPoint.h
@@ -21,34 +21,45 @@
#pragma once
-#include "OrientedPoint.h"
-
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/interface/core/RobotState.h>
+
#include "FramedPose.h"
+#include "OrientedPoint.h"
namespace armarx::VariantType
{
// variant types
- const VariantTypeId FramedOrientedPoint = Variant::addTypeName("::armarx::FramedOrientedPointBase");
-}
+ const VariantTypeId FramedOrientedPoint =
+ Variant::addTypeName("::armarx::FramedOrientedPointBase");
+} // namespace armarx::VariantType
namespace armarx
{
class FramedOrientedPoint;
using FramedOrientedPointPtr = IceInternal::Handle<FramedOrientedPoint>;
- class FramedOrientedPoint :
- virtual public FramedOrientedPointBase,
- virtual public OrientedPoint
+ class FramedOrientedPoint : virtual public FramedOrientedPointBase, virtual public OrientedPoint
{
public:
FramedOrientedPoint();
FramedOrientedPoint(const FramedOrientedPoint& source);
- FramedOrientedPoint(const Eigen::Vector3f& position, const Eigen::Vector3f& normal, const std::string& frame, const std::string& agent);
- FramedOrientedPoint(const OrientedPoint& pointWithNormal, const std::string& frame, const std::string& agent);
- FramedOrientedPoint(Ice::Float px, ::Ice::Float py, ::Ice::Float pz, Ice::Float nx, ::Ice::Float ny, ::Ice::Float nz, const std::string& frame, const std::string& agent);
+ FramedOrientedPoint(const Eigen::Vector3f& position,
+ const Eigen::Vector3f& normal,
+ const std::string& frame,
+ const std::string& agent);
+ FramedOrientedPoint(const OrientedPoint& pointWithNormal,
+ const std::string& frame,
+ const std::string& agent);
+ FramedOrientedPoint(Ice::Float px,
+ ::Ice::Float py,
+ ::Ice::Float pz,
+ Ice::Float nx,
+ ::Ice::Float ny,
+ ::Ice::Float nz,
+ const std::string& frame,
+ const std::string& agent);
std::string getFrame() const;
void changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame);
@@ -72,33 +83,43 @@ namespace armarx
FramedDirection getFramedNormal() const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
+ Ice::ObjectPtr
+ ice_clone() const override
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
+
+ VariantDataClassPtr
+ clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new FramedOrientedPoint(*this);
}
+
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
+
+ VariantTypeId
+ getType(const Ice::Current& c = Ice::emptyCurrent) const override
{
return VariantType::FramedOrientedPoint;
}
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
+
+ bool
+ validate(const Ice::Current& c = Ice::emptyCurrent) override
{
return true;
}
- friend std::ostream& operator<<(std::ostream& stream, const FramedOrientedPoint& rhs)
+ friend std::ostream&
+ operator<<(std::ostream& stream, const FramedOrientedPoint& rhs)
{
stream << "FramedOrientedPoint: " << std::endl << rhs.output() << std::endl;
return stream;
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
-
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 0f18dcad581ed5ff28b27d8a8edd512a9307b6f3..ea5b4483f0336e2af6054803826fc9ea5524c89a 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -23,21 +23,20 @@
*/
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include <ArmarXCore/observers/AbstractObjectSerializer.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
-#include <VirtualRobot/Robot.h>
#include <VirtualRobot/LinkedCoordinate.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/VirtualRobot.h>
-#include <Eigen/Core>
-#include <Eigen/Geometry>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
using namespace Eigen;
@@ -51,8 +50,7 @@ namespace armarx
{
- FramedDirection::FramedDirection()
- = default;
+ FramedDirection::FramedDirection() = default;
FramedDirection::FramedDirection(const FramedDirection& source) :
IceUtil::Shared(source),
@@ -63,33 +61,45 @@ namespace armarx
{
}
- FramedDirection::FramedDirection(const Eigen::Vector3f& vec, const std::string& frame, const std::string& agent) :
+ FramedDirection::FramedDirection(const Eigen::Vector3f& vec,
+ const std::string& frame,
+ const std::string& agent) :
Vector3(vec)
{
this->frame = frame;
this->agent = agent;
-
}
- FramedDirection::FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame, const std::string& agent) :
+ FramedDirection::FramedDirection(Ice::Float x,
+ ::Ice::Float y,
+ ::Ice::Float z,
+ const std::string& frame,
+ const std::string& agent) :
Vector3(x, y, z)
{
this->frame = frame;
this->agent = agent;
}
- std::string FramedDirection::getFrame() const
+ std::string
+ FramedDirection::getFrame() const
{
return frame;
}
- FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const std::string& newFrame)
+ FramedDirectionPtr
+ FramedDirection::ChangeFrame(const VirtualRobot::RobotPtr& robot,
+ const FramedDirection& framedVec,
+ const std::string& newFrame)
{
ARMARX_CHECK_NOT_NULL(robot);
return ChangeFrame(*robot, framedVec, newFrame);
}
- FramedDirectionPtr FramedDirection::ChangeFrame(const VirtualRobot::Robot& robot, const FramedDirection& framedVec, const std::string& newFrame)
+ FramedDirectionPtr
+ FramedDirection::ChangeFrame(const VirtualRobot::Robot& robot,
+ const FramedDirection& framedVec,
+ const std::string& newFrame)
{
if (framedVec.getFrame() == newFrame)
{
@@ -98,10 +108,12 @@ namespace armarx
if (!robot.hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << robot.getName();
+ throw LocalException() << "The requested frame '" << newFrame
+ << "' does not exists in the robot " << robot.getName();
}
- Eigen::Matrix4f rotToNewFrame = __GetRotationBetweenFrames(framedVec.frame, newFrame, robot);
+ Eigen::Matrix4f rotToNewFrame =
+ __GetRotationBetweenFrames(framedVec.frame, newFrame, robot);
Eigen::Vector3f vecOldFrame = framedVec.Vector3::toEigen();
@@ -116,13 +128,15 @@ namespace armarx
return result;
}
- void FramedDirection::changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame)
+ void
+ FramedDirection::changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame)
{
ARMARX_CHECK_NOT_NULL(robot);
changeFrame(*robot, newFrame);
}
- void FramedDirection::changeFrame(const VirtualRobot::Robot& robot, const std::string& newFrame)
+ void
+ FramedDirection::changeFrame(const VirtualRobot::Robot& robot, const std::string& newFrame)
{
if (frame == "")
{
@@ -143,12 +157,14 @@ namespace armarx
if (!robot.hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exist in the robot " << robot.getName();
+ throw LocalException() << "The requested frame '" << newFrame
+ << "' does not exist in the robot " << robot.getName();
}
if (frame != GlobalFrame && !robot.hasRobotNode(frame))
{
- throw LocalException() << "The current frame '" << frame << "' does not exist in the robot " << robot.getName();
+ throw LocalException() << "The current frame '" << frame
+ << "' does not exist in the robot " << robot.getName();
}
@@ -166,19 +182,22 @@ namespace armarx
agent = robot.getName();
}
- void FramedDirection::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
+ void
+ FramedDirection::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeToGlobal(sharedRobot);
}
- void FramedDirection::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
+ void
+ FramedDirection::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
changeToGlobal(*referenceRobot);
}
- void FramedDirection::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
+ void
+ FramedDirection::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
{
if (frame == GlobalFrame)
{
@@ -186,7 +205,8 @@ namespace armarx
}
changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
- Eigen::Vector3f pos = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
+ Eigen::Vector3f pos =
+ referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
x = pos[0];
y = pos[1];
z = pos[2];
@@ -194,82 +214,107 @@ namespace armarx
agent = "";
}
- FramedDirectionPtr FramedDirection::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
+ FramedDirectionPtr
+ FramedDirection::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
- FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ FramedDirectionPtr
+ FramedDirection::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toGlobal(*referenceRobot);
}
- FramedDirectionPtr FramedDirection::toGlobal(const VirtualRobot::Robot& referenceRobot) const
+
+ FramedDirectionPtr
+ FramedDirection::toGlobal(const VirtualRobot::Robot& referenceRobot) const
{
FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
newDirection->changeToGlobal(referenceRobot);
return newDirection;
}
- Eigen::Vector3f FramedDirection::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Vector3f
+ FramedDirection::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobalEigen(sharedRobot);
}
- Eigen::Vector3f FramedDirection::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ Eigen::Vector3f
+ FramedDirection::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toGlobalEigen(*referenceRobot);
}
- Eigen::Vector3f FramedDirection::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
+
+ Eigen::Vector3f
+ FramedDirection::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedDirection newDirection(toEigen(), frame, agent);
newDirection.changeToGlobal(referenceRobot);
return newDirection.toEigen();
}
- FramedDirectionPtr FramedDirection::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+ FramedDirectionPtr
+ FramedDirection::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootFrame(sharedRobot);
}
- FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ FramedDirectionPtr
+ FramedDirection::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toRootFrame(*referenceRobot);
}
- FramedDirectionPtr FramedDirection::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
+
+ FramedDirectionPtr
+ FramedDirection::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
{
FramedDirectionPtr newDirection = FramedDirectionPtr::dynamicCast(this->clone());
newDirection->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newDirection;
}
- Eigen::Vector3f FramedDirection::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Vector3f
+ FramedDirection::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootEigen(sharedRobot);
}
- Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ Eigen::Vector3f
+ FramedDirection::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toRootEigen(*referenceRobot);
}
- Eigen::Vector3f FramedDirection::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
+
+ Eigen::Vector3f
+ FramedDirection::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedDirection newDirection(toEigen(), frame, agent);
newDirection.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newDirection.toEigen();
}
- std::string FramedDirection::output(const Ice::Current& c) const
+ std::string
+ FramedDirection::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << toEigen().format(ArmarXEigenFormat) << std::endl
+ << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
- Eigen::Matrix4f FramedDirection::__GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, const VirtualRobot::Robot& robotState)
+ Eigen::Matrix4f
+ FramedDirection::__GetRotationBetweenFrames(const std::string& oldFrame,
+ const std::string& newFrame,
+ const VirtualRobot::Robot& robotState)
{
Eigen::Matrix4f toNewFrame;
@@ -282,7 +327,9 @@ namespace armarx
else
{
auto node = robotState.getRobotNode(oldFrame);
- ARMARX_CHECK_EXPRESSION(node) << "old frame: " + oldFrame + ValueToString(robotState.getConfig()->getRobotNodeJointValueMap());
+ ARMARX_CHECK_EXPRESSION(node)
+ << "old frame: " + oldFrame +
+ ValueToString(robotState.getConfig()->getRobotNodeJointValueMap());
auto newNode = robotState.getRobotNode(newFrame);
ARMARX_CHECK_EXPRESSION(newNode) << newFrame;
toNewFrame = node->getTransformationTo(newNode);
@@ -295,17 +342,17 @@ namespace armarx
return toNewFrame;
}
-
- void FramedDirection::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+ void
+ FramedDirection::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
Vector3::serialize(serializer);
obj->setString("frame", frame);
obj->setString("agent", agent);
-
}
- void FramedDirection::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
+ void
+ FramedDirection::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
Vector3::deserialize(serializer);
@@ -317,35 +364,38 @@ namespace armarx
}
}
- Ice::ObjectPtr FramedDirection::ice_clone() const
+ Ice::ObjectPtr
+ FramedDirection::ice_clone() const
{
return this->clone();
}
- VariantDataClassPtr FramedDirection::clone(const Ice::Current& c) const
+ VariantDataClassPtr
+ FramedDirection::clone(const Ice::Current& c) const
{
return new FramedDirection(*this);
}
- VariantTypeId FramedDirection::getType(const Ice::Current& c) const
+ VariantTypeId
+ FramedDirection::getType(const Ice::Current& c) const
{
return VariantType::FramedDirection;
}
- bool FramedDirection::validate(const Ice::Current& c)
+ bool
+ FramedDirection::validate(const Ice::Current& c)
{
return true;
}
- std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs)
+ std::ostream&
+ operator<<(std::ostream& stream, const FramedDirection& rhs)
{
stream << "FramedDirection: " << std::endl << rhs.output() << std::endl;
return stream;
}
-
- FramedPose::FramedPose() :
- Pose()
+ FramedPose::FramedPose() : Pose()
{
frame = "";
}
@@ -358,52 +408,66 @@ namespace armarx
FramedPoseBase(pose),
Pose(pose)
{
-
}
- FramedPose::FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const std::string& agent) :
+ FramedPose::FramedPose(const Eigen::Matrix3f& m,
+ const Eigen::Vector3f& v,
+ const std::string& s,
+ const std::string& agent) :
Pose(m, v)
{
frame = s;
this->agent = agent;
}
- FramedPose::FramedPose(const Eigen::Matrix4f& m, const std::string& s, const std::string& agent) :
+ FramedPose::FramedPose(const Eigen::Matrix4f& m,
+ const std::string& s,
+ const std::string& agent) :
Pose(m)
{
frame = s;
this->agent = agent;
}
- FramedPose::FramedPose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori, const std::string& frame, const std::string& agent) :
+ FramedPose::FramedPose(const armarx::Vector3BasePtr pos,
+ const armarx::QuaternionBasePtr ori,
+ const std::string& frame,
+ const std::string& agent) :
Pose(pos, ori)
{
this->frame = frame;
this->agent = agent;
}
- FramedPose::FramedPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const std::string& frame, const std::string& agent) :
+ FramedPose::FramedPose(const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
+ const std::string& frame,
+ const std::string& agent) :
Pose(pos, ori)
{
this->frame = frame;
this->agent = agent;
}
-
- std::string FramedPose::getFrame() const
+ std::string
+ FramedPose::getFrame() const
{
return frame;
}
- std::string FramedPose::output(const Ice::Current& c) const
+ std::string
+ FramedPose::output(const Ice::Current& c) const
{
std::stringstream s;
const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, 4, " ", "\n", "", "");
- s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << toEigen().format(ArmarXEigenFormat) << std::endl
+ << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
- void FramedPose::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame)
+ void
+ FramedPose::changeFrame(const SharedRobotInterfacePrx& referenceRobot,
+ const std::string& newFrame)
{
if (newFrame == frame)
{
@@ -414,13 +478,16 @@ namespace armarx
changeFrame(sharedRobot, newFrame);
}
- void FramedPose::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
+ void
+ FramedPose::changeFrame(const VirtualRobot::RobotPtr& referenceRobot,
+ const std::string& newFrame)
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
changeFrame(*referenceRobot, newFrame);
}
- void FramedPose::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
+ void
+ FramedPose::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
{
if (newFrame == frame)
{
@@ -437,11 +504,13 @@ namespace armarx
if (frame != GlobalFrame and !referenceRobot.hasRobotNode(frame))
{
- throw LocalException() << "The current frame '" << frame << "' does not exists in the robot " << referenceRobot.getName();
+ throw LocalException() << "The current frame '" << frame
+ << "' does not exists in the robot " << referenceRobot.getName();
}
if (!referenceRobot.hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
+ throw LocalException() << "The requested frame '" << newFrame
+ << "' does not exists in the robot " << referenceRobot.getName();
}
if (frame != GlobalFrame)
@@ -454,7 +523,9 @@ namespace armarx
}
Eigen::Matrix4f newPose =
- (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * toEigen();
+ (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() *
+ oldFrameTransformation) *
+ toEigen();
orientation = new Quaternion(newPose);
Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
@@ -464,18 +535,22 @@ namespace armarx
init();
}
- void FramedPose::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
+ void
+ FramedPose::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeToGlobal(sharedRobot);
-
}
- void FramedPose::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
+
+ void
+ FramedPose::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
changeToGlobal(*referenceRobot);
}
- void FramedPose::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
+
+ void
+ FramedPose::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
{
if (frame == GlobalFrame)
{
@@ -497,88 +572,112 @@ namespace armarx
agent = "";
}
- FramedPosePtr FramedPose::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
+ FramedPosePtr
+ FramedPose::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
- FramedPosePtr FramedPose::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ FramedPosePtr
+ FramedPose::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toGlobal(*referenceRobot);
}
- FramedPosePtr FramedPose::toGlobal(const VirtualRobot::Robot& referenceRobot) const
+
+ FramedPosePtr
+ FramedPose::toGlobal(const VirtualRobot::Robot& referenceRobot) const
{
FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
newPose->changeToGlobal(referenceRobot);
return newPose;
}
- Eigen::Matrix4f FramedPose::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Matrix4f
+ FramedPose::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobalEigen(sharedRobot);
}
- Eigen::Matrix4f FramedPose::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ Eigen::Matrix4f
+ FramedPose::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toGlobalEigen(*referenceRobot);
}
- Eigen::Matrix4f FramedPose::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
+
+ Eigen::Matrix4f
+ FramedPose::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedPose newPose(toEigen(), frame, agent);
newPose.changeToGlobal(referenceRobot);
return newPose.toEigen();
}
- FramedPosePtr FramedPose::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+ FramedPosePtr
+ FramedPose::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootFrame(sharedRobot);
}
- FramedPosePtr FramedPose::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ FramedPosePtr
+ FramedPose::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toRootFrame(*referenceRobot);
}
- FramedPosePtr FramedPose::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
+
+ FramedPosePtr
+ FramedPose::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
{
FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
newPose->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newPose;
}
- Eigen::Matrix4f FramedPose::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Matrix4f
+ FramedPose::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootEigen(sharedRobot);
}
- Eigen::Matrix4f FramedPose::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ Eigen::Matrix4f
+ FramedPose::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toRootEigen(*referenceRobot);
}
- Eigen::Matrix4f FramedPose::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
+
+ Eigen::Matrix4f
+ FramedPose::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedPose newPose(toEigen(), frame, agent);
newPose.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newPose.toEigen();
}
-
- FramedPositionPtr FramedPose::getPosition() const
+ FramedPositionPtr
+ FramedPose::getPosition() const
{
- FramedPositionPtr result = new FramedPosition(Vector3Ptr::dynamicCast(position)->toEigen(), frame, agent);
+ FramedPositionPtr result =
+ new FramedPosition(Vector3Ptr::dynamicCast(position)->toEigen(), frame, agent);
return result;
}
- FramedOrientationPtr FramedPose::getOrientation() const
+ FramedOrientationPtr
+ FramedPose::getOrientation() const
{
- FramedOrientationPtr result = new FramedOrientation(QuaternionPtr::dynamicCast(this->orientation)->toEigen(), frame, agent);
+ FramedOrientationPtr result = new FramedOrientation(
+ QuaternionPtr::dynamicCast(this->orientation)->toEigen(), frame, agent);
return result;
}
- void FramedPose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
+ void
+ FramedPose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -587,7 +686,8 @@ namespace armarx
obj->setString("agent", agent);
}
- void FramedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ void
+ FramedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -600,20 +700,23 @@ namespace armarx
}
}
- FramedPosition::FramedPosition() :
- Vector3()
+ FramedPosition::FramedPosition() : Vector3()
{
frame = "";
}
- FramedPosition::FramedPosition(const Eigen::Vector3f& v, const std::string& s, const std::string& agent) :
+ FramedPosition::FramedPosition(const Eigen::Vector3f& v,
+ const std::string& s,
+ const std::string& agent) :
Vector3(v)
{
frame = s;
this->agent = agent;
}
- FramedPosition::FramedPosition(const Eigen::Matrix4f& m, const std::string& s, const std::string& agent) :
+ FramedPosition::FramedPosition(const Eigen::Matrix4f& m,
+ const std::string& s,
+ const std::string& agent) :
Vector3(m)
{
frame = s;
@@ -629,12 +732,15 @@ namespace armarx
// this->frame = frame;
// }
- std::string FramedPosition::getFrame() const
+ std::string
+ FramedPosition::getFrame() const
{
return this->frame;
}
- void FramedPosition::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame)
+ void
+ FramedPosition::changeFrame(const SharedRobotInterfacePrx& referenceRobot,
+ const std::string& newFrame)
{
if (newFrame == frame)
{
@@ -643,20 +749,26 @@ namespace armarx
if (!referenceRobot->hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot->getName();
+ throw LocalException()
+ << "The requested frame '" << newFrame << "' does not exists in the robot "
+ << referenceRobot->getName();
}
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeFrame(sharedRobot, newFrame);
}
- void FramedPosition::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
+ void
+ FramedPosition::changeFrame(const VirtualRobot::RobotPtr& referenceRobot,
+ const std::string& newFrame)
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
changeFrame(*referenceRobot, newFrame);
}
- void FramedPosition::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
+ void
+ FramedPosition::changeFrame(const VirtualRobot::Robot& referenceRobot,
+ const std::string& newFrame)
{
if (newFrame == frame)
{
@@ -671,7 +783,8 @@ namespace armarx
if (newFrame.empty())
{
- ARMARX_WARNING_S << "Frame Conversion: Frame is empty, always set frame names! ...assuming GlobalFrame";
+ ARMARX_WARNING_S << "Frame Conversion: Frame is empty, always set frame names! "
+ "...assuming GlobalFrame";
changeToGlobal(referenceRobot);
return;
}
@@ -680,7 +793,8 @@ namespace armarx
if (!referenceRobot.hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
+ throw LocalException() << "The requested frame '" << newFrame
+ << "' does not exists in the robot " << referenceRobot.getName();
}
if (frame != GlobalFrame)
@@ -695,7 +809,9 @@ namespace armarx
Eigen::Matrix4f oldPose = Eigen::Matrix4f::Identity();
oldPose.block<3, 1>(0, 3) = toEigen();
Eigen::Matrix4f newPose =
- (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
+ (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() *
+ oldFrameTransformation) *
+ oldPose;
Eigen::Vector3f pos = newPose.block<3, 1>(0, 3);
x = pos[0];
@@ -705,17 +821,22 @@ namespace armarx
frame = newFrame;
}
- void FramedPosition::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
+ void
+ FramedPosition::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeToGlobal(sharedRobot);
}
- void FramedPosition::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
+
+ void
+ FramedPosition::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
changeToGlobal(*referenceRobot);
}
- void FramedPosition::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
+
+ void
+ FramedPosition::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
{
if (frame == GlobalFrame)
{
@@ -723,8 +844,9 @@ namespace armarx
}
changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
- Eigen::Vector3f pos = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen()
- + referenceRobot.getRootNode()->getGlobalPose().block<3, 1>(0, 3);
+ Eigen::Vector3f pos =
+ referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen() +
+ referenceRobot.getRootNode()->getGlobalPose().block<3, 1>(0, 3);
x = pos[0];
y = pos[1];
z = pos[2];
@@ -732,82 +854,106 @@ namespace armarx
agent = "";
}
- FramedPositionPtr FramedPosition::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
+ FramedPositionPtr
+ FramedPosition::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
- FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ FramedPositionPtr
+ FramedPosition::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toGlobal(*referenceRobot);
}
- FramedPositionPtr FramedPosition::toGlobal(const VirtualRobot::Robot& referenceRobot) const
+
+ FramedPositionPtr
+ FramedPosition::toGlobal(const VirtualRobot::Robot& referenceRobot) const
{
FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
newPosition->changeToGlobal(referenceRobot);
return newPosition;
}
- Eigen::Vector3f FramedPosition::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Vector3f
+ FramedPosition::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobalEigen(sharedRobot);
}
- Eigen::Vector3f FramedPosition::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ Eigen::Vector3f
+ FramedPosition::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toGlobalEigen(*referenceRobot);
}
- Eigen::Vector3f FramedPosition::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
+
+ Eigen::Vector3f
+ FramedPosition::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedPosition newPosition(toEigen(), frame, agent);
newPosition.changeToGlobal(referenceRobot);
return newPosition.toEigen();
}
- FramedPositionPtr FramedPosition::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+ FramedPositionPtr
+ FramedPosition::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootFrame(sharedRobot);
}
- FramedPositionPtr FramedPosition::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ FramedPositionPtr
+ FramedPosition::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toRootFrame(*referenceRobot);
}
- FramedPositionPtr FramedPosition::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
+
+ FramedPositionPtr
+ FramedPosition::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
{
FramedPositionPtr newPosition = FramedPositionPtr::dynamicCast(this->clone());
newPosition->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newPosition;
}
- Eigen::Vector3f FramedPosition::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Vector3f
+ FramedPosition::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootEigen(sharedRobot);
}
- Eigen::Vector3f FramedPosition::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ Eigen::Vector3f
+ FramedPosition::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toRootEigen(*referenceRobot);
}
- Eigen::Vector3f FramedPosition::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
+
+ Eigen::Vector3f
+ FramedPosition::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedPosition newPosition(toEigen(), frame, agent);
newPosition.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newPosition.toEigen();
}
- std::string FramedPosition::output(const Ice::Current& c) const
+ std::string
+ FramedPosition::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << toEigen().format(ArmarXEigenFormat) << std::endl
+ << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
- void FramedPosition::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
+ void
+ FramedPosition::serialize(const ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& c) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -816,7 +962,8 @@ namespace armarx
obj->setString("agent", agent);
}
- void FramedPosition::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ void
+ FramedPosition::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -830,15 +977,16 @@ namespace armarx
}
FramedPosition::FramedPosition(const FramedPosition& other) :
- Shared(other),
- armarx::Serializable(other),
- Vector3Base(other.x, other.y, other.z),
- FramedPositionBase(other.x, other.y, other.z, other.frame, other.agent),
- Vector3(other.x, other.y, other.z)
+ Shared(other),
+ armarx::Serializable(other),
+ Vector3Base(other.x, other.y, other.z),
+ FramedPositionBase(other.x, other.y, other.z, other.frame, other.agent),
+ Vector3(other.x, other.y, other.z)
{
}
- FramedPosition& FramedPosition::operator=(const FramedPosition& other)
+ FramedPosition&
+ FramedPosition::operator=(const FramedPosition& other)
{
x = other.x;
y = other.y;
@@ -848,54 +996,63 @@ namespace armarx
return *this;
}
- Ice::ObjectPtr FramedPosition::ice_clone() const
+ Ice::ObjectPtr
+ FramedPosition::ice_clone() const
{
return this->clone();
}
- VariantDataClassPtr FramedPosition::clone(const Ice::Current& c) const
+ VariantDataClassPtr
+ FramedPosition::clone(const Ice::Current& c) const
{
return new FramedPosition(*this);
}
- VariantTypeId FramedPosition::getType(const Ice::Current& c) const
+ VariantTypeId
+ FramedPosition::getType(const Ice::Current& c) const
{
return VariantType::FramedPosition;
}
- bool FramedPosition::validate(const Ice::Current& c)
+ bool
+ FramedPosition::validate(const Ice::Current& c)
{
return true;
}
- std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs)
+ std::ostream&
+ operator<<(std::ostream& stream, const FramedPosition& rhs)
{
stream << "FramedPosition: " << std::endl << rhs.output() << std::endl;
return stream;
}
-
- FramedOrientation::FramedOrientation() :
- Quaternion()
+ FramedOrientation::FramedOrientation() : Quaternion()
{
frame = "";
}
- FramedOrientation::FramedOrientation(const Eigen::Matrix3f& m, const std::string& s, const std::string& agent) :
+ FramedOrientation::FramedOrientation(const Eigen::Matrix3f& m,
+ const std::string& s,
+ const std::string& agent) :
Quaternion(m)
{
frame = s;
this->agent = agent;
}
- FramedOrientation::FramedOrientation(const Quaternionf& q, const std::string& frame, const std::string& agent):
+ FramedOrientation::FramedOrientation(const Quaternionf& q,
+ const std::string& frame,
+ const std::string& agent) :
Quaternion(q)
{
this->frame = frame;
this->agent = agent;
}
- FramedOrientation::FramedOrientation(const Eigen::Matrix4f& m, const std::string& s, const std::string& agent) :
+ FramedOrientation::FramedOrientation(const Eigen::Matrix4f& m,
+ const std::string& s,
+ const std::string& agent) :
Quaternion(m)
{
frame = s;
@@ -910,19 +1067,24 @@ namespace armarx
// FramedOrientation(rot, frame);
// }
- std::string FramedOrientation::getFrame() const
+ std::string
+ FramedOrientation::getFrame() const
{
return this->frame;
}
- std::string FramedOrientation::output(const Ice::Current& c) const
+ std::string
+ FramedOrientation::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen()/*.format(ArmarXEigenFormat)*/ << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << toEigen() /*.format(ArmarXEigenFormat)*/ << std::endl
+ << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
- void FramedOrientation::changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame)
+ void
+ FramedOrientation::changeFrame(const SharedRobotInterfacePrx& referenceRobot,
+ const std::string& newFrame)
{
if (newFrame == frame)
{
@@ -934,13 +1096,17 @@ namespace armarx
changeFrame(sharedRobot, newFrame);
}
- void FramedOrientation::changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame)
+ void
+ FramedOrientation::changeFrame(const VirtualRobot::RobotPtr& referenceRobot,
+ const std::string& newFrame)
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
changeFrame(*referenceRobot, newFrame);
}
- void FramedOrientation::changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame)
+ void
+ FramedOrientation::changeFrame(const VirtualRobot::Robot& referenceRobot,
+ const std::string& newFrame)
{
if (newFrame == frame)
{
@@ -957,7 +1123,8 @@ namespace armarx
if (!referenceRobot.hasRobotNode(newFrame))
{
- throw LocalException() << "The requested frame '" << newFrame << "' does not exists in the robot " << referenceRobot.getName();
+ throw LocalException() << "The requested frame '" << newFrame
+ << "' does not exists in the robot " << referenceRobot.getName();
}
if (frame != GlobalFrame)
@@ -973,7 +1140,9 @@ namespace armarx
oldPose.block<3, 3>(0, 0) = toEigen();
Eigen::Matrix4f newPose =
- (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() * oldFrameTransformation) * oldPose;
+ (referenceRobot.getRobotNode(newFrame)->getPoseInRootFrame().inverse() *
+ oldFrameTransformation) *
+ oldPose;
Eigen::Quaternionf quat(newPose.block<3, 3>(0, 0));
qw = quat.w();
@@ -984,18 +1153,22 @@ namespace armarx
frame = newFrame;
}
- void FramedOrientation::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
+ void
+ FramedOrientation::changeToGlobal(const SharedRobotInterfacePrx& referenceRobot)
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
changeToGlobal(sharedRobot);
}
- void FramedOrientation::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
+ void
+ FramedOrientation::changeToGlobal(const VirtualRobot::RobotPtr& referenceRobot)
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
changeToGlobal(*referenceRobot);
}
- void FramedOrientation::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
+
+ void
+ FramedOrientation::changeToGlobal(const VirtualRobot::Robot& referenceRobot)
{
if (frame == GlobalFrame)
{
@@ -1003,7 +1176,8 @@ namespace armarx
}
changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
- Eigen::Matrix3f rot = referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
+ Eigen::Matrix3f rot =
+ referenceRobot.getRootNode()->getGlobalPose().block<3, 3>(0, 0) * toEigen();
Eigen::Quaternionf quat(rot);
qw = quat.w();
qx = quat.x();
@@ -1013,75 +1187,97 @@ namespace armarx
agent = "";
}
- FramedOrientationPtr FramedOrientation::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
+ FramedOrientationPtr
+ FramedOrientation::toGlobal(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobal(sharedRobot);
}
- FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ FramedOrientationPtr
+ FramedOrientation::toGlobal(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toGlobal(*referenceRobot);
}
- FramedOrientationPtr FramedOrientation::toGlobal(const VirtualRobot::Robot& referenceRobot) const
+
+ FramedOrientationPtr
+ FramedOrientation::toGlobal(const VirtualRobot::Robot& referenceRobot) const
{
FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
newOrientation->changeToGlobal(referenceRobot);
return newOrientation;
}
- Eigen::Matrix3f FramedOrientation::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Matrix3f
+ FramedOrientation::toGlobalEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toGlobalEigen(sharedRobot);
}
- Eigen::Matrix3f FramedOrientation::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ Eigen::Matrix3f
+ FramedOrientation::toGlobalEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toGlobalEigen(*referenceRobot);
}
- Eigen::Matrix3f FramedOrientation::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
+
+ Eigen::Matrix3f
+ FramedOrientation::toGlobalEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedOrientation newOrientation(toEigen(), frame, agent);
newOrientation.changeToGlobal(referenceRobot);
return newOrientation.toEigen();
}
- FramedOrientationPtr FramedOrientation::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
+ FramedOrientationPtr
+ FramedOrientation::toRootFrame(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootFrame(sharedRobot);
}
- FramedOrientationPtr FramedOrientation::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ FramedOrientationPtr
+ FramedOrientation::toRootFrame(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toRootFrame(*referenceRobot);
}
- FramedOrientationPtr FramedOrientation::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
+
+ FramedOrientationPtr
+ FramedOrientation::toRootFrame(const VirtualRobot::Robot& referenceRobot) const
{
FramedOrientationPtr newOrientation = FramedOrientationPtr::dynamicCast(this->clone());
newOrientation->changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newOrientation;
}
- Eigen::Matrix3f FramedOrientation::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
+ Eigen::Matrix3f
+ FramedOrientation::toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toRootEigen(sharedRobot);
}
- Eigen::Matrix3f FramedOrientation::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
+
+ Eigen::Matrix3f
+ FramedOrientation::toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const
{
ARMARX_CHECK_NOT_NULL(referenceRobot);
return toRootEigen(*referenceRobot);
}
- Eigen::Matrix3f FramedOrientation::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
+
+ Eigen::Matrix3f
+ FramedOrientation::toRootEigen(const VirtualRobot::Robot& referenceRobot) const
{
FramedOrientation newOrientation(toEigen(), frame, agent);
newOrientation.changeFrame(referenceRobot, referenceRobot.getRootNode()->getName());
return newOrientation.toEigen();
}
- void FramedOrientation::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
+ void
+ FramedOrientation::serialize(const ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& c) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -1090,7 +1286,9 @@ namespace armarx
obj->setString("agent", agent);
}
- void FramedOrientation::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ void
+ FramedOrientation::deserialize(const ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& c)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -1103,34 +1301,41 @@ namespace armarx
}
}
- Ice::ObjectPtr FramedOrientation::ice_clone() const
+ Ice::ObjectPtr
+ FramedOrientation::ice_clone() const
{
return this->clone();
}
- VariantDataClassPtr FramedOrientation::clone(const Ice::Current& c) const
+ VariantDataClassPtr
+ FramedOrientation::clone(const Ice::Current& c) const
{
return new FramedOrientation(*this);
}
- VariantTypeId FramedOrientation::getType(const Ice::Current& c) const
+ VariantTypeId
+ FramedOrientation::getType(const Ice::Current& c) const
{
return VariantType::FramedOrientation;
}
- bool FramedOrientation::validate(const Ice::Current& c)
+ bool
+ FramedOrientation::validate(const Ice::Current& c)
{
return true;
}
- std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
+ std::ostream&
+ operator<<(std::ostream& stream, const FramedOrientation& rhs)
{
stream << "FramedOrientation: " << std::endl << rhs.output() << std::endl;
return stream;
}
-
- VirtualRobot::LinkedCoordinate FramedPose::createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation)
+ VirtualRobot::LinkedCoordinate
+ FramedPose::createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot,
+ const FramedPositionPtr& position,
+ const FramedOrientationPtr& orientation)
{
VirtualRobot::LinkedCoordinate c(virtualRobot);
std::string frame;
@@ -1151,7 +1356,8 @@ namespace armarx
{
if (!orientation)
{
- armarx::UserException("createLinkedCoordinate: orientation and position are both NULL");
+ armarx::UserException(
+ "createLinkedCoordinate: orientation and position are both NULL");
}
else
{
@@ -1176,13 +1382,17 @@ namespace armarx
return c;
}
- FramedPosePtr FramedPose::toFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const
+ FramedPosePtr
+ FramedPose::toFrame(const SharedRobotInterfacePrx& referenceRobot,
+ const std::string& newFrame) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toFrame(sharedRobot, newFrame);
}
- FramedPosePtr FramedPose::toFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const
+ FramedPosePtr
+ FramedPose::toFrame(const VirtualRobot::RobotPtr& referenceRobot,
+ const std::string& newFrame) const
{
FramedPosePtr newPose = FramedPosePtr::dynamicCast(this->clone());
newPose->changeFrame(referenceRobot, newFrame);
@@ -1190,54 +1400,64 @@ namespace armarx
}
Eigen::Matrix4f
- FramedPose::toFrameEigen(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const
+ FramedPose::toFrameEigen(const SharedRobotInterfacePrx& referenceRobot,
+ const std::string& newFrame) const
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
return toFrameEigen(sharedRobot, newFrame);
}
Eigen::Matrix4f
- FramedPose::toFrameEigen(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const
+ FramedPose::toFrameEigen(const VirtualRobot::RobotPtr& referenceRobot,
+ const std::string& newFrame) const
{
return toFrame(referenceRobot, newFrame)->toEigen();
}
- Ice::ObjectPtr FramedPose::ice_clone() const
+ Ice::ObjectPtr
+ FramedPose::ice_clone() const
{
return this->clone();
}
- VariantDataClassPtr FramedPose::clone(const Ice::Current& c) const
+ VariantDataClassPtr
+ FramedPose::clone(const Ice::Current& c) const
{
return new FramedPose(*this);
}
- VariantTypeId FramedPose::getType(const Ice::Current& c) const
+ VariantTypeId
+ FramedPose::getType(const Ice::Current& c) const
{
return VariantType::FramedPose;
}
- bool FramedPose::validate(const Ice::Current& c)
+ bool
+ FramedPose::validate(const Ice::Current& c)
{
return true;
}
- std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
+ std::ostream&
+ operator<<(std::ostream& stream, const FramedPose& rhs)
{
stream << "FramedPose: " << std::endl << rhs.output() << std::endl;
return stream;
}
- bool operator==(const FramedPose& pose1, const FramedPose& pose2)
+ bool
+ operator==(const FramedPose& pose1, const FramedPose& pose2)
{
- if (pose1.frame != pose2.frame || pose1.agent != pose2.agent) return false;
+ if (pose1.frame != pose2.frame || pose1.agent != pose2.agent)
+ return false;
return (pose1.toEigen().isApprox(pose2.toEigen()));
}
- bool operator!=(const FramedPose& pose1, const FramedPose& pose2)
+ bool
+ operator!=(const FramedPose& pose1, const FramedPose& pose2)
{
return !(pose1 == pose2);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 8538d6ba250a8ee543b3cf1484ce4d810b092c6c..130e61b4d76a1c19fbb5932db73a36161a583dac 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -24,12 +24,11 @@
#pragma once
-#include "Pose.h"
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/interface/core/FramedPoseBase.h>
-
+#include "Pose.h"
namespace armarx::VariantType
{
@@ -38,23 +37,27 @@ namespace armarx::VariantType
const VariantTypeId FramedDirection = Variant::addTypeName("::armarx::FramedDirectionBase");
const VariantTypeId FramedPosition = Variant::addTypeName("::armarx::FramedPositionBase");
const VariantTypeId FramedOrientation = Variant::addTypeName("::armarx::FramedOrientationBase");
-}
+} // namespace armarx::VariantType
namespace VirtualRobot
{
class LinkedCoordinate;
}
+
namespace IceProxy::armarx
{
class SharedRobotInterface;
}
+
namespace Eigen
{
using Matrix6f = Matrix<Ice::Float, 6, 6>;
}
+
namespace armarx
{
- using SharedRobotInterfacePrx = ::IceInternal::ProxyHandle< ::IceProxy::armarx::SharedRobotInterface> ;
+ using SharedRobotInterfacePrx =
+ ::IceInternal::ProxyHandle<::IceProxy::armarx::SharedRobotInterface>;
/**
* @ingroup RobotAPI-FramedPose
* Variable of the global coordinate system. use this if you are specifying a global pose.
@@ -80,20 +83,28 @@ namespace armarx
class FramedDirection;
using FramedDirectionPtr = IceInternal::Handle<FramedDirection>;
- class FramedDirection :
- virtual public FramedDirectionBase,
- virtual public Vector3
+ class FramedDirection : virtual public FramedDirectionBase, virtual public Vector3
{
public:
FramedDirection();
FramedDirection(const FramedDirection& source);
- FramedDirection(const Eigen::Vector3f& vec, const std::string& frame, const std::string& agent);
- FramedDirection(Ice::Float x, ::Ice::Float y, ::Ice::Float z, const std::string& frame, const std::string& agent);
+ FramedDirection(const Eigen::Vector3f& vec,
+ const std::string& frame,
+ const std::string& agent);
+ FramedDirection(Ice::Float x,
+ ::Ice::Float y,
+ ::Ice::Float z,
+ const std::string& frame,
+ const std::string& agent);
FramedDirection& operator=(const FramedDirection&) = default;
std::string getFrame() const;
- static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr& robot, const FramedDirection& framedVec, const std::string& newFrame);
- static FramedDirectionPtr ChangeFrame(const VirtualRobot::Robot& robot, const FramedDirection& framedVec, const std::string& newFrame);
+ static FramedDirectionPtr ChangeFrame(const VirtualRobot::RobotPtr& robot,
+ const FramedDirection& framedVec,
+ const std::string& newFrame);
+ static FramedDirectionPtr ChangeFrame(const VirtualRobot::Robot& robot,
+ const FramedDirection& framedVec,
+ const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& robot, const std::string& newFrame);
void changeFrame(const VirtualRobot::Robot& robot, const std::string& newFrame);
@@ -123,11 +134,15 @@ namespace armarx
friend std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs);
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
private:
- static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string& oldFrame, const std::string& newFrame, const VirtualRobot::Robot& robotState);
+ static Eigen::Matrix4f __GetRotationBetweenFrames(const std::string& oldFrame,
+ const std::string& newFrame,
+ const VirtualRobot::Robot& robotState);
};
class FramedPosition;
@@ -139,9 +154,7 @@ namespace armarx
* @ingroup RobotAPI-FramedPose
* @brief The FramedPosition class
*/
- class FramedPosition :
- virtual public FramedPositionBase,
- virtual public Vector3
+ class FramedPosition : virtual public FramedPositionBase, virtual public Vector3
{
public:
FramedPosition();
@@ -154,7 +167,8 @@ namespace armarx
std::string getFrame() const;
- void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
+ void changeFrame(const SharedRobotInterfacePrx& referenceRobot,
+ const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
void changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
@@ -183,8 +197,10 @@ namespace armarx
friend std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs);
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
};
class FramedOrientation;
@@ -196,20 +212,24 @@ namespace armarx
* @ingroup RobotAPI-FramedPose
* @brief The FramedOrientation class
*/
- class FramedOrientation :
- virtual public FramedOrientationBase,
- virtual public Quaternion
+ class FramedOrientation : virtual public FramedOrientationBase, virtual public Quaternion
{
public:
FramedOrientation();
- FramedOrientation(const Eigen::Matrix4f&, const std::string& frame, const std::string& agent);
- FramedOrientation(const Eigen::Matrix3f&, const std::string& frame, const std::string& agent);
- FramedOrientation(const Eigen::Quaternionf& q, const std::string& frame, const std::string& agent);
+ FramedOrientation(const Eigen::Matrix4f&,
+ const std::string& frame,
+ const std::string& agent);
+ FramedOrientation(const Eigen::Matrix3f&,
+ const std::string& frame,
+ const std::string& agent);
+ FramedOrientation(const Eigen::Quaternionf& q,
+ const std::string& frame,
+ const std::string& agent);
FramedOrientation& operator=(const FramedOrientation&) = default;
// this doesnt work for an unknown reason
//FramedOrientation(const QuaternionBasePtr ori, const std::string &frame );
- std::string getFrame()const ;
+ std::string getFrame() const;
// inherited from VariantDataClass
Ice::ObjectPtr ice_clone() const override;
@@ -218,7 +238,8 @@ namespace armarx
VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
- void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
+ void changeFrame(const SharedRobotInterfacePrx& referenceRobot,
+ const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
void changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
@@ -240,12 +261,13 @@ namespace armarx
friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs);
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
};
-
class FramedPose;
using FramedPosePtr = IceInternal::Handle<FramedPose>;
@@ -255,17 +277,24 @@ namespace armarx
* @ingroup RobotAPI-FramedPose
* @brief The FramedPose class
*/
- class FramedPose :
- virtual public FramedPoseBase,
- virtual public Pose
+ class FramedPose : virtual public FramedPoseBase, virtual public Pose
{
public:
FramedPose();
FramedPose(const FramedPose& pose);
- FramedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& frame, const std::string& agent);
+ FramedPose(const Eigen::Matrix3f& m,
+ const Eigen::Vector3f& v,
+ const std::string& frame,
+ const std::string& agent);
FramedPose(const Eigen::Matrix4f& m, const std::string& frame, const std::string& agent);
- FramedPose(const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, const std::string& frame, const std::string& agent);
- FramedPose(const armarx::Vector3BasePtr pos, const armarx::QuaternionBasePtr ori, const std::string& frame, const std::string& agent);
+ FramedPose(const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
+ const std::string& frame,
+ const std::string& agent);
+ FramedPose(const armarx::Vector3BasePtr pos,
+ const armarx::QuaternionBasePtr ori,
+ const std::string& frame,
+ const std::string& agent);
FramedPose& operator=(const armarx::FramedPose&) = default;
@@ -282,7 +311,8 @@ namespace armarx
bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
- void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
+ void changeFrame(const SharedRobotInterfacePrx& referenceRobot,
+ const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
void changeFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame);
void changeToGlobal(const SharedRobotInterfacePrx& referenceRobot);
@@ -300,35 +330,42 @@ namespace armarx
Eigen::Matrix4f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Matrix4f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
Eigen::Matrix4f toRootEigen(const VirtualRobot::Robot& referenceRobot) const;
- FramedPosePtr toFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const;
- FramedPosePtr toFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const;
- FramedPosePtr toFrame(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame) const;
- Eigen::Matrix4f toFrameEigen(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const;
- Eigen::Matrix4f toFrameEigen(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const;
- Eigen::Matrix4f toFrameEigen(const VirtualRobot::Robot& referenceRobot, const std::string& newFrame) const;
+ FramedPosePtr toFrame(const SharedRobotInterfacePrx& referenceRobot,
+ const std::string& newFrame) const;
+ FramedPosePtr toFrame(const VirtualRobot::RobotPtr& referenceRobot,
+ const std::string& newFrame) const;
+ FramedPosePtr toFrame(const VirtualRobot::Robot& referenceRobot,
+ const std::string& newFrame) const;
+ Eigen::Matrix4f toFrameEigen(const SharedRobotInterfacePrx& referenceRobot,
+ const std::string& newFrame) const;
+ Eigen::Matrix4f toFrameEigen(const VirtualRobot::RobotPtr& referenceRobot,
+ const std::string& newFrame) const;
+ Eigen::Matrix4f toFrameEigen(const VirtualRobot::Robot& referenceRobot,
+ const std::string& newFrame) const;
friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs);
FramedPositionPtr getPosition() const;
FramedOrientationPtr getOrientation() const;
- static VirtualRobot::LinkedCoordinate createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation);
+ static VirtualRobot::LinkedCoordinate
+ createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot,
+ const FramedPositionPtr& position,
+ const FramedOrientationPtr& orientation);
friend bool operator==(const FramedPose& pose1, const FramedPose& pose2);
friend bool operator!=(const FramedPose& pose1, const FramedPose& pose2);
public:
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
-
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
};
using FramedPosePtr = IceInternal::Handle<FramedPose>;
-}
-
-extern template class ::IceInternal::Handle< ::armarx::FramedPose>;
-extern template class ::IceInternal::Handle< ::armarx::FramedPosition>;
-extern template class ::IceInternal::Handle< ::armarx::FramedDirection>;
-extern template class ::IceInternal::Handle< ::armarx::FramedOrientation>;
-
-
+} // namespace armarx
+extern template class ::IceInternal::Handle<::armarx::FramedPose>;
+extern template class ::IceInternal::Handle<::armarx::FramedPosition>;
+extern template class ::IceInternal::Handle<::armarx::FramedDirection>;
+extern template class ::IceInternal::Handle<::armarx::FramedOrientation>;
diff --git a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
index 3619a4fc6675dd82c580cf5ecca3a798be820ab2..d146de8ed940e47d2737af7ac1db5433f23ccfd9 100644
--- a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
+++ b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
@@ -22,19 +22,23 @@
*/
#include "JointVelocityRamp.h"
+
#include <ArmarXCore/core/logging/Logging.h>
namespace armarx
{
JointVelocityRamp::JointVelocityRamp()
- { }
+ {
+ }
- void JointVelocityRamp::setState(const Eigen::VectorXf& state)
+ void
+ JointVelocityRamp::setState(const Eigen::VectorXf& state)
{
this->state = state;
}
- Eigen::VectorXf JointVelocityRamp::update(const Eigen::VectorXf& target, float dt)
+ Eigen::VectorXf
+ JointVelocityRamp::update(const Eigen::VectorXf& target, float dt)
{
if (dt <= 0)
{
@@ -49,17 +53,17 @@ namespace armarx
state += delta / factor;
// ARMARX_IMPORTANT << "JointRamp state " << state;
return state;
-
}
- void JointVelocityRamp::setMaxAccelaration(float maxAcceleration)
+ void
+ JointVelocityRamp::setMaxAccelaration(float maxAcceleration)
{
this->maxAcceleration = maxAcceleration;
-
}
- float JointVelocityRamp::getMaxAccelaration()
+ float
+ JointVelocityRamp::getMaxAccelaration()
{
return maxAcceleration;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/JointVelocityRamp.h b/source/RobotAPI/libraries/core/JointVelocityRamp.h
index 95c29ac2660774691e1805a6af27ac805e6635c4..4eb1b8097788e4f568317fbe69e693ca5160d92d 100644
--- a/source/RobotAPI/libraries/core/JointVelocityRamp.h
+++ b/source/RobotAPI/libraries/core/JointVelocityRamp.h
@@ -44,9 +44,7 @@ namespace armarx
float getMaxAccelaration();
private:
-
Eigen::VectorXf state;
float maxAcceleration;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/LinkedPose.cpp b/source/RobotAPI/libraries/core/LinkedPose.cpp
index 1f4a2b6fe5f66815fb87eb53d60bcf876e6a39ed..c9dd76ed2bb657ed573b5938c0507256eb7619d1 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.cpp
+++ b/source/RobotAPI/libraries/core/LinkedPose.cpp
@@ -23,26 +23,23 @@
*/
#include "LinkedPose.h"
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <Eigen/Geometry>
#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <Ice/ObjectAdapter.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
-#include <Ice/ObjectAdapter.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
namespace armarx
{
- LinkedPose::LinkedPose() :
- Pose(),
- FramedPose()
+ LinkedPose::LinkedPose() : Pose(), FramedPose()
{
this->referenceRobot = nullptr;
}
@@ -79,21 +76,23 @@ namespace armarx
//ARMARX_WARNING_S << "Calling referenceRobot->ref() in cctor of LinkedPose";
referenceRobot->ref();
}
-
}
- LinkedPose::LinkedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
- Pose(m, v),
- FramedPose(m, v, s, referenceRobot->getName())
+ LinkedPose::LinkedPose(const Eigen::Matrix3f& m,
+ const Eigen::Vector3f& v,
+ const std::string& s,
+ const SharedRobotInterfacePrx& referenceRobot) :
+ Pose(m, v), FramedPose(m, v, s, referenceRobot->getName())
{
ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero";
referenceRobot->ref();
this->referenceRobot = referenceRobot;
}
- LinkedPose::LinkedPose(const Eigen::Matrix4f& m, const std::string& s, const SharedRobotInterfacePrx& referenceRobot) :
- Pose(m),
- FramedPose(m, s, referenceRobot->getName())
+ LinkedPose::LinkedPose(const Eigen::Matrix4f& m,
+ const std::string& s,
+ const SharedRobotInterfacePrx& referenceRobot) :
+ Pose(m), FramedPose(m, s, referenceRobot->getName())
{
ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero";
referenceRobot->ref();
@@ -115,8 +114,8 @@ namespace armarx
}
}
-
- VirtualRobot::LinkedCoordinate LinkedPose::createLinkedCoordinate()
+ VirtualRobot::LinkedCoordinate
+ LinkedPose::createLinkedCoordinate()
{
VirtualRobot::RobotPtr sharedRobot(new RemoteRobot(referenceRobot));
VirtualRobot::LinkedCoordinate c(sharedRobot);
@@ -132,52 +131,61 @@ namespace armarx
return c;
}
- Ice::ObjectPtr LinkedPose::ice_clone() const
+ Ice::ObjectPtr
+ LinkedPose::ice_clone() const
{
return this->clone();
}
- VariantDataClassPtr LinkedPose::clone(const Ice::Current& c) const
+ VariantDataClassPtr
+ LinkedPose::clone(const Ice::Current& c) const
{
return new LinkedPose(*this);
}
- std::string LinkedPose::output(const Ice::Current& c) const
+ std::string
+ LinkedPose::output(const Ice::Current& c) const
{
std::stringstream s;
- s << FramedPose::output() << std::endl << "reference robot: " << referenceRobot->ice_toString();
+ s << FramedPose::output() << std::endl
+ << "reference robot: " << referenceRobot->ice_toString();
return s.str();
}
- VariantTypeId LinkedPose::getType(const Ice::Current& c) const
+ VariantTypeId
+ LinkedPose::getType(const Ice::Current& c) const
{
return VariantType::LinkedPose;
}
- bool LinkedPose::validate(const Ice::Current& c)
+ bool
+ LinkedPose::validate(const Ice::Current& c)
{
return true;
}
- void LinkedPose::changeFrame(const std::string& newFrame, const Ice::Current& c)
+ void
+ LinkedPose::changeFrame(const std::string& newFrame, const Ice::Current& c)
{
FramedPose::changeFrame(referenceRobot, newFrame);
}
- void LinkedPose::changeToGlobal()
+ void
+ LinkedPose::changeToGlobal()
{
FramedPose::changeToGlobal(referenceRobot);
}
- LinkedPosePtr LinkedPose::toGlobal() const
+ LinkedPosePtr
+ LinkedPose::toGlobal() const
{
FramedPosePtr fp = this->FramedPose::toGlobal(referenceRobot);
LinkedPosePtr newPose = new LinkedPose(fp->toEigen(), fp->frame, referenceRobot);
return newPose;
}
-
- void LinkedPose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
+ void
+ LinkedPose::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -185,22 +193,26 @@ namespace armarx
obj->setString("referenceRobot", "");
}
- void LinkedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+ void
+ LinkedPose::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
FramedPose::deserialize(obj);
std::string remoteRobotId = obj->getString("referenceRobot");
- referenceRobot = SharedRobotInterfacePrx::uncheckedCast(c.adapter->getCommunicator()->stringToProxy(remoteRobotId));
+ referenceRobot = SharedRobotInterfacePrx::uncheckedCast(
+ c.adapter->getCommunicator()->stringToProxy(remoteRobotId));
if (!referenceRobot)
{
- ARMARX_ERROR_S << "ReferenceRobot for LinkedPose not registered: " << remoteRobotId << flush;
+ ARMARX_ERROR_S << "ReferenceRobot for LinkedPose not registered: " << remoteRobotId
+ << flush;
}
}
- void LinkedPose::ice_postUnmarshal()
+ void
+ LinkedPose::ice_postUnmarshal()
{
if (referenceRobot)
{
@@ -211,9 +223,7 @@ namespace armarx
FramedPose::ice_postUnmarshal();
}
-
- LinkedDirection::LinkedDirection()
- = default;
+ LinkedDirection::LinkedDirection() = default;
LinkedDirection::LinkedDirection(const LinkedDirection& source) :
IceUtil::Shared(source),
@@ -233,7 +243,9 @@ namespace armarx
}
}
- LinkedDirection::LinkedDirection(const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot) :
+ LinkedDirection::LinkedDirection(const Eigen::Vector3f& v,
+ const std::string& frame,
+ const SharedRobotInterfacePrx& referenceRobot) :
FramedDirection(v, frame, referenceRobot->getName())
{
referenceRobot->ref();
@@ -255,7 +267,8 @@ namespace armarx
}
}
- void LinkedDirection::changeFrame(const std::string& newFrame, const Ice::Current& c)
+ void
+ LinkedDirection::changeFrame(const std::string& newFrame, const Ice::Current& c)
{
if (newFrame == frame)
{
@@ -271,18 +284,20 @@ namespace armarx
frame = frVec->frame;
}
-
- void LinkedDirection::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+ void
+ LinkedDirection::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
{
throw LocalException("LinkedDirection cannot be serialized! Serialize FramedDirection");
}
- void LinkedDirection::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
+ void
+ LinkedDirection::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
{
throw LocalException("LinkedDirection cannot be deserialized! Deserialize FramedDirection");
}
- void LinkedDirection::ice_postUnmarshal()
+ void
+ LinkedDirection::ice_postUnmarshal()
{
if (referenceRobot)
{
@@ -293,11 +308,11 @@ namespace armarx
FramedDirection::ice_postUnmarshal();
}
-
- void VariantType::suppressWarningUnusedVariableForLinkedPoseAndDirection()
+ void
+ VariantType::suppressWarningUnusedVariableForLinkedPoseAndDirection()
{
ARMARX_DEBUG_S << VAROUT(LinkedPose);
ARMARX_DEBUG_S << VAROUT(LinkedDirection);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/LinkedPose.h b/source/RobotAPI/libraries/core/LinkedPose.h
index 71a4240c81060d86e75cbf204ef3de726bd685a3..172d7ba069c0617658c8579fcd88ca88fe8d7ba5 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.h
+++ b/source/RobotAPI/libraries/core/LinkedPose.h
@@ -24,18 +24,18 @@
#pragma once
-#include "FramedPose.h"
+#include <sstream>
-#include <RobotAPI/interface/core/LinkedPoseBase.h>
-#include <RobotAPI/interface/core/RobotState.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
#include <ArmarXCore/observers/variant/Variant.h>
-#include <Eigen/Core>
-#include <Eigen/Geometry>
+#include <RobotAPI/interface/core/LinkedPoseBase.h>
+#include <RobotAPI/interface/core/RobotState.h>
-#include <sstream>
+#include "FramedPose.h"
namespace armarx::VariantType
{
@@ -44,7 +44,7 @@ namespace armarx::VariantType
const VariantTypeId LinkedDirection = Variant::addTypeName("::armarx::LinkedDirectionBase");
void suppressWarningUnusedVariableForLinkedPoseAndDirection();
-}
+} // namespace armarx::VariantType
namespace armarx
{
@@ -57,16 +57,19 @@ namespace armarx
* @ingroup RobotAPI-FramedPose
* @brief The LinkedPose class
*/
- class LinkedPose :
- virtual public LinkedPoseBase,
- virtual public FramedPose
+ class LinkedPose : virtual public LinkedPoseBase, virtual public FramedPose
{
public:
LinkedPose();
LinkedPose(const LinkedPose& other);
LinkedPose(const FramedPose& other, const SharedRobotInterfacePrx& referenceRobot);
- LinkedPose(const Eigen::Matrix3f& m, const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot);
- LinkedPose(const Eigen::Matrix4f& m, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot);
+ LinkedPose(const Eigen::Matrix3f& m,
+ const Eigen::Vector3f& v,
+ const std::string& frame,
+ const SharedRobotInterfacePrx& referenceRobot);
+ LinkedPose(const Eigen::Matrix4f& m,
+ const std::string& frame,
+ const SharedRobotInterfacePrx& referenceRobot);
~LinkedPose() override;
@@ -84,25 +87,27 @@ namespace armarx
bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
- void changeFrame(const std::string& newFrame, const Ice::Current& c = Ice::emptyCurrent) override;
+ void changeFrame(const std::string& newFrame,
+ const Ice::Current& c = Ice::emptyCurrent) override;
void changeToGlobal();
LinkedPosePtr toGlobal() const;
- friend std::ostream& operator<<(std::ostream& stream, const LinkedPose& rhs)
+ friend std::ostream&
+ operator<<(std::ostream& stream, const LinkedPose& rhs)
{
stream << "LinkedPose: " << std::endl << rhs.output() << std::endl;
return stream;
};
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
- protected:
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
+ protected:
void ice_postUnmarshal() override;
};
-
-
/**
* @class LinkedDirection is a direction vector (NOT a position vector) with an attached robotstate proxy
* for frame changes.
@@ -110,63 +115,68 @@ namespace armarx
* @ingroup RobotAPI-FramedPose
* @brief The LinkedDirection class
*/
- class LinkedDirection :
- virtual public LinkedDirectionBase,
- virtual public FramedDirection
+ class LinkedDirection : virtual public LinkedDirectionBase, virtual public FramedDirection
{
public:
LinkedDirection();
LinkedDirection(const LinkedDirection& source);
- LinkedDirection(const Eigen::Vector3f& v, const std::string& frame, const SharedRobotInterfacePrx& referenceRobot);
+ LinkedDirection(const Eigen::Vector3f& v,
+ const std::string& frame,
+ const SharedRobotInterfacePrx& referenceRobot);
~LinkedDirection() override;
- void changeFrame(const std::string& newFrame, const Ice::Current& c = Ice::emptyCurrent) override;
+ void changeFrame(const std::string& newFrame,
+ const Ice::Current& c = Ice::emptyCurrent) override;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
+ Ice::ObjectPtr
+ ice_clone() const override
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
+ VariantDataClassPtr
+ clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new LinkedDirection(*this);
}
- std::string output(const Ice::Current& c = Ice::emptyCurrent) const override
+ std::string
+ output(const Ice::Current& c = Ice::emptyCurrent) const override
{
std::stringstream s;
s << FramedDirection::toEigen() << std::endl << "reference robot: " << referenceRobot;
return s.str();
}
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
+ VariantTypeId
+ getType(const Ice::Current& c = Ice::emptyCurrent) const override
{
return VariantType::LinkedDirection;
}
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
+ bool
+ validate(const Ice::Current& c = Ice::emptyCurrent) override
{
return true;
}
-
- friend std::ostream& operator<<(std::ostream& stream, const LinkedDirection& rhs)
+ friend std::ostream&
+ operator<<(std::ostream& stream, const LinkedDirection& rhs)
{
stream << "LinkedDirection: " << std::endl << rhs.output() << std::endl;
return stream;
};
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
protected:
-
void ice_postUnmarshal() override;
-
-
};
- using LinkedDirectionPtr = IceInternal::Handle<LinkedDirection>;
-}
+ using LinkedDirectionPtr = IceInternal::Handle<LinkedDirection>;
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
index 97ecb3dcc73811482a8c9e2be738c2753709aa07..d7b4cb2b45af7edbce5db705b8af16123f9242d4 100644
--- a/source/RobotAPI/libraries/core/MultiDimPIDController.h
+++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h
@@ -23,230 +23,250 @@
*/
#pragma once
-#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <mutex>
+
+#include <Eigen/Core>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <Eigen/Core>
-
-#include <mutex>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
namespace armarx
{
template <int dimensions = Eigen::Dynamic>
- class MultiDimPIDControllerTemplate :
- public Logging
+ class MultiDimPIDControllerTemplate : public Logging
{
public:
- using PIDVectorX = Eigen::Matrix<float, dimensions, 1>;
-
- MultiDimPIDControllerTemplate(float Kp,
- float Ki,
- float Kd,
- double maxControlValue = std::numeric_limits<double>::max(),
- double maxDerivation = std::numeric_limits<double>::max(),
- bool threadSafe = true,
- std::vector<bool> limitless = {}) :
- Kp(Kp),
- Ki(Ki),
- Kd(Kd),
- integral(0),
- derivative(0),
- previousError(0),
- maxControlValue(maxControlValue),
- maxDerivation(maxDerivation),
- threadSafe(threadSafe),
- limitless(limitless)
- {
- reset();
- }
-
- void preallocate(size_t size)
- {
- stackAllocations.zeroVec = PIDVectorX::Zero(size);
- stackAllocations.errorVec = stackAllocations.zeroVec;
- stackAllocations.direction = stackAllocations.zeroVec;
- stackAllocations.oldControlValue = stackAllocations.zeroVec;
- }
-
- ~MultiDimPIDControllerTemplate() {}
- void update(const double deltaSec, const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
- {
- ScopedRecursiveLockPtr lock = getLock();
- if (stackAllocations.zeroVec.rows() == 0)
- {
- preallocate(measuredValue.rows());
- }
- ARMARX_CHECK_EQUAL(measuredValue.rows(), targetValue.rows());
- ARMARX_CHECK_EQUAL(measuredValue.rows(), stackAllocations.zeroVec.rows());
- processValue = measuredValue;
- target = targetValue;
-
- stackAllocations.errorVec = target - processValue;
-
- if (limitless.size() != 0)
- {
- ARMARX_CHECK_EQUAL(limitless.size(), (size_t)stackAllocations.errorVec.rows());
- for (size_t i = 0; i < limitless.size(); i++)
- {
- if (limitless.at(i))
- {
- stackAllocations.errorVec(i) = math::MathUtils::angleModPI(stackAllocations.errorVec(i));
- }
- }
- }
-
-
- double error = stackAllocations.errorVec.norm();
-
- //double dt = (now - lastUpdateTime).toSecondsDouble();
- // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
- if (!firstRun)
- {
- integral += error * deltaSec;
- integral = std::min(integral, maxIntegral);
- if (deltaSec > 0.0)
- {
- derivative = (error - previousError) / deltaSec;
- }
- }
-
- firstRun = false;
- stackAllocations.direction = targetValue; // copy size
-
- if (error > 0)
- {
- stackAllocations.direction = stackAllocations.errorVec.normalized();
- }
- else
- {
- stackAllocations.direction.setZero();
- }
-
- if (controlValue.rows() > 0)
- {
- stackAllocations.oldControlValue = controlValue;
- }
- else
- {
- stackAllocations.oldControlValue = stackAllocations.zeroVec;
- }
- controlValue = stackAllocations.direction * (Kp * error + Ki * integral + Kd * derivative);
-
- if (deltaSec > 0.0)
- {
- PIDVectorX accVec = (controlValue - stackAllocations.oldControlValue) / deltaSec;
- float maxNewJointAcc = accVec.maxCoeff();
- float minNewJointAcc = accVec.minCoeff();
- maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
- if (maxNewJointAcc > maxDerivation)
- {
- auto newValue = stackAllocations.oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
- ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue) << VAROUT(stackAllocations.oldControlValue) << VAROUT(newValue);
- controlValue = newValue;
- }
- }
-
-
- float max = controlValue.maxCoeff();
- float min = controlValue.minCoeff();
- max = std::max<float>(fabs(min), fabs(max));
-
-
-
- if (max > maxControlValue)
- {
- auto newValue = controlValue * maxControlValue / max;
- ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
- controlValue = newValue;
- }
- ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
-
- previousError = error;
- lastUpdateTime += IceUtil::Time::seconds(deltaSec);
-
- }
- void update(const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
- {
- ScopedRecursiveLockPtr lock = getLock();
- IceUtil::Time now = TimeUtil::GetTime();
-
- if (firstRun)
- {
- lastUpdateTime = TimeUtil::GetTime();
- }
-
- double dt = (now - lastUpdateTime).toSecondsDouble();
- update(dt, measuredValue, targetValue);
- lastUpdateTime = now;
- }
- const PIDVectorX&
- getControlValue() const
- {
- return controlValue;
- }
- void setMaxControlValue(double value)
- {
- ScopedRecursiveLockPtr lock = getLock();
- maxControlValue = value;
- }
-
- void reset()
- {
- ScopedRecursiveLockPtr lock = getLock();
- firstRun = true;
- previousError = 0;
- integral = 0;
- lastUpdateTime = TimeUtil::GetTime();
- //reset control
- controlValue.setZero();
- processValue.setZero();
- target.setZero();
-
-
- }
- // protected:
- float Kp, Ki, Kd;
- double integral;
- double maxIntegral = std::numeric_limits<double>::max();
- double derivative;
- double previousError;
- PIDVectorX processValue;
- PIDVectorX target;
- IceUtil::Time lastUpdateTime;
- PIDVectorX controlValue;
- double maxControlValue;
- double maxDerivation;
- bool firstRun;
- mutable std::recursive_mutex mutex;
- bool threadSafe = true;
- std::vector<bool> limitless;
+ using PIDVectorX = Eigen::Matrix<float, dimensions, 1>;
+
+ MultiDimPIDControllerTemplate(float Kp,
+ float Ki,
+ float Kd,
+ double maxControlValue = std::numeric_limits<double>::max(),
+ double maxDerivation = std::numeric_limits<double>::max(),
+ bool threadSafe = true,
+ std::vector<bool> limitless = {}) :
+ Kp(Kp),
+ Ki(Ki),
+ Kd(Kd),
+ integral(0),
+ derivative(0),
+ previousError(0),
+ maxControlValue(maxControlValue),
+ maxDerivation(maxDerivation),
+ threadSafe(threadSafe),
+ limitless(limitless)
+ {
+ reset();
+ }
+
+ void
+ preallocate(size_t size)
+ {
+ stackAllocations.zeroVec = PIDVectorX::Zero(size);
+ stackAllocations.errorVec = stackAllocations.zeroVec;
+ stackAllocations.direction = stackAllocations.zeroVec;
+ stackAllocations.oldControlValue = stackAllocations.zeroVec;
+ }
+
+ ~MultiDimPIDControllerTemplate()
+ {
+ }
+
+ void
+ update(const double deltaSec,
+ const PIDVectorX& measuredValue,
+ const PIDVectorX& targetValue)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ if (stackAllocations.zeroVec.rows() == 0)
+ {
+ preallocate(measuredValue.rows());
+ }
+ ARMARX_CHECK_EQUAL(measuredValue.rows(), targetValue.rows());
+ ARMARX_CHECK_EQUAL(measuredValue.rows(), stackAllocations.zeroVec.rows());
+ processValue = measuredValue;
+ target = targetValue;
+
+ stackAllocations.errorVec = target - processValue;
+
+ if (limitless.size() != 0)
+ {
+ ARMARX_CHECK_EQUAL(limitless.size(), (size_t)stackAllocations.errorVec.rows());
+ for (size_t i = 0; i < limitless.size(); i++)
+ {
+ if (limitless.at(i))
+ {
+ stackAllocations.errorVec(i) =
+ math::MathUtils::angleModPI(stackAllocations.errorVec(i));
+ }
+ }
+ }
+
+
+ double error = stackAllocations.errorVec.norm();
+
+ //double dt = (now - lastUpdateTime).toSecondsDouble();
+ // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
+ if (!firstRun)
+ {
+ integral += error * deltaSec;
+ integral = std::min(integral, maxIntegral);
+ if (deltaSec > 0.0)
+ {
+ derivative = (error - previousError) / deltaSec;
+ }
+ }
+
+ firstRun = false;
+ stackAllocations.direction = targetValue; // copy size
+
+ if (error > 0)
+ {
+ stackAllocations.direction = stackAllocations.errorVec.normalized();
+ }
+ else
+ {
+ stackAllocations.direction.setZero();
+ }
+
+ if (controlValue.rows() > 0)
+ {
+ stackAllocations.oldControlValue = controlValue;
+ }
+ else
+ {
+ stackAllocations.oldControlValue = stackAllocations.zeroVec;
+ }
+ controlValue =
+ stackAllocations.direction * (Kp * error + Ki * integral + Kd * derivative);
+
+ if (deltaSec > 0.0)
+ {
+ PIDVectorX accVec = (controlValue - stackAllocations.oldControlValue) / deltaSec;
+ float maxNewJointAcc = accVec.maxCoeff();
+ float minNewJointAcc = accVec.minCoeff();
+ maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
+ if (maxNewJointAcc > maxDerivation)
+ {
+ auto newValue = stackAllocations.oldControlValue +
+ accVec * maxDerivation / maxNewJointAcc * deltaSec;
+ ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation)
+ << VAROUT(maxNewJointAcc) << VAROUT(controlValue)
+ << VAROUT(stackAllocations.oldControlValue) << VAROUT(newValue);
+ controlValue = newValue;
+ }
+ }
+
+
+ float max = controlValue.maxCoeff();
+ float min = controlValue.minCoeff();
+ max = std::max<float>(fabs(min), fabs(max));
+
+
+ if (max > maxControlValue)
+ {
+ auto newValue = controlValue * maxControlValue / max;
+ ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue
+ << " max value: " << maxControlValue << " new value: " << newValue;
+ controlValue = newValue;
+ }
+ ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue)
+ << " i: " << (Ki * integral) << " d: " << (Kd * derivative)
+ << " dt: " << deltaSec;
+
+ previousError = error;
+ lastUpdateTime += IceUtil::Time::seconds(deltaSec);
+ }
+
+ void
+ update(const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ IceUtil::Time now = TimeUtil::GetTime();
+
+ if (firstRun)
+ {
+ lastUpdateTime = TimeUtil::GetTime();
+ }
+
+ double dt = (now - lastUpdateTime).toSecondsDouble();
+ update(dt, measuredValue, targetValue);
+ lastUpdateTime = now;
+ }
+
+ const PIDVectorX&
+ getControlValue() const
+ {
+ return controlValue;
+ }
+
+ void
+ setMaxControlValue(double value)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ maxControlValue = value;
+ }
+
+ void
+ reset()
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ firstRun = true;
+ previousError = 0;
+ integral = 0;
+ lastUpdateTime = TimeUtil::GetTime();
+ //reset control
+ controlValue.setZero();
+ processValue.setZero();
+ target.setZero();
+ }
+
+ // protected:
+ float Kp, Ki, Kd;
+ double integral;
+ double maxIntegral = std::numeric_limits<double>::max();
+ double derivative;
+ double previousError;
+ PIDVectorX processValue;
+ PIDVectorX target;
+ IceUtil::Time lastUpdateTime;
+ PIDVectorX controlValue;
+ double maxControlValue;
+ double maxDerivation;
+ bool firstRun;
+ mutable std::recursive_mutex mutex;
+ bool threadSafe = true;
+ std::vector<bool> limitless;
+
private:
+ struct StackAllocationHelper
+ {
+ PIDVectorX errorVec;
+ PIDVectorX direction;
+ PIDVectorX oldControlValue;
+ PIDVectorX zeroVec;
+ } stackAllocations;
- struct StackAllocationHelper
- {
- PIDVectorX errorVec;
- PIDVectorX direction;
- PIDVectorX oldControlValue;
- PIDVectorX zeroVec;
- } stackAllocations;
-
- using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
- using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
- ScopedRecursiveLockPtr getLock() const
- {
- if (threadSafe)
- {
- return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
- }
- else
- {
- return ScopedRecursiveLockPtr();
- }
- }
+ using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
+ using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
+
+ ScopedRecursiveLockPtr
+ getLock() const
+ {
+ if (threadSafe)
+ {
+ return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
+ }
+ else
+ {
+ return ScopedRecursiveLockPtr();
+ }
+ }
};
+
using MultiDimPIDController = MultiDimPIDControllerTemplate<>;
using MultiDimPIDControllerPtr = std::shared_ptr<MultiDimPIDControllerTemplate<>>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/OrientedPoint.cpp b/source/RobotAPI/libraries/core/OrientedPoint.cpp
index 7d4654a44dbc1ea7bcc9179ff5e611ff4e02934d..7a17402d7e8aa8e6cd3c4d86a3e7bf126b3e9dd0 100644
--- a/source/RobotAPI/libraries/core/OrientedPoint.cpp
+++ b/source/RobotAPI/libraries/core/OrientedPoint.cpp
@@ -45,8 +45,12 @@ OrientedPoint::OrientedPoint(const Vector3f& position, const Vector3f& normal)
nz = normal(2);
}
-
-OrientedPoint::OrientedPoint(::Ice::Float px, ::Ice::Float py, ::Ice::Float pz, ::Ice::Float nx, ::Ice::Float ny, ::Ice::Float nz)
+OrientedPoint::OrientedPoint(::Ice::Float px,
+ ::Ice::Float py,
+ ::Ice::Float pz,
+ ::Ice::Float nx,
+ ::Ice::Float ny,
+ ::Ice::Float nz)
{
this->px = px;
this->py = py;
@@ -57,22 +61,24 @@ OrientedPoint::OrientedPoint(::Ice::Float px, ::Ice::Float py, ::Ice::Float pz,
this->nz = nz;
}
-Vector3f OrientedPoint::positionToEigen() const
+Vector3f
+OrientedPoint::positionToEigen() const
{
Vector3f p;
p << this->px, this->py, this->pz;
return p;
}
-Vector3f OrientedPoint::normalToEigen() const
+Vector3f
+OrientedPoint::normalToEigen() const
{
Vector3f n;
n << this->nx, this->ny, this->nz;
return n;
}
-
-std::string OrientedPoint::output(const Ice::Current& c) const
+std::string
+OrientedPoint::output(const Ice::Current& c) const
{
Eigen::IOFormat vecfmt(Eigen::FullPrecision, 0, "", ", ", "", "", "(", ")");
std::stringstream s;
@@ -80,7 +86,8 @@ std::string OrientedPoint::output(const Ice::Current& c) const
return s.str();
}
-void OrientedPoint::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
+void
+OrientedPoint::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -93,7 +100,8 @@ void OrientedPoint::serialize(const ObjectSerializerBasePtr& serializer, const :
obj->setFloat("nz", nz);
}
-void OrientedPoint::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
+void
+OrientedPoint::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
diff --git a/source/RobotAPI/libraries/core/OrientedPoint.h b/source/RobotAPI/libraries/core/OrientedPoint.h
index a347e4954ec175669386c5e3ae9f67909e8d8e86..636be48542ca9186ee99ab80299c2c9297724347 100644
--- a/source/RobotAPI/libraries/core/OrientedPoint.h
+++ b/source/RobotAPI/libraries/core/OrientedPoint.h
@@ -21,65 +21,80 @@
#pragma once
-#include <RobotAPI/interface/core/OrientedPoint.h>
-#include <ArmarXCore/observers/variant/Variant.h>
-#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include <sstream>
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include <sstream>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <RobotAPI/interface/core/OrientedPoint.h>
namespace armarx::VariantType
{
// variant types
const VariantTypeId OrientedPoint = Variant::addTypeName("::armarx::OrientedPointBase");
-}
+} // namespace armarx::VariantType
namespace armarx
{
- class OrientedPoint:
- public virtual OrientedPointBase
+ class OrientedPoint : public virtual OrientedPointBase
{
public:
OrientedPoint();
OrientedPoint(const Eigen::Vector3f& position, const Eigen::Vector3f& normal);
- OrientedPoint(::Ice::Float px, ::Ice::Float py, ::Ice::Float pz, ::Ice::Float nx, ::Ice::Float ny, ::Ice::Float nz);
+ OrientedPoint(::Ice::Float px,
+ ::Ice::Float py,
+ ::Ice::Float pz,
+ ::Ice::Float nx,
+ ::Ice::Float ny,
+ ::Ice::Float nz);
virtual Eigen::Vector3f positionToEigen() const;
virtual Eigen::Vector3f normalToEigen() const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
+ Ice::ObjectPtr
+ ice_clone() const override
{
return this->clone();
}
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
+
+ VariantDataClassPtr
+ clone(const Ice::Current& c = Ice::emptyCurrent) const override
{
return new OrientedPoint(*this);
}
+
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
+
+ VariantTypeId
+ getType(const Ice::Current& c = Ice::emptyCurrent) const override
{
return VariantType::OrientedPoint;
}
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
+
+ bool
+ validate(const Ice::Current& c = Ice::emptyCurrent) override
{
return true;
}
- friend std::ostream& operator<<(std::ostream& stream, const OrientedPoint& rhs)
+ friend std::ostream&
+ operator<<(std::ostream& stream, const OrientedPoint& rhs)
{
stream << "OrientedPoint: " << std::endl << rhs.output() << std::endl;
return stream;
}
public: // serialization
- void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) override;
+ void serialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const armarx::ObjectSerializerBasePtr& serializer,
+ const ::Ice::Current& = Ice::emptyCurrent) override;
};
using OrientedPointPtr = IceInternal::Handle<OrientedPoint>;
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index d3b4eb51940bfdf8dbefb2daf7809c1e9d27c7a4..1356141172d4a06cceeeee6c29cb0cd31ef54e85 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -23,13 +23,15 @@
*/
#include "PIDController.h"
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
+
+#include <memory>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
+#include <ArmarXCore/observers/filters/rtfilters/RTFilterBase.h>
-#include <memory>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
namespace armarx
{
@@ -54,7 +56,9 @@ namespace armarx
threadSafe(o.threadSafe),
differentialFilter(std::move(o.differentialFilter)),
pdOutputFilter(std::move(o.pdOutputFilter))
- {}
+ {
+ }
+
PIDController::PIDController(const PIDController& o) :
Kp(o.Kp),
Ki(o.Ki),
@@ -76,8 +80,11 @@ namespace armarx
threadSafe(o.threadSafe),
differentialFilter(o.differentialFilter->clone()),
pdOutputFilter(o.pdOutputFilter->clone())
- {}
- PIDController& PIDController::operator=(PIDController&& o)
+ {
+ }
+
+ PIDController&
+ PIDController::operator=(PIDController&& o)
{
Kp = o.Kp;
Ki = o.Ki;
@@ -101,7 +108,9 @@ namespace armarx
pdOutputFilter = std::move(o.pdOutputFilter);
return *this;
}
- PIDController& PIDController::operator=(const PIDController& o)
+
+ PIDController&
+ PIDController::operator=(const PIDController& o)
{
Kp = o.Kp;
Ki = o.Ki;
@@ -126,7 +135,13 @@ namespace armarx
return *this;
}
- PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless, bool threadSafe) :
+ PIDController::PIDController(float Kp,
+ float Ki,
+ float Kd,
+ double maxControlValue,
+ double maxDerivation,
+ bool limitless,
+ bool threadSafe) :
Kp(Kp),
Ki(Ki),
Kd(Kd),
@@ -145,7 +160,8 @@ namespace armarx
reset();
}
- void PIDController::reset()
+ void
+ PIDController::reset()
{
ScopedRecursiveLockPtr lock = getLock();
firstRun = true;
@@ -162,7 +178,8 @@ namespace armarx
}
}
- auto PIDController::getLock() const -> ScopedRecursiveLockPtr
+ auto
+ PIDController::getLock() const -> ScopedRecursiveLockPtr
{
if (threadSafe)
{
@@ -174,7 +191,8 @@ namespace armarx
}
}
- void PIDController::update(double measuredValue, double targetValue)
+ void
+ PIDController::update(double measuredValue, double targetValue)
{
ScopedRecursiveLockPtr lock = getLock();
IceUtil::Time now = TimeUtil::GetTime();
@@ -189,26 +207,29 @@ namespace armarx
lastUpdateTime = now;
}
-
- void PIDController::update(double measuredValue)
+ void
+ PIDController::update(double measuredValue)
{
ScopedRecursiveLockPtr lock = getLock();
update(measuredValue, target);
}
- void PIDController::setTarget(double newTarget)
+ void
+ PIDController::setTarget(double newTarget)
{
ScopedRecursiveLockPtr lock = getLock();
target = newTarget;
}
- void PIDController::setMaxControlValue(double newMax)
+ void
+ PIDController::setMaxControlValue(double newMax)
{
ScopedRecursiveLockPtr lock = getLock();
maxControlValue = newMax;
}
- void PIDController::update(double deltaSec, double measuredValue, double targetValue)
+ void
+ PIDController::update(double deltaSec, double measuredValue, double targetValue)
{
ScopedRecursiveLockPtr lock = getLock();
processValue = measuredValue;
@@ -262,28 +283,20 @@ namespace armarx
controlValue = oldControlValue + controlValueDerivation;
}
}
- controlValue = std::min(fabs(controlValue), maxControlValue) * math::MathUtils::Sign(controlValue);
- ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
+ controlValue =
+ std::min(fabs(controlValue), maxControlValue) * math::MathUtils::Sign(controlValue);
+ ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue)
+ << " i: " << (Ki * integral) << " d: " << (Kd * derivative)
+ << " dt: " << deltaSec;
previousError = error;
lastUpdateTime += IceUtil::Time::secondsDouble(deltaSec);
-
}
- double PIDController::getControlValue() const
+ double
+ PIDController::getControlValue() const
{
ScopedRecursiveLockPtr lock = getLock();
return controlValue;
}
-}
-
-
-
-
-
-
-
-
-
-
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index 9e183e77050ab09abf3a5e1b1daa309b087eccf5..5063e1bf8ee77e81a743c733bf35d8c4dba2abed 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -24,14 +24,14 @@
#pragma once
-#include "MultiDimPIDController.h"
-
-#include <ArmarXCore/core/logging/Logging.h>
+#include <memory>
+#include <mutex>
#include <Eigen/Core>
-#include <memory>
-#include <mutex>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "MultiDimPIDController.h"
namespace armarx
{
@@ -40,8 +40,7 @@ namespace armarx
class RTFilterBase;
}
- class PIDController :
- public Logging
+ class PIDController : public Logging
{
public:
PIDController() = default;
@@ -55,7 +54,8 @@ namespace armarx
float Kd,
double maxControlValue = std::numeric_limits<double>::max(),
double maxDerivation = std::numeric_limits<double>::max(),
- bool limitless = false, bool threadSafe = true);
+ bool limitless = false,
+ bool threadSafe = true);
void update(double deltaSec, double measuredValue, double targetValue);
void update(double measuredValue, double targetValue);
void update(double measuredValue);
@@ -83,14 +83,15 @@ namespace armarx
bool threadSafe = true;
std::shared_ptr<rtfilters::RTFilterBase> differentialFilter;
std::shared_ptr<rtfilters::RTFilterBase> pdOutputFilter;
+
private:
using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
ScopedRecursiveLockPtr getLock() const;
mutable std::recursive_mutex mutex;
};
- using PIDControllerPtr = std::shared_ptr<PIDController>;
+ using PIDControllerPtr = std::shared_ptr<PIDController>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
index 4305b29ceb6166b81d238e2309c799db83ce1e66..dab0ef183aa2bc5ef4529caabc8d3ed4d7711e13 100644
--- a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
+++ b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp
@@ -22,27 +22,29 @@
#include "RobotAPIObjectFactories.h"
-#include "Trajectory.h"
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/FramedOrientedPoint.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/LinkedPose.h>
-#include <RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h>
-#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
+#include <RobotAPI/libraries/core/OrientedPoint.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
-#include <RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h>
-#include <RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h>
#include <RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h>
-#include <RobotAPI/libraries/core/OrientedPoint.h>
-#include <RobotAPI/libraries/core/FramedOrientedPoint.h>
-#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
+#include <RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h>
+#include <RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h>
+#include <RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h>
+
+#include "Trajectory.h"
using namespace armarx;
using namespace armarx::ObjectFactories;
-ObjectFactoryMap RobotAPIObjectFactories::getFactories()
+ObjectFactoryMap
+RobotAPIObjectFactories::getFactories()
{
ObjectFactoryMap map;
@@ -59,7 +61,8 @@ ObjectFactoryMap RobotAPIObjectFactories::getFactories()
add<armarx::OrientedPointBase, armarx::OrientedPoint>(map);
add<armarx::FramedOrientedPointBase, armarx::FramedOrientedPoint>(map);
- add<armarx::CartesianPositionControllerConfigBase, armarx::CartesianPositionControllerConfig>(map);
+ add<armarx::CartesianPositionControllerConfigBase, armarx::CartesianPositionControllerConfig>(
+ map);
add<armarx::PoseMedianFilterBase, armarx::filters::PoseMedianFilter>(map);
add<armarx::PoseAverageFilterBase, armarx::filters::PoseAverageFilter>(map);
@@ -71,7 +74,8 @@ ObjectFactoryMap RobotAPIObjectFactories::getFactories()
add<armarx::MatrixAvgFilterBase, armarx::filters::MatrixAvgFilter>(map);
add<armarx::MatrixPercentileFilterBase, armarx::filters::MatrixPercentileFilter>(map);
add<armarx::MatrixPercentilesFilterBase, armarx::filters::MatrixPercentilesFilter>(map);
- add<armarx::MatrixCumulativeFrequencyFilterBase, armarx::filters::MatrixCumulativeFrequencyFilter>(map);
+ add<armarx::MatrixCumulativeFrequencyFilterBase,
+ armarx::filters::MatrixCumulativeFrequencyFilter>(map);
add<armarx::TrajectoryBase, armarx::Trajectory>(map);
return map;
diff --git a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
index 4bdf585bb02f8bc7ac91dae7bda03a26b2fd14f4..3936fb35b9c89da0c34caf42f37848c877a9e33a 100644
--- a/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
+++ b/source/RobotAPI/libraries/core/RobotAPIObjectFactories.h
@@ -35,6 +35,8 @@ namespace armarx::ObjectFactories
public:
ObjectFactoryMap getFactories() override;
};
- const FactoryCollectionBaseCleanUp RobotAPIObjectFactoriesVar = FactoryCollectionBase::addToPreregistration(new RobotAPIObjectFactories());
-}
+ const FactoryCollectionBaseCleanUp RobotAPIObjectFactoriesVar =
+ FactoryCollectionBase::addToPreregistration(new RobotAPIObjectFactories());
+
+} // namespace armarx::ObjectFactories
diff --git a/source/RobotAPI/libraries/core/RobotPool.cpp b/source/RobotAPI/libraries/core/RobotPool.cpp
index 731646d134359076c163f2aa002f2e80946079d0..e6e09a5249040588eebb1dc54698914c2da2eee9 100644
--- a/source/RobotAPI/libraries/core/RobotPool.cpp
+++ b/source/RobotAPI/libraries/core/RobotPool.cpp
@@ -26,14 +26,13 @@
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/Robot.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
namespace armarx
{
- RobotPool::RobotPool(VirtualRobot::RobotPtr robot, size_t defaultSize) :
- baseRobot(robot)
+ RobotPool::RobotPool(VirtualRobot::RobotPtr robot, size_t defaultSize) : baseRobot(robot)
{
std::vector<VirtualRobot::RobotPtr> tempVec;
for (size_t i = 0; i < defaultSize; ++i)
@@ -42,7 +41,8 @@ namespace armarx
}
}
- VirtualRobot::RobotPtr RobotPool::getRobot(int inflation)
+ VirtualRobot::RobotPtr
+ RobotPool::getRobot(int inflation)
{
std::scoped_lock lock(mutex);
for (auto& r : robots[inflation])
@@ -54,7 +54,9 @@ namespace armarx
}
ARMARX_INFO << "Cloning robot";
- auto newRobot = baseRobot->clone(baseRobot->getName(), VirtualRobot::CollisionCheckerPtr(new VirtualRobot::CollisionChecker()));
+ auto newRobot = baseRobot->clone(
+ baseRobot->getName(),
+ VirtualRobot::CollisionCheckerPtr(new VirtualRobot::CollisionChecker()));
newRobot->inflateCollisionModel(inflation);
ARMARX_CHECK_EQUAL(newRobot.use_count(), 1);
ARMARX_INFO << "created new robot clone n with inflation " << inflation;
@@ -62,7 +64,8 @@ namespace armarx
return newRobot;
}
- size_t RobotPool::getPoolSize() const
+ size_t
+ RobotPool::getPoolSize() const
{
std::scoped_lock lock(mutex);
size_t size = 0;
@@ -73,7 +76,8 @@ namespace armarx
return size;
}
- size_t RobotPool::getRobotsInUseCount() const
+ size_t
+ RobotPool::getRobotsInUseCount() const
{
std::scoped_lock lock(mutex);
size_t count = 0;
@@ -90,7 +94,8 @@ namespace armarx
return count;
}
- size_t RobotPool::clean()
+ size_t
+ RobotPool::clean()
{
std::scoped_lock lock(mutex);
size_t count = 0;
diff --git a/source/RobotAPI/libraries/core/RobotPool.h b/source/RobotAPI/libraries/core/RobotPool.h
index c65afe3036082809c3f0d36674f40f674d69cc52..32900b24012532b9b0e666270601b606abeb1d5b 100644
--- a/source/RobotAPI/libraries/core/RobotPool.h
+++ b/source/RobotAPI/libraries/core/RobotPool.h
@@ -23,10 +23,10 @@
*/
#pragma once
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <mutex>
#include <map>
+#include <mutex>
+
+#include <VirtualRobot/VirtualRobot.h>
namespace armarx
{
@@ -54,10 +54,12 @@ namespace armarx
* @return number of cleaned up robots
*/
size_t clean();
+
private:
std::map<int, std::vector<VirtualRobot::RobotPtr>> robots;
VirtualRobot::RobotPtr baseRobot;
mutable std::mutex mutex;
};
+
using RobotPoolPtr = std::shared_ptr<RobotPool>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
index 0aa460711fa83b19dc8902bd0c4cfd604d8b633b..b8756b67e1bb3e55e088fa2899ba828ca2ebec76 100644
--- a/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
+++ b/source/RobotAPI/libraries/core/RobotStatechartContext.cpp
@@ -22,25 +22,27 @@
#include "RobotStatechartContext.h"
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/statechart/Statechart.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
namespace armarx
{
// ****************************************************************
// Implementation of Component
// ****************************************************************
- void RobotStatechartContext::onInitStatechartContext()
+ void
+ RobotStatechartContext::onInitStatechartContext()
{
// StatechartContext::onInitStatechart();
ARMARX_INFO << "Init RobotStatechartContext" << flush;
- kinematicUnitObserverName = getProperty<std::string>("KinematicUnitObserverName").getValue();
+ kinematicUnitObserverName =
+ getProperty<std::string>("KinematicUnitObserverName").getValue();
usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
@@ -68,8 +70,8 @@ namespace armarx
}
}
-
- void RobotStatechartContext::onConnectStatechartContext()
+ void
+ RobotStatechartContext::onConnectStatechartContext()
{
// StatechartContext::onConnectStatechart();
ARMARX_INFO << "Starting RobotStatechartContext" << flush;
@@ -79,14 +81,17 @@ namespace armarx
robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(rbStateName);
std::string kinUnitName = getProperty<std::string>("KinematicUnitName").getValue();
kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinUnitName);
- kinematicUnitObserverPrx = getProxy<KinematicUnitObserverInterfacePrx>(kinematicUnitObserverName);
+ kinematicUnitObserverPrx =
+ getProxy<KinematicUnitObserverInterfacePrx>(kinematicUnitObserverName);
- ARMARX_INFO << "Fetched proxies " << kinUnitName << ":" << kinematicUnitPrx << ", " << rbStateName << ": " << robotStateComponent << flush;
+ ARMARX_INFO << "Fetched proxies " << kinUnitName << ":" << kinematicUnitPrx << ", "
+ << rbStateName << ": " << robotStateComponent << flush;
if (!headIKUnitName.empty())
{
headIKUnitPrx = getProxy<HeadIKUnitInterfacePrx>(headIKUnitName);
- ARMARX_INFO << "Fetched headIK proxy " << headIKUnitName << ":" << headIKUnitPrx << ", head IK kin chain:" << headIKKinematicChainName << flush;
+ ARMARX_INFO << "Fetched headIK proxy " << headIKUnitName << ":" << headIKUnitPrx
+ << ", head IK kin chain:" << headIKKinematicChainName << flush;
}
@@ -100,7 +105,8 @@ namespace armarx
simox::alg::trim(handUnitList.at(i));
HandUnitInterfacePrx handPrx = getProxy<HandUnitInterfacePrx>(handUnitList.at(i));
handUnits[handUnitList.at(i)] = handPrx;
- ARMARX_INFO << "Fetched handUnit proxy " << handUnitList.at(i) << ": " << handPrx << flush;
+ ARMARX_INFO << "Fetched handUnit proxy " << handUnitList.at(i) << ": " << handPrx
+ << flush;
}
}
@@ -109,13 +115,14 @@ namespace armarx
ARMARX_INFO << "Created remote robot" << flush;
}
- PropertyDefinitionsPtr RobotStatechartContext::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ RobotStatechartContext::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new RobotStatechartContextProperties(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(new RobotStatechartContextProperties(getConfigIdentifier()));
}
- HandUnitInterfacePrx RobotStatechartContext::getHandUnit(const std::string& handUnitName)
+ HandUnitInterfacePrx
+ RobotStatechartContext::getHandUnit(const std::string& handUnitName)
{
if (handUnits.find(handUnitName) != handUnits.end())
{
@@ -128,7 +135,8 @@ namespace armarx
while (it != handUnits.end())
{
- ARMARX_INFO << "************ Known hand units: " << it->first << ":" << it->second << flush;
+ ARMARX_INFO << "************ Known hand units: " << it->first << ":" << it->second
+ << flush;
it++;
}
@@ -140,5 +148,4 @@ namespace armarx
return remoteRobot;
}*/
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/RobotStatechartContext.h b/source/RobotAPI/libraries/core/RobotStatechartContext.h
index 511c41feab84494c21b80b398715c7fa32d68bf5..764879627978b3a07e75be3d7544578d55994763 100644
--- a/source/RobotAPI/libraries/core/RobotStatechartContext.h
+++ b/source/RobotAPI/libraries/core/RobotStatechartContext.h
@@ -23,18 +23,18 @@
#pragma once
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/interface/units/HandUnitInterface.h>
-#include <RobotAPI/interface/units/TCPControlUnit.h>
-#include <RobotAPI/interface/units/HeadIKUnit.h>
-#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
+#include <IceUtil/Time.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/statechart/StatechartContext.h>
-#include <IceUtil/Time.h>
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
+#include <RobotAPI/interface/units/HandUnitInterface.h>
+#include <RobotAPI/interface/units/HeadIKUnit.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <RobotAPI/interface/units/TCPControlUnit.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx
{
@@ -45,17 +45,30 @@ namespace armarx
struct RobotStatechartContextProperties : StatechartContextPropertyDefinitions
{
- RobotStatechartContextProperties(std::string prefix):
+ RobotStatechartContextProperties(std::string prefix) :
StatechartContextPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("KinematicUnitName", "Name of the kinematic unit that should be used");
- defineRequiredProperty<std::string>("KinematicUnitObserverName", "Name of the kinematic unit observer that should be used");
+ defineRequiredProperty<std::string>("KinematicUnitName",
+ "Name of the kinematic unit that should be used");
+ defineRequiredProperty<std::string>(
+ "KinematicUnitObserverName",
+ "Name of the kinematic unit observer that should be used");
//HandUnits should only be changed via config file and default parameter should remain empty
- defineOptionalProperty<std::string>("HandUnits", "", "Name of the comma-seperated hand units that should be used. Unitname for left hand should be LeftHandUnit, and for right hand RightHandUnit");
- defineOptionalProperty<std::string>("HeadIKUnitName", "", "Name of the head unit that should be used.");
- defineOptionalProperty<std::string>("HeadIKKinematicChainName", "", "Name of the kinematic chain that should be used for head IK.");
- defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
-
+ defineOptionalProperty<std::string>(
+ "HandUnits",
+ "",
+ "Name of the comma-seperated hand units that should be used. Unitname for left "
+ "hand should be LeftHandUnit, and for right hand RightHandUnit");
+ defineOptionalProperty<std::string>(
+ "HeadIKUnitName", "", "Name of the head unit that should be used.");
+ defineOptionalProperty<std::string>(
+ "HeadIKKinematicChainName",
+ "",
+ "Name of the kinematic chain that should be used for head IK.");
+ defineOptionalProperty<std::string>(
+ "RobotStateComponentName",
+ "RobotStateComponent",
+ "Name of the RobotStateComponent that should be used");
}
};
@@ -63,15 +76,16 @@ namespace armarx
* @class HumanoidRobotStatechartContext is the implementation of the StatechartContext
* for a HumanoidRobot
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT RobotStatechartContext :
- virtual public StatechartContext
+ class ARMARXCOMPONENT_IMPORT_EXPORT RobotStatechartContext : virtual public StatechartContext
{
public:
// inherited from Component
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotStatechartContext";
}
+
void onInitStatechartContext() override;
void onConnectStatechartContext() override;
@@ -85,7 +99,8 @@ namespace armarx
*/
PropertyDefinitionsPtr createPropertyDefinitions() override;
- std::string getKinematicUnitObserverName()
+ std::string
+ getKinematicUnitObserverName()
{
return kinematicUnitObserverName;
}
@@ -107,5 +122,4 @@ namespace armarx
std::string kinematicUnitObserverName;
std::string headIKUnitName;
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/SimpleGridReachability.cpp b/source/RobotAPI/libraries/core/SimpleGridReachability.cpp
index c8b8ec74ed12854692a885ca3684a2fa04d1ffc0..a11a70aff132a393b6b714d15a376526d7453f1f 100644
--- a/source/RobotAPI/libraries/core/SimpleGridReachability.cpp
+++ b/source/RobotAPI/libraries/core/SimpleGridReachability.cpp
@@ -1,9 +1,12 @@
-#include "CartesianVelocityController.h"
#include "SimpleGridReachability.h"
+#include "CartesianVelocityController.h"
+
namespace armarx
{
- SimpleGridReachability::Result SimpleGridReachability::calculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params)
+ SimpleGridReachability::Result
+ SimpleGridReachability::calculateDiffIK(const Eigen::Matrix4f targetPose,
+ const Parameters& params)
{
VirtualRobot::RobotNodePtr rns = params.nodeSet;
VirtualRobot::RobotNodeSetPtr tcp = params.tcp ? params.tcp : rns->getTCP();
@@ -20,11 +23,14 @@ namespace armarx
Eigen::VectorXf cartesialVel(6);
cartesialVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
- Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
- Eigen::VectorXf jv = velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
+ Eigen::VectorXf jnv =
+ params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+ Eigen::VectorXf jv =
+ velocityController.calculate(cartesialVel, jnv, VirtualRobot::IKSolver::All);
- float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ float stepLength =
+ i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
jv = jv * stepLength;
for (size_t n = 0; n < rns->getSize(); n++)
@@ -37,8 +43,9 @@ namespace armarx
result.jointValues = rns->getJointValuesEigen();
result.posError = positionController.getPositionDiff(targetPose);
result.oriError = positionController.getOrientationError(targetPose);
- result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
+ result.reached =
+ result.posError < params.maxPosError && result.oriError < params.maxOriError;
return result;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/SimpleGridReachability.h b/source/RobotAPI/libraries/core/SimpleGridReachability.h
index 4ac9bc70f16524bbf86cbba53efc5f034a5c4f96..d8bbf8a0846aa317b36f100800940cf9da76076d 100644
--- a/source/RobotAPI/libraries/core/SimpleGridReachability.h
+++ b/source/RobotAPI/libraries/core/SimpleGridReachability.h
@@ -20,6 +20,7 @@ namespace armarx
float maxOriError = 0.05f;
float jointLimitAvoidanceKp = 2.0f;
};
+
struct Result
{
Eigen::VectorXf jointValues;
@@ -29,9 +30,6 @@ namespace armarx
};
static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, const Parameters& params);
-
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index a1bb8fe9b2c75cfb479d1290256a9e70e1793f4a..7d34f659e097e4eb6583023d2ccaf09808e7b735 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -22,30 +22,33 @@
* GNU General Public License
*/
#include "Trajectory.h"
+
+#include <cmath>
+#include <memory>
+
+#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
+
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+
#include "TrajectoryController.h"
#include "VectorHelpers.h"
-#include <ArmarXCore/observers/AbstractObjectSerializer.h>
#include "math/MathUtils.h"
-#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
-
-#include <memory>
-#include <cmath>
namespace armarx
{
- Trajectory::~Trajectory()
- = default;
+ Trajectory::~Trajectory() = default;
- void Trajectory::ice_preMarshal()
+ void
+ Trajectory::ice_preMarshal()
{
dataVec.clear();
timestamps.clear();
dataVec.reserve(dim());
for (const auto& it : *this)
{
- std::vector< DoubleSeqPtr >& data = it.data;
+ std::vector<DoubleSeqPtr>& data = it.data;
DoubleSeqSeq new2DVec;
new2DVec.reserve(data.size());
for (DoubleSeqPtr& subVec : data)
@@ -64,7 +67,8 @@ namespace armarx
timestamps = getTimestamps();
}
- void Trajectory::ice_postUnmarshal()
+ void
+ Trajectory::ice_postUnmarshal()
{
int i = 0;
dataMap.clear();
@@ -82,7 +86,8 @@ namespace armarx
}
}
- void Trajectory::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+ void
+ Trajectory::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
{
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
Ice::StringSeq columnContent;
@@ -105,7 +110,8 @@ namespace armarx
obj->setStringArray("dimensionNames", dimensionNames);
}
- void Trajectory::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
+ void
+ Trajectory::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
{
clear();
AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
@@ -128,19 +134,22 @@ namespace armarx
obj->getStringArray("dimensionNames", dimensionNames);
}
- VariantDataClassPtr Trajectory::clone(const Ice::Current&) const
+ VariantDataClassPtr
+ Trajectory::clone(const Ice::Current&) const
{
return new Trajectory(*this);
}
- TrajectoryPtr Trajectory::cloneMetaData() const
+ TrajectoryPtr
+ Trajectory::cloneMetaData() const
{
TrajectoryPtr t = new Trajectory();
CopyMetaData(*this, *t);
return t;
}
- std::string Trajectory::output(const Ice::Current&) const
+ std::string
+ Trajectory::output(const Ice::Current&) const
{
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -149,7 +158,8 @@ namespace armarx
s << "Dimensions names: \n";
for (size_t i = 0; i < dimensionNames.size(); i++)
{
- s << dimensionNames.at(i) << (i < limitless.size() && limitless.at(i).enabled ? " Limitless " : "") << "\n";
+ s << dimensionNames.at(i)
+ << (i < limitless.size() && limitless.at(i).enabled ? " Limitless " : "") << "\n";
}
for (; itMap != ordv.end(); itMap++)
@@ -159,17 +169,20 @@ namespace armarx
return s.str();
}
- Ice::Int Trajectory::getType(const Ice::Current&) const
+ Ice::Int
+ Trajectory::getType(const Ice::Current&) const
{
return VariantType::Trajectory;
}
- bool Trajectory::validate(const Ice::Current&)
+ bool
+ Trajectory::validate(const Ice::Current&)
{
return true;
}
- Ice::ObjectPtr Trajectory::ice_clone() const
+ Ice::ObjectPtr
+ Trajectory::ice_clone() const
{
return clone();
}
@@ -183,16 +196,14 @@ namespace armarx
CopyData(source, *this);
}
-
-
- Trajectory::Trajectory(const std::map<double, Ice::DoubleSeq >& data)
+ Trajectory::Trajectory(const std::map<double, Ice::DoubleSeq>& data)
{
if (data.size() == 0)
{
return;
}
- typename std::map<double, Ice::DoubleSeq >::const_iterator it = data.begin();
+ typename std::map<double, Ice::DoubleSeq>::const_iterator it = data.begin();
for (; it != data.end(); it++)
{
@@ -215,14 +226,15 @@ namespace armarx
// copyMap(map, dataMap);
// }
- Trajectory& Trajectory::operator=(const Trajectory& source)
+ Trajectory&
+ Trajectory::operator=(const Trajectory& source)
{
CopyData(source, *this);
return *this;
}
-
- double Trajectory::getState(double t, size_t dim, size_t derivation)
+ double
+ Trajectory::getState(double t, size_t dim, size_t derivation)
{
if (dim >= this->dim())
{
@@ -231,10 +243,11 @@ namespace armarx
typename timestamp_view::iterator it = dataMap.find(t);
- if (it == dataMap.end() || dim >= it->data.size() || !it->data.at(dim) || it->data.at(dim)->size() <= derivation)
+ if (it == dataMap.end() || dim >= it->data.size() || !it->data.at(dim) ||
+ it->data.at(dim)->size() <= derivation)
{
// ARMARX_INFO << "interpolating for " << VAROUT(t) << VAROUT(dim);
- __fillBaseDataAtTimestamp(t);// interpolates and retrieves
+ __fillBaseDataAtTimestamp(t); // interpolates and retrieves
it = dataMap.find(t);
}
@@ -242,17 +255,17 @@ namespace armarx
{
// ARMARX_ERROR << "FAILED!";
// ARMARX_INFO << VAROUT(t) << VAROUT(dim) << VAROUT(it->data.size()) << this->output();
- throw LocalException() << "std::vector ptr is not the correct size!? " << VAROUT(dim) << VAROUT(it->data.size());
+ throw LocalException() << "std::vector ptr is not the correct size!? " << VAROUT(dim)
+ << VAROUT(it->data.size());
}
if (!it->data.at(dim))
- // it->data.at(dim).reset(new Ice::DoubleSeq());
+ // it->data.at(dim).reset(new Ice::DoubleSeq());
{
throw LocalException() << "std::vector ptr is NULL!?";
}
-
std::vector<DoubleSeqPtr>& vec = it->data;
ARMARX_CHECK_GREATER(vec.size(), dim);
if (derivation != 0 && vec.at(dim)->size() <= derivation)
@@ -272,17 +285,18 @@ namespace armarx
vec.at(dim)->at(curDeriv) = derivValue;
curDeriv++;
}
-
}
- ARMARX_CHECK_GREATER(vec.at(dim)->size(), derivation) << VAROUT(t) << VAROUT(dim) << VAROUT(*this);
+ ARMARX_CHECK_GREATER(vec.at(dim)->size(), derivation)
+ << VAROUT(t) << VAROUT(dim) << VAROUT(*this);
// std::cout << "dimensions: " <<it->data.size() << " derivations: " << it->data.at(dim)->size() << std::endl;
double result = vec.at(dim)->at(derivation);
// checkValue(result);
return result;
}
- double Trajectory::getState(double t, size_t dim, size_t derivation) const
+ double
+ Trajectory::getState(double t, size_t dim, size_t derivation) const
{
if (dim >= this->dim())
@@ -315,7 +329,8 @@ namespace armarx
}
}
- std::vector<Ice::DoubleSeq> Trajectory::getAllStates(double t, int maxDeriv)
+ std::vector<Ice::DoubleSeq>
+ Trajectory::getAllStates(double t, int maxDeriv)
{
std::vector<Ice::DoubleSeq> res;
@@ -334,11 +349,13 @@ namespace armarx
return res;
}
- Ice::DoubleSeq Trajectory::getDerivations(double t, size_t dimension, size_t derivations) const
+ Ice::DoubleSeq
+ Trajectory::getDerivations(double t, size_t dimension, size_t derivations) const
{
if (dimension >= dim())
{
- throw LocalException() << "Dimension out of bounds: requested: " << dimension << " available: " << dim();
+ throw LocalException()
+ << "Dimension out of bounds: requested: " << dimension << " available: " << dim();
}
Ice::DoubleSeq result;
@@ -351,17 +368,20 @@ namespace armarx
return result;
}
- std::string Trajectory::getDimensionName(size_t dim) const
+ std::string
+ Trajectory::getDimensionName(size_t dim) const
{
return dimensionNames.at(dim);
}
- const Ice::StringSeq& Trajectory::getDimensionNames() const
+ const Ice::StringSeq&
+ Trajectory::getDimensionNames() const
{
return dimensionNames;
}
- Trajectory::TrajDataContainer& Trajectory::data()
+ Trajectory::TrajDataContainer&
+ Trajectory::data()
{
return dataMap;
}
@@ -371,7 +391,8 @@ namespace armarx
{
if (dimension >= dim())
{
- throw LocalException("dimension is out of range: ") << dimension << " actual dimensions: " << dim();
+ throw LocalException("dimension is out of range: ")
+ << dimension << " actual dimensions: " << dim();
}
Ice::DoubleSeq result;
@@ -389,19 +410,25 @@ namespace armarx
}
return result;
-
}
- Eigen::VectorXd Trajectory::getDimensionDataAsEigen(size_t dimension, size_t derivation) const
+ Eigen::VectorXd
+ Trajectory::getDimensionDataAsEigen(size_t dimension, size_t derivation) const
{
- return getDimensionDataAsEigen(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
+ return getDimensionDataAsEigen(
+ dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
- Eigen::VectorXd Trajectory::getDimensionDataAsEigen(size_t dimension, size_t derivation, double startTime, double endTime) const
+ Eigen::VectorXd
+ Trajectory::getDimensionDataAsEigen(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const
{
if (dimension >= dim())
{
- throw LocalException("dimension is out of range: ") << dimension << " actual dimensions: " << dim();
+ throw LocalException("dimension is out of range: ")
+ << dimension << " actual dimensions: " << dim();
}
Ice::DoubleSeq result;
@@ -428,7 +455,8 @@ namespace armarx
return resultVec;
}
- Eigen::MatrixXd Trajectory::toEigen(size_t derivation, double startTime, double endTime) const
+ Eigen::MatrixXd
+ Trajectory::toEigen(size_t derivation, double startTime, double endTime) const
{
Eigen::MatrixXd result(size(), dim());
@@ -450,7 +478,8 @@ namespace armarx
return result;
}
- Eigen::MatrixXd Trajectory::toEigen(size_t derivation) const
+ Eigen::MatrixXd
+ Trajectory::toEigen(size_t derivation) const
{
if (size() == 0 || dim() == 0)
{
@@ -459,7 +488,8 @@ namespace armarx
return toEigen(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
- TrajectoryPtr Trajectory::getPart(double startTime, double endTime, size_t numberOfDerivations) const
+ TrajectoryPtr
+ Trajectory::getPart(double startTime, double endTime, size_t numberOfDerivations) const
{
TrajectoryPtr result(new Trajectory());
@@ -488,7 +518,8 @@ namespace armarx
return result;
}
- size_t Trajectory::dim() const
+ size_t
+ Trajectory::dim() const
{
if (dataMap.size() == 0)
{
@@ -500,14 +531,14 @@ namespace armarx
}
}
- size_t Trajectory::size() const
+ size_t
+ Trajectory::size() const
{
return dataMap.size();
}
-
-
- double Trajectory::getTimeLength() const
+ double
+ Trajectory::getTimeLength() const
{
const ordered_view& ordView = dataMap.get<TagOrdered>();
@@ -521,7 +552,8 @@ namespace armarx
}
}
- double Trajectory::getLength(size_t derivation) const
+ double
+ Trajectory::getLength(size_t derivation) const
{
if (size() == 0)
{
@@ -530,7 +562,8 @@ namespace armarx
return getLength(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
- double Trajectory::getLength(size_t derivation, double startTime, double endTime) const
+ double
+ Trajectory::getLength(size_t derivation, double startTime, double endTime) const
{
double length = 0.0;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -550,10 +583,7 @@ namespace armarx
itO++;
double segmentLength = 0;
- for (;
- itO != itOEnd && itO != ordv.end();
- itO++
- )
+ for (; itO != itOEnd && itO != ordv.end(); itO++)
{
if (itO->getTimestamp() >= endTime)
{
@@ -569,7 +599,6 @@ namespace armarx
prevValue[d] = value;
}
length += sqrt(segmentLength);
-
}
segmentLength = 0;
// interpolate end values
@@ -584,7 +613,8 @@ namespace armarx
return length;
}
- double Trajectory::getLength(size_t dimension, size_t derivation) const
+ double
+ Trajectory::getLength(size_t dimension, size_t derivation) const
{
if (size() == 0)
{
@@ -593,7 +623,11 @@ namespace armarx
return getLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
- double Trajectory::getLength(size_t dimension, size_t derivation, double startTime, double endTime) const
+ double
+ Trajectory::getLength(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const
{
double length = 0.0;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -605,10 +639,7 @@ namespace armarx
}
double prevValue = getState(startTime, dimension, derivation);
- for (;
- itO != itOEnd && itO != ordv.end();
- itO++
- )
+ for (; itO != itOEnd && itO != ordv.end(); itO++)
{
if (itO->getTimestamp() >= endTime)
{
@@ -621,12 +652,18 @@ namespace armarx
return length;
}
- double Trajectory::getSquaredLength(size_t dimension, size_t derivation) const
+ double
+ Trajectory::getSquaredLength(size_t dimension, size_t derivation) const
{
- return getSquaredLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
+ return getSquaredLength(
+ dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
- double Trajectory::getSquaredLength(size_t dimension, size_t derivation, double startTime, double endTime) const
+ double
+ Trajectory::getSquaredLength(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const
{
double length = 0.0;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -639,10 +676,7 @@ namespace armarx
double prevValue = itO->getDeriv(dimension, derivation);
- for (;
- itO != itOEnd;
- itO++
- )
+ for (; itO != itOEnd; itO++)
{
length += fabs(pow(prevValue, 2.0) - pow(itO->getDeriv(dimension, derivation), 2.0));
prevValue = itO->getDeriv(dimension, derivation);
@@ -651,17 +685,35 @@ namespace armarx
return length;
}
- double Trajectory::getMax(size_t dimension, size_t derivation, double startTime, double endTime) const
+ double
+ Trajectory::getMax(size_t dimension, size_t derivation, double startTime, double endTime) const
{
- return getWithFunc(&std::max<double>, -std::numeric_limits<double>::max(), dimension, derivation, startTime, endTime);
+ return getWithFunc(&std::max<double>,
+ -std::numeric_limits<double>::max(),
+ dimension,
+ derivation,
+ startTime,
+ endTime);
}
- double Trajectory::getMin(size_t dimension, size_t derivation, double startTime, double endTime) const
+ double
+ Trajectory::getMin(size_t dimension, size_t derivation, double startTime, double endTime) const
{
- return getWithFunc(&std::min<double>, std::numeric_limits<double>::max(), dimension, derivation, startTime, endTime);
+ return getWithFunc(&std::min<double>,
+ std::numeric_limits<double>::max(),
+ dimension,
+ derivation,
+ startTime,
+ endTime);
}
- double Trajectory::getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t derivation, double startTime, double endTime) const
+ double
+ Trajectory::getWithFunc(const double& (*foo)(const double&, const double&),
+ double initValue,
+ size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const
{
double bestValue = initValue;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -672,10 +724,7 @@ namespace armarx
{
return bestValue;
}
- for (;
- itO != itOEnd;
- itO++
- )
+ for (; itO != itOEnd; itO++)
{
bestValue = foo(bestValue, itO->getDeriv(dimension, derivation));
}
@@ -683,12 +732,21 @@ namespace armarx
return bestValue;
}
- double Trajectory::getAmplitude(size_t dimension, size_t derivation, double startTime, double endTime) const
+ double
+ Trajectory::getAmplitude(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const
{
- return getMax(dimension, derivation, startTime, endTime) - getMin(dimension, derivation, startTime, endTime);
+ return getMax(dimension, derivation, startTime, endTime) -
+ getMin(dimension, derivation, startTime, endTime);
}
- Ice::DoubleSeq Trajectory::getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const
+ Ice::DoubleSeq
+ Trajectory::getMinima(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const
{
Ice::DoubleSeq result;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -700,10 +758,9 @@ namespace armarx
return result;
}
double preValue = itO->getDeriv(dimension, derivation);
- for (;
- itO != itOEnd;
+ for (; itO != itOEnd;
- )
+ )
{
// double t = itO->getTimestamp();
double cur = itO->getDeriv(dimension, derivation);
@@ -723,7 +780,11 @@ namespace armarx
return result;
}
- Ice::DoubleSeq Trajectory::getMinimaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const
+ Ice::DoubleSeq
+ Trajectory::getMinimaTimestamps(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const
{
Ice::DoubleSeq result;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -735,10 +796,9 @@ namespace armarx
return result;
}
double preValue = itO->getDeriv(dimension, derivation);
- for (;
- itO != itOEnd;
+ for (; itO != itOEnd;
- )
+ )
{
// double t = itO->getTimestamp();
double cur = itO->getDeriv(dimension, derivation);
@@ -758,7 +818,11 @@ namespace armarx
return result;
}
- Ice::DoubleSeq Trajectory::getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const
+ Ice::DoubleSeq
+ Trajectory::getMaxima(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const
{
Ice::DoubleSeq result;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -770,10 +834,9 @@ namespace armarx
return result;
}
double preValue = itO->getDeriv(dimension, derivation);
- for (;
- itO != itOEnd;
+ for (; itO != itOEnd;
- )
+ )
{
double cur = itO->getDeriv(dimension, derivation);
itO++;
@@ -792,7 +855,11 @@ namespace armarx
return result;
}
- Ice::DoubleSeq Trajectory::getMaximaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const
+ Ice::DoubleSeq
+ Trajectory::getMaximaTimestamps(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const
{
Ice::DoubleSeq result;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -804,10 +871,9 @@ namespace armarx
return result;
}
double preValue = itO->getDeriv(dimension, derivation);
- for (;
- itO != itOEnd;
+ for (; itO != itOEnd;
- )
+ )
{
double cur = itO->getDeriv(dimension, derivation);
itO++;
@@ -827,8 +893,8 @@ namespace armarx
return result;
}
-
- std::vector<DoubleSeqPtr> Trajectory::__calcBaseDataAtTimestamp(const double& t) const
+ std::vector<DoubleSeqPtr>
+ Trajectory::__calcBaseDataAtTimestamp(const double& t) const
{
// ARMARX_INFO << "calcBaseDataAtTimestamp for " << t;
// typename timestamp_view::const_iterator it = dataMap.find(t);
@@ -844,14 +910,15 @@ namespace armarx
}
return result;
-
}
- bool Trajectory::dataExists(double t, size_t dimension, size_t derivation) const
+ bool
+ Trajectory::dataExists(double t, size_t dimension, size_t derivation) const
{
typename timestamp_view::iterator it = dataMap.find(t);
- if (it == dataMap.end() || !it->data.at(dimension) || it->data.at(dimension)->size() <= derivation)
+ if (it == dataMap.end() || !it->data.at(dimension) ||
+ it->data.at(dimension)->size() <= derivation)
{
return false;
}
@@ -861,8 +928,8 @@ namespace armarx
}
}
-
- Trajectory::timestamp_view::iterator Trajectory::__fillBaseDataAtTimestamp(const double& t)
+ Trajectory::timestamp_view::iterator
+ Trajectory::__fillBaseDataAtTimestamp(const double& t)
{
typename timestamp_view::const_iterator it = dataMap.find(t);
@@ -900,7 +967,8 @@ namespace armarx
return it;
}
- std::vector<DoubleSeqPtr>& Trajectory::getStates(double t)
+ std::vector<DoubleSeqPtr>&
+ Trajectory::getStates(double t)
{
typename timestamp_view::const_iterator it = dataMap.find(t);
@@ -914,7 +982,8 @@ namespace armarx
return it->data;
}
- std::vector<DoubleSeqPtr> Trajectory::getStates(double t) const
+ std::vector<DoubleSeqPtr>
+ Trajectory::getStates(double t) const
{
typename timestamp_view::const_iterator it = dataMap.find(t);
@@ -928,9 +997,8 @@ namespace armarx
return it->data;
}
-
-
- Ice::DoubleSeq Trajectory::getStates(double t, size_t derivation) const
+ Ice::DoubleSeq
+ Trajectory::getStates(double t, size_t derivation) const
{
size_t dimensions = dim();
Ice::DoubleSeq result;
@@ -943,7 +1011,8 @@ namespace armarx
return result;
}
- std::map<double, Ice::DoubleSeq> Trajectory::getStatesAround(double t, size_t derivation, size_t extend) const
+ std::map<double, Ice::DoubleSeq>
+ Trajectory::getStatesAround(double t, size_t derivation, size_t extend) const
{
std::map<double, Ice::DoubleSeq> res;
@@ -983,8 +1052,8 @@ namespace armarx
if (itPrev == ordv.begin() || itNext == ordv.end())
{
- std::cout << "Warning: the timestamp is out of the range. " <<
- "The current result will be returned" << std::endl;
+ std::cout << "Warning: the timestamp is out of the range. "
+ << "The current result will be returned" << std::endl;
break;
}
itPrev--;
@@ -992,11 +1061,10 @@ namespace armarx
}
return res;
-
}
-
- Trajectory& Trajectory::operator +=(const Trajectory traj)
+ Trajectory&
+ Trajectory::operator+=(const Trajectory traj)
{
size_t dims = traj.dim();
Ice::DoubleSeq timestamps = traj.getTimestamps();
@@ -1009,8 +1077,8 @@ namespace armarx
return *this;
}
-
- void Trajectory::__addDimension()
+ void
+ Trajectory::__addDimension()
{
size_t newDim = dim() + 1;
@@ -1022,7 +1090,8 @@ namespace armarx
}
}
- void Trajectory::setEntries(const double t, const size_t dimIndex, const Ice::DoubleSeq& y)
+ void
+ Trajectory::setEntries(const double t, const size_t dimIndex, const Ice::DoubleSeq& y)
{
typename timestamp_view::iterator itMap = dataMap.find(t);
@@ -1031,7 +1100,7 @@ namespace armarx
// double dura = getTimeLength();
TrajData newData(this);
newData.timestamp = t;
- newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
+ newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
newData.data[dimIndex] = std::make_shared<Ice::DoubleSeq>(y);
dataMap.insert(std::move(newData));
}
@@ -1048,11 +1117,10 @@ namespace armarx
ARMARX_CHECK_GREATER(itMap->data.size(), dimIndex);
itMap->data.at(dimIndex) = std::make_shared<Ice::DoubleSeq>(y);
}
-
}
-
- void Trajectory::setPositionEntry(const double t, const size_t dimIndex, const double& y)
+ void
+ Trajectory::setPositionEntry(const double t, const size_t dimIndex, const double& y)
{
typename timestamp_view::iterator itMap = dataMap.find(t);
@@ -1060,7 +1128,7 @@ namespace armarx
{
TrajData newData(this);
newData.timestamp = t;
- newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
+ newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
newData.data[dimIndex] = std::make_shared<Ice::DoubleSeq>(1, y);
dataMap.insert(newData);
}
@@ -1076,11 +1144,10 @@ namespace armarx
itMap->data.resize(dim());
itMap->data.at(dimIndex) = std::make_shared<Ice::DoubleSeq>(1, y);
}
-
}
-
- void Trajectory::__fillAllEmptyFields()
+ void
+ Trajectory::__fillAllEmptyFields()
{
const ordered_view& ordv = dataMap.get<TagOrdered>();
typename ordered_view::const_iterator itMap = ordv.begin();
@@ -1091,16 +1158,15 @@ namespace armarx
{
if (!itMap->data[dimension])
{
- itMap->data[dimension] = std::make_shared<Ice::DoubleSeq>(__interpolate(itMap, dimension, 0));
+ itMap->data[dimension] =
+ std::make_shared<Ice::DoubleSeq>(__interpolate(itMap, dimension, 0));
}
}
}
}
-
-
-
- Ice::DoubleSeq Trajectory::getTimestamps() const
+ Ice::DoubleSeq
+ Trajectory::getTimestamps() const
{
Ice::DoubleSeq result;
result.reserve(size());
@@ -1115,7 +1181,8 @@ namespace armarx
return result;
}
- Ice::FloatSeq Trajectory::getTimestampsFloat() const
+ Ice::FloatSeq
+ Trajectory::getTimestampsFloat() const
{
Ice::FloatSeq result;
result.reserve(size());
@@ -1130,14 +1197,13 @@ namespace armarx
return result;
}
-
-
Ice::DoubleSeq
Trajectory::getDiscreteDifferentiationForDim(size_t trajDimension, size_t derivation) const
{
if (trajDimension >= dim())
{
- throw LocalException("dimension is out of range: ") << trajDimension << " actual dimensions: " << dim();
+ throw LocalException("dimension is out of range: ")
+ << trajDimension << " actual dimensions: " << dim();
}
@@ -1175,14 +1241,15 @@ namespace armarx
// first step-> reestablish that current is > prev
itPrev--;
}
-
}
return result;
}
-
- Ice::DoubleSeq Trajectory::DifferentiateDiscretly(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& values, int derivationCount)
+ Ice::DoubleSeq
+ Trajectory::DifferentiateDiscretly(const Ice::DoubleSeq& timestamps,
+ const Ice::DoubleSeq& values,
+ int derivationCount)
{
if (derivationCount < 0)
{
@@ -1205,13 +1272,15 @@ namespace armarx
result.resize(size);
// boundaries
- result[0] = (values.at(1) - values.at(0)) / (timestamps.at(1) - timestamps.at(0)) ;
- result[size - 1] = (values.at(size - 1) - values.at(size - 2)) / (timestamps.at(size - 1) - timestamps.at(size - 2)) ;
+ result[0] = (values.at(1) - values.at(0)) / (timestamps.at(1) - timestamps.at(0));
+ result[size - 1] = (values.at(size - 1) - values.at(size - 2)) /
+ (timestamps.at(size - 1) - timestamps.at(size - 2));
//#pragma omp parallel for
for (int i = 1; i < size - 1; ++i)
{
- result[i] = (values.at(i + 1) - values.at(i - 1)) / (timestamps.at(i + 1) - timestamps.at(i - 1)) ;
+ result[i] = (values.at(i + 1) - values.at(i - 1)) /
+ (timestamps.at(i + 1) - timestamps.at(i - 1));
checkValue(result[i]);
}
@@ -1221,11 +1290,12 @@ namespace armarx
}
return result;
-
}
-
- double Trajectory::getDiscretDifferentiationForDimAtT(double t, size_t trajDimension, size_t derivation) const
+ double
+ Trajectory::getDiscretDifferentiationForDimAtT(double t,
+ size_t trajDimension,
+ size_t derivation) const
{
if (derivation == 0)
{
@@ -1246,7 +1316,7 @@ namespace armarx
if (it != dataMap.end() && itNext == ordV.end())
{
- itNext = itCurrent; // current item is last, set next to current
+ itNext = itCurrent; // current item is last, set next to current
}
else if (itNext == ordV.end() && it == dataMap.end())
{
@@ -1258,7 +1328,7 @@ namespace armarx
if (itCurrent != ordV.end()) // check if current item exists
{
- itPrev--; //element in front of t
+ itPrev--; //element in front of t
}
if (itPrev == ordV.end())
@@ -1266,7 +1336,7 @@ namespace armarx
//element in front of t does not exist
if (itCurrent != ordV.end())
{
- itPrev = itCurrent; // set prev element to current
+ itPrev = itCurrent; // set prev element to current
}
else
{
@@ -1279,7 +1349,7 @@ namespace armarx
//element after t does not exist
if (itCurrent != ordV.end())
{
- itNext = itCurrent; // set next element to current
+ itNext = itCurrent; // set next element to current
}
else
{
@@ -1289,7 +1359,10 @@ namespace armarx
if (itNext == itPrev)
{
- throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << " " << VAROUT(size());
+ throw LocalException()
+ << "Interpolation failed: the next data and the previous are missing.\nInfo:\n"
+ << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << " "
+ << VAROUT(size());
}
// double diff = itNext->data[trajDimension]->at(derivation-1) - itPrev->data[trajDimension]->at(derivation-1);
@@ -1321,7 +1394,10 @@ namespace armarx
if (fabs(tNext - tBefore) < 1e-10)
{
- throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(tNext) << VAROUT(tBefore) << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << VAROUT(size()) << VAROUT(getTimestamps());
+ throw LocalException()
+ << "Interpolation failed: the next data and the previous are missing.\nInfo:\n"
+ << VAROUT(tNext) << VAROUT(tBefore) << VAROUT(t) << VAROUT(trajDimension)
+ << (getDimensionName(trajDimension)) << VAROUT(size()) << VAROUT(getTimestamps());
}
double duration = tNext - tBefore;
@@ -1329,7 +1405,8 @@ namespace armarx
double delta;
if (trajDimension < limitless.size() && limitless.at(trajDimension).enabled)
{
- double range = limitless.at(trajDimension).limitHi - limitless.at(trajDimension).limitLo;
+ double range =
+ limitless.at(trajDimension).limitHi - limitless.at(trajDimension).limitLo;
double dist1 = next - before;
double dist2 = next - (before + range);
@@ -1368,7 +1445,8 @@ namespace armarx
return delta;
}
- void Trajectory::differentiateDiscretly(size_t derivation)
+ void
+ Trajectory::differentiateDiscretly(size_t derivation)
{
for (size_t d = 0; d < dim(); d++)
@@ -1377,7 +1455,8 @@ namespace armarx
}
}
- void Trajectory::differentiateDiscretlyForDim(size_t trajDimension, size_t derivation)
+ void
+ Trajectory::differentiateDiscretlyForDim(size_t trajDimension, size_t derivation)
{
removeDerivation(trajDimension, derivation);
typename ordered_view::iterator itOrd = begin();
@@ -1388,8 +1467,11 @@ namespace armarx
}
}
-
- void Trajectory::reconstructFromDerivativeForDim(double valueAtFirstTimestamp, size_t trajDimension, size_t sourceDimOfSystemState, size_t targetDimOfSystemState)
+ void
+ Trajectory::reconstructFromDerivativeForDim(double valueAtFirstTimestamp,
+ size_t trajDimension,
+ size_t sourceDimOfSystemState,
+ size_t targetDimOfSystemState)
{
const ordered_view& ordv = dataMap.get<TagOrdered>();
typename ordered_view::iterator it = ordv.begin();
@@ -1421,15 +1503,15 @@ namespace armarx
previousTimestamp = it->timestamp;
previousValue = it->data[trajDimension]->at(targetDimOfSystemState);
}
-
}
-
- void Trajectory::negateDim(size_t trajDimension)
+ void
+ Trajectory::negateDim(size_t trajDimension)
{
if (trajDimension >= dim())
{
- throw LocalException("dimension is out of range: ") << trajDimension << " actual dimensions: " << dim();
+ throw LocalException("dimension is out of range: ")
+ << trajDimension << " actual dimensions: " << dim();
}
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -1442,16 +1524,13 @@ namespace armarx
}
}
-
-
-
-
double
Trajectory::__interpolate(double t, size_t dimension, size_t derivation) const
{
typename timestamp_view::const_iterator it = dataMap.find(t);
- if (it != dataMap.end() && it->data.size() > dimension && it->data.at(dimension) && it->data.at(dimension)->size() > derivation)
+ if (it != dataMap.end() && it->data.size() > dimension && it->data.at(dimension) &&
+ it->data.at(dimension)->size() > derivation)
{
return it->data.at(dimension)->at(derivation);
}
@@ -1472,7 +1551,9 @@ namespace armarx
}
double
- Trajectory::__interpolate(typename ordered_view::const_iterator itMap, size_t dimension, size_t derivation) const
+ Trajectory::__interpolate(typename ordered_view::const_iterator itMap,
+ size_t dimension,
+ size_t derivation) const
{
typename ordered_view::iterator itPrev = itMap;
itPrev--;
@@ -1482,14 +1563,19 @@ namespace armarx
}
double
- Trajectory::__interpolate(double t, typename ordered_view::const_iterator itPrev, typename ordered_view::const_iterator itNext, size_t dimension, size_t derivation) const
+ Trajectory::__interpolate(double t,
+ typename ordered_view::const_iterator itPrev,
+ typename ordered_view::const_iterator itNext,
+ size_t dimension,
+ size_t derivation) const
{
const ordered_view& ordView = dataMap.get<TagOrdered>();
double previous = 0;
double next = 0;
// find previous SystemState that exists for that dimension
- while (itPrev != ordView.end() && (itPrev->data.at(dimension) == nullptr || itPrev->data.at(dimension)->size() <= derivation))
+ while (itPrev != ordView.end() && (itPrev->data.at(dimension) == nullptr ||
+ itPrev->data.at(dimension)->size() <= derivation))
{
itPrev--;
}
@@ -1497,12 +1583,14 @@ namespace armarx
if (itPrev != ordView.end())
{
// ARMARX_INFO << "Found prev state at " << itPrev->timestamp;
- ARMARX_CHECK_NOT_EQUAL(t, itPrev->timestamp) << VAROUT(t) << VAROUT(itPrev->timestamp) << VAROUT(*this);
+ ARMARX_CHECK_NOT_EQUAL(t, itPrev->timestamp)
+ << VAROUT(t) << VAROUT(itPrev->timestamp) << VAROUT(*this);
previous = getState(itPrev->timestamp, dimension, derivation);
}
// find next SystemState that exists for that dimension
- while (itNext != ordView.end() && (!itNext->data.at(dimension) || itNext->data.at(dimension)->size() <= derivation))
+ while (itNext != ordView.end() &&
+ (!itNext->data.at(dimension) || itNext->data.at(dimension)->size() <= derivation))
{
itNext++;
}
@@ -1515,21 +1603,23 @@ namespace armarx
}
-
if (itNext == ordView.end() && itPrev == ordView.end())
{
- throw LocalException() << "Cannot find next or prev values in dim " << dimension << " at timestamp " << t;
+ throw LocalException()
+ << "Cannot find next or prev values in dim " << dimension << " at timestamp " << t;
}
if (itNext == ordView.end())
{
// ARMARX_INFO << "Extrapolating to the right from " << itPrev->timestamp;
- return getState(itPrev->timestamp, dimension, derivation) + getState(itPrev->timestamp, dimension, derivation + 1) * (t - itPrev->timestamp);
+ return getState(itPrev->timestamp, dimension, derivation) +
+ getState(itPrev->timestamp, dimension, derivation + 1) * (t - itPrev->timestamp);
}
else if (itPrev == ordView.end())
{
// ARMARX_INFO << "Extrapolating to the left from " << itNext->timestamp;
- return getState(itNext->timestamp, dimension, derivation) - getState(itNext->timestamp, dimension, derivation + 1) * (itNext->timestamp - t);
+ return getState(itNext->timestamp, dimension, derivation) -
+ getState(itNext->timestamp, dimension, derivation + 1) * (itNext->timestamp - t);
}
else
{
@@ -1548,7 +1638,8 @@ namespace armarx
}
}
- void Trajectory::gaussianFilter(double filterRadius)
+ void
+ Trajectory::gaussianFilter(double filterRadius)
{
const ordered_view& ordv = dataMap.get<TagOrdered>();
Trajectory filteredTraj;
@@ -1572,7 +1663,10 @@ namespace armarx
}
double
- Trajectory::__gaussianFilter(double filterRadiusInTime, typename ordered_view::iterator centerIt, size_t trajNum, size_t dim)
+ Trajectory::__gaussianFilter(double filterRadiusInTime,
+ typename ordered_view::iterator centerIt,
+ size_t trajNum,
+ size_t dim)
{
const ordered_view& ordView = dataMap.get<TagOrdered>();
const double sigma = filterRadiusInTime / 2.5;
@@ -1586,12 +1680,11 @@ namespace armarx
for (int sign = -1; sign < 2; sign += 2)
{
for (ordered_view::iterator it = start;
- it != ordView.end()
- && fabs(it->timestamp - centerIt->timestamp) <= fabs(filterRadiusInTime * 2);
- )
+ it != ordView.end() &&
+ fabs(it->timestamp - centerIt->timestamp) <= fabs(filterRadiusInTime * 2);)
{
double value;
- value = it->getDeriv(trajNum, dim);//data[trajNum]->at(dim);
+ value = it->getDeriv(trajNum, dim); //data[trajNum]->at(dim);
double diff = (it->timestamp - centerIt->timestamp);
double squared = diff * diff;
const double gaussValue = exp(-squared / (2 * sigma * sigma)) / (sigma * sqrt2PI);
@@ -1610,18 +1703,17 @@ namespace armarx
checkValue(weightedSum);
}
- start++;// skip center value in second loop iteration
+ start++; // skip center value in second loop iteration
}
double result;
result = weightedSum / sumOfWeight;
checkValue(result);
return result;
-
}
-
- void Trajectory::CopyData(const Trajectory& source, Trajectory& destination)
+ void
+ Trajectory::CopyData(const Trajectory& source, Trajectory& destination)
{
if (&source == &destination)
{
@@ -1634,21 +1726,21 @@ namespace armarx
for (size_t dim = 0; dim < source.dim(); dim++)
{
- destination.addDimension(source.getDimensionData(dim), timestamps
- );
+ destination.addDimension(source.getDimensionData(dim), timestamps);
}
CopyMetaData(source, destination);
}
- void Trajectory::CopyMetaData(const Trajectory& source, Trajectory& destination)
+ void
+ Trajectory::CopyMetaData(const Trajectory& source, Trajectory& destination)
{
destination.setDimensionNames(source.getDimensionNames());
destination.setLimitless(source.getLimitless());
}
-
- void Trajectory::clear(bool keepMetaData)
+ void
+ Trajectory::clear(bool keepMetaData)
{
dataMap.erase(dataMap.begin(), dataMap.end());
if (!keepMetaData)
@@ -1658,9 +1750,8 @@ namespace armarx
}
}
-
-
- Ice::DoubleSeq Trajectory::GenerateTimestamps(double startTime, double endTime, double stepSize)
+ Ice::DoubleSeq
+ Trajectory::GenerateTimestamps(double startTime, double endTime, double stepSize)
{
if (startTime >= endTime)
{
@@ -1681,12 +1772,15 @@ namespace armarx
currentTimestamp += stepSize;
i++;
}
- ARMARX_CHECK_EQUAL(result.size(), size) << VAROUT(startTime) << VAROUT(endTime) << VAROUT(stepSize);
+ ARMARX_CHECK_EQUAL(result.size(), size)
+ << VAROUT(startTime) << VAROUT(endTime) << VAROUT(stepSize);
return result;
}
-
- Ice::DoubleSeq Trajectory::NormalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime, const double endTime)
+ Ice::DoubleSeq
+ Trajectory::NormalizeTimestamps(const Ice::DoubleSeq& timestamps,
+ const double startTime,
+ const double endTime)
{
Ice::DoubleSeq normTimestamps;
normTimestamps.resize(timestamps.size());
@@ -1696,19 +1790,23 @@ namespace armarx
for (size_t i = 0; i < timestamps.size(); i++)
{
- normTimestamps[i] = startTime + (timestamps.at(i) - minValue) / duration * (endTime - startTime);
+ normTimestamps[i] =
+ startTime + (timestamps.at(i) - minValue) / duration * (endTime - startTime);
}
return normTimestamps;
}
-
- Trajectory Trajectory::NormalizeTimestamps(const Trajectory& traj, const double startTime, const double endTime)
+ Trajectory
+ Trajectory::NormalizeTimestamps(const Trajectory& traj,
+ const double startTime,
+ const double endTime)
{
- if (traj.size() <= 1 || (traj.begin()->timestamp == startTime && traj.rbegin()->timestamp == endTime))
+ if (traj.size() <= 1 ||
+ (traj.begin()->timestamp == startTime && traj.rbegin()->timestamp == endTime))
{
- return traj; // already normalized
+ return traj; // already normalized
}
@@ -1720,31 +1818,39 @@ namespace armarx
for (size_t dim = 0; dim < traj.dim(); dim++)
{
- Ice::DoubleSeq dimensionData = traj.getDimensionData(dim);
+ Ice::DoubleSeq dimensionData = traj.getDimensionData(dim);
normExampleTraj.addDimension(dimensionData, normTimestamps);
}
normExampleTraj.setDimensionNames(traj.getDimensionNames());
normExampleTraj.setLimitless(traj.getLimitless());
return normExampleTraj;
-
-
}
- TrajectoryPtr Trajectory::normalize(const double startTime, const double endTime)
+ TrajectoryPtr
+ Trajectory::normalize(const double startTime, const double endTime)
{
Trajectory normTraj = NormalizeTimestamps(*this, startTime, endTime);
TrajectoryPtr newTraj = new Trajectory(normTraj);
return newTraj;
}
- TrajectoryPtr Trajectory::calculateTimeOptimalTrajectory(double maxVelocity, double maxAcceleration, double maxDeviation, const IceUtil::Time& timestep)
+ TrajectoryPtr
+ Trajectory::calculateTimeOptimalTrajectory(double maxVelocity,
+ double maxAcceleration,
+ double maxDeviation,
+ const IceUtil::Time& timestep)
{
Eigen::VectorXd maxVelocities = Eigen::VectorXd::Constant(dim(), maxVelocity);
Eigen::VectorXd maxAccelerations = Eigen::VectorXd::Constant(dim(), maxAcceleration);
- return calculateTimeOptimalTrajectory(maxVelocities, maxAccelerations, maxDeviation, timestep);
+ return calculateTimeOptimalTrajectory(
+ maxVelocities, maxAccelerations, maxDeviation, timestep);
}
- TrajectoryPtr Trajectory::calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities, const Eigen::VectorXd& maxAccelerations, double maxDeviation, IceUtil::Time const& timestep)
+ TrajectoryPtr
+ Trajectory::calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities,
+ const Eigen::VectorXd& maxAccelerations,
+ double maxDeviation,
+ IceUtil::Time const& timestep)
{
bool hasLimitlessDimension = false;
@@ -1770,9 +1876,11 @@ namespace armarx
waypointList.push_back(Eigen::Map<Eigen::VectorXd>(positions.data(), dimensions));
}
- VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(VirtualRobot::Path(waypointList, maxDeviation),
- maxVelocities,
- maxAccelerations, timestepValue);
+ VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(
+ VirtualRobot::Path(waypointList, maxDeviation),
+ maxVelocities,
+ maxAccelerations,
+ timestepValue);
ARMARX_CHECK_EXPRESSION(timeOptimalTraj.isValid());
@@ -1821,17 +1929,14 @@ namespace armarx
return newTraj;
}
-
-
-
-
-
-
-
- size_t Trajectory::addDimension(const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps, const std::string name)
+ size_t
+ Trajectory::addDimension(const Ice::DoubleSeq& values,
+ const Ice::DoubleSeq& timestamps,
+ const std::string name)
{
- const auto& tempTimestamps = timestamps.size() > 0 ? timestamps : GenerateTimestamps(values);
+ const auto& tempTimestamps =
+ timestamps.size() > 0 ? timestamps : GenerateTimestamps(values);
size_t newDimIndex = dim();
@@ -1849,13 +1954,14 @@ namespace armarx
return newDimIndex;
}
- void Trajectory::removeDimension(size_t dimension)
+ void
+ Trajectory::removeDimension(size_t dimension)
{
typename timestamp_view::iterator itMap = dataMap.begin();
for (; itMap != dataMap.end(); itMap++)
{
- std::vector< DoubleSeqPtr >& data = itMap->data;
+ std::vector<DoubleSeqPtr>& data = itMap->data;
if (dimension < data.size())
{
@@ -1868,17 +1974,18 @@ namespace armarx
}
}
- void Trajectory::removeDerivation(size_t derivation)
+ void
+ Trajectory::removeDerivation(size_t derivation)
{
typename timestamp_view::iterator itMap = dataMap.begin();
for (; itMap != dataMap.end(); itMap++)
{
- std::vector< DoubleSeqPtr >& data = itMap->data;
+ std::vector<DoubleSeqPtr>& data = itMap->data;
for (auto& vec : data)
{
- if (derivation + 1 < vec->size())
+ if (derivation + 1 < vec->size())
{
vec->resize(derivation);
}
@@ -1886,13 +1993,14 @@ namespace armarx
}
}
- void Trajectory::removeDerivation(size_t dimension, size_t derivation)
+ void
+ Trajectory::removeDerivation(size_t dimension, size_t derivation)
{
typename timestamp_view::iterator itMap = dataMap.begin();
for (; itMap != dataMap.end(); itMap++)
{
- std::vector< DoubleSeqPtr >& data = itMap->data;
+ std::vector<DoubleSeqPtr>& data = itMap->data;
if (data.size() > dimension && derivation + 1 < data.at(dimension)->size())
{
@@ -1901,34 +2009,40 @@ namespace armarx
}
}
- Trajectory::ordered_view::const_iterator Trajectory::begin() const
+ Trajectory::ordered_view::const_iterator
+ Trajectory::begin() const
{
return dataMap.get<TagOrdered>().begin();
}
- Trajectory::ordered_view::const_iterator Trajectory::end() const
+ Trajectory::ordered_view::const_iterator
+ Trajectory::end() const
{
return dataMap.get<TagOrdered>().end();
}
- Trajectory::ordered_view::const_reverse_iterator Trajectory::rbegin() const
+ Trajectory::ordered_view::const_reverse_iterator
+ Trajectory::rbegin() const
{
return dataMap.get<TagOrdered>().rbegin();
}
- Trajectory::ordered_view::const_reverse_iterator Trajectory::rend() const
+ Trajectory::ordered_view::const_reverse_iterator
+ Trajectory::rend() const
{
return dataMap.get<TagOrdered>().rend();
}
- std::vector<DoubleSeqPtr>& Trajectory::operator[](double timestamp)
+ std::vector<DoubleSeqPtr>&
+ Trajectory::operator[](double timestamp)
{
return getStates(timestamp);
}
-
-
- void Trajectory::addPositionsToDimension(size_t dimension, const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps)
+ void
+ Trajectory::addPositionsToDimension(size_t dimension,
+ const Ice::DoubleSeq& values,
+ const Ice::DoubleSeq& timestamps)
{
if (dimension >= dim() && dim() > 0)
{
@@ -1936,7 +2050,8 @@ namespace armarx
}
else
{
- ARMARX_CHECK_EXPRESSION(timestamps.size() == values.size()) << timestamps.size() << ", " << values.size();
+ ARMARX_CHECK_EXPRESSION(timestamps.size() == values.size())
+ << timestamps.size() << ", " << values.size();
for (size_t i = 0; i < timestamps.size(); ++i)
{
@@ -1947,8 +2062,10 @@ namespace armarx
}
}
-
- void Trajectory::addDerivationsToDimension(size_t dimension, const double t, const Ice::DoubleSeq& derivs)
+ void
+ Trajectory::addDerivationsToDimension(size_t dimension,
+ const double t,
+ const Ice::DoubleSeq& derivs)
{
setEntries(t, dimension, derivs);
}
@@ -1958,22 +2075,26 @@ namespace armarx
trajectory = traj;
}
- DoubleSeqPtr Trajectory::TrajData::operator[](size_t dim) const
+ DoubleSeqPtr
+ Trajectory::TrajData::operator[](size_t dim) const
{
return data.at(dim);
}
- double Trajectory::TrajData::getTimestamp() const
+ double
+ Trajectory::TrajData::getTimestamp() const
{
return timestamp;
}
- double Trajectory::TrajData::getPosition(size_t dim) const
+ double
+ Trajectory::TrajData::getPosition(size_t dim) const
{
return getDeriv(dim, 0);
}
- Eigen::VectorXf Trajectory::TrajData::getPositionsAsVectorXf() const
+ Eigen::VectorXf
+ Trajectory::TrajData::getPositionsAsVectorXf() const
{
if (!trajectory)
{
@@ -1988,7 +2109,8 @@ namespace armarx
return result;
}
- Eigen::VectorXd Trajectory::TrajData::getPositionsAsVectorXd() const
+ Eigen::VectorXd
+ Trajectory::TrajData::getPositionsAsVectorXd() const
{
if (!trajectory)
{
@@ -2003,7 +2125,8 @@ namespace armarx
return result;
}
- double Trajectory::TrajData::getDeriv(size_t dim, size_t derivation) const
+ double
+ Trajectory::TrajData::getDeriv(size_t dim, size_t derivation) const
{
if (!trajectory)
{
@@ -2012,15 +2135,14 @@ namespace armarx
return trajectory->getState(timestamp, dim, derivation);
}
- const std::vector<DoubleSeqPtr>& Trajectory::TrajData::getData() const
+ const std::vector<DoubleSeqPtr>&
+ Trajectory::TrajData::getData() const
{
return data;
}
-
-
-
- void Trajectory::shiftTime(double shift)
+ void
+ Trajectory::shiftTime(double shift)
{
const ordered_view& ordv = dataMap.get<TagOrdered>();
typename ordered_view::const_iterator itMap = ordv.begin();
@@ -2038,11 +2160,13 @@ namespace armarx
*this = shiftedTraj;
}
- void Trajectory::shiftValue(const Ice::DoubleSeq& shift)
+ void
+ Trajectory::shiftValue(const Ice::DoubleSeq& shift)
{
if (shift.size() > dim())
{
- throw LocalException("dimension is out of range: ") << shift.size() << " actual dimensions: " << dim();
+ throw LocalException("dimension is out of range: ")
+ << shift.size() << " actual dimensions: " << dim();
}
@@ -2056,14 +2180,15 @@ namespace armarx
itMap->data[dimension]->at(0) += shift[dimension];
}
}
-
}
- void Trajectory::scaleValue(const Ice::DoubleSeq& factor)
+ void
+ Trajectory::scaleValue(const Ice::DoubleSeq& factor)
{
if (factor.size() > dim())
{
- throw LocalException("dimension is out of range: ") << factor.size() << " actual dimensions: " << dim();
+ throw LocalException("dimension is out of range: ")
+ << factor.size() << " actual dimensions: " << dim();
}
@@ -2077,16 +2202,16 @@ namespace armarx
itMap->data[dimension]->at(0) *= factor[dimension];
}
}
-
}
-
- void Trajectory::setLimitless(const LimitlessStateSeq& limitlessStates)
+ void
+ Trajectory::setLimitless(const LimitlessStateSeq& limitlessStates)
{
limitless = limitlessStates;
}
- LimitlessStateSeq Trajectory::getLimitless() const
+ LimitlessStateSeq
+ Trajectory::getLimitless() const
{
return limitless;
}
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 774b324ee1d50515576625900555a8ef15510758..9d2905a90676c48aed90167633aed8b829d7b2ce 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -23,26 +23,26 @@
*/
#pragma once
-#include <RobotAPI/interface/core/Trajectory.h>
+#include <boost/multi_index/hashed_index.hpp>
+#include <boost/multi_index/member.hpp>
+#include <boost/multi_index/ordered_index.hpp>
+#include <boost/multi_index/random_access_index.hpp>
+#include <boost/multi_index_container.hpp>
+
+#include <Eigen/Core>
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/Variant.h>
-#include <boost/multi_index_container.hpp>
-#include <boost/multi_index/hashed_index.hpp>
-#include <boost/multi_index/random_access_index.hpp>
-#include <boost/multi_index/ordered_index.hpp>
-#include <boost/multi_index/member.hpp>
-
-#include <Eigen/Core>
+#include <RobotAPI/interface/core/Trajectory.h>
namespace armarx::VariantType
{
// Variant types
const VariantTypeId Trajectory = Variant::addTypeName("::armarx::TrajectoryBase");
-}
+} // namespace armarx::VariantType
namespace armarx
{
@@ -77,17 +77,21 @@ namespace armarx
{
protected:
~Trajectory() override;
- public:
-
- using TrajMap = std::map<double, std::vector< DoubleSeqPtr > >;
+ public:
+ using TrajMap = std::map<double, std::vector<DoubleSeqPtr>>;
struct TrajData
{
- TrajData() {}
+ TrajData()
+ {
+ }
+
TrajData(Trajectory* traj);
DoubleSeqPtr operator[](size_t dim) const;
- inline TrajData& operator=(const TrajData& s)
+
+ inline TrajData&
+ operator=(const TrajData& s)
{
timestamp = s.timestamp;
data.resize(s.data.size());
@@ -100,12 +104,12 @@ namespace armarx
return *this;
}
-
double getTimestamp() const;
double getPosition(size_t dim) const;
- template<class T = double>
- std::vector<T> getPositionsAs() const
+ template <class T = double>
+ std::vector<T>
+ getPositionsAs() const
{
if (!trajectory)
{
@@ -120,16 +124,19 @@ namespace armarx
}
return result;
}
- std::vector<double> getPositions() const
+
+ std::vector<double>
+ getPositions() const
{
return getPositionsAs<double>();
}
+
Eigen::VectorXf getPositionsAsVectorXf() const;
Eigen::VectorXd getPositionsAsVectorXd() const;
-
- template<class T>
- void writePositionsToNameValueMap(std::map<std::string, T>& map) const
+ template <class T>
+ void
+ writePositionsToNameValueMap(std::map<std::string, T>& map) const
{
if (!trajectory)
{
@@ -141,24 +148,28 @@ namespace armarx
map[trajectory->getDimensionName(i)] = getPosition(i);
}
}
- template<class T>
- std::map<std::string, T> getPositionsAsNameValueMap() const
+
+ template <class T>
+ std::map<std::string, T>
+ getPositionsAsNameValueMap() const
{
std::map<std::string, T> result;
writePositionsToNameValueMap(result);
return result;
}
-
double getDeriv(size_t dim, size_t derivation) const;
- const std::vector< DoubleSeqPtr >& getData() const;
+ const std::vector<DoubleSeqPtr>& getData() const;
- friend std::ostream& operator<<(std::ostream& stream, const TrajData& rhs)
+ friend std::ostream&
+ operator<<(std::ostream& stream, const TrajData& rhs)
{
stream << rhs.timestamp << ": ";
for (size_t d = 0; d < rhs.data.size(); ++d)
{
- stream << (rhs.data[d] && rhs.data[d]->size() > 0 ? to_string(rhs.data[d]->at(0)) : std::string("-"));
+ stream << (rhs.data[d] && rhs.data[d]->size() > 0
+ ? to_string(rhs.data[d]->at(0))
+ : std::string("-"));
if (d <= rhs.data.size() - 1)
{
stream << ", ";
@@ -168,35 +179,52 @@ namespace armarx
}
double timestamp;
- mutable std::vector< DoubleSeqPtr > data; // mutable so that it can be changed
+ mutable std::vector<DoubleSeqPtr> data; // mutable so that it can be changed
protected:
Trajectory* trajectory = nullptr;
// friend class Trajectory;
-
};
// structs for indices
- struct TagTimestamp {};
- struct TagOrdered {};
+ struct TagTimestamp
+ {
+ };
+ struct TagOrdered
+ {
+ };
// Trajectory data container
- typedef boost::multi_index::multi_index_container <
- TrajData,
- boost::multi_index::indexed_by <
- boost::multi_index::hashed_unique<boost::multi_index::tag<TagTimestamp>, boost::multi_index::member<TrajData, double, &TrajData::timestamp> >,
- boost::multi_index::ordered_unique<boost::multi_index::tag<TagOrdered>, boost::multi_index::member<TrajData, double, &TrajData::timestamp> > // this index represents timestamp incremental order
- >
- > TrajDataContainer;
- using timestamp_view = typename boost::multi_index::index<TrajDataContainer, TagTimestamp>::type;
- using ordered_view = typename boost::multi_index::index<TrajDataContainer, TagOrdered>::type;
-
-
+ typedef boost::multi_index::multi_index_container<
+ TrajData,
+ boost::multi_index::indexed_by<
+ boost::multi_index::hashed_unique<
+ boost::multi_index::tag<TagTimestamp>,
+ boost::multi_index::member<TrajData, double, &TrajData::timestamp>>,
+ boost::multi_index::ordered_unique<
+ boost::multi_index::tag<TagOrdered>,
+ boost::multi_index::member<
+ TrajData,
+ double,
+ &TrajData::timestamp>> // this index represents timestamp incremental order
+ >>
+ TrajDataContainer;
+ using timestamp_view =
+ typename boost::multi_index::index<TrajDataContainer, TagTimestamp>::type;
+ using ordered_view =
+ typename boost::multi_index::index<TrajDataContainer, TagOrdered>::type;
+
+ Trajectory()
+ {
+ }
- Trajectory() {}
Trajectory(const Trajectory& source);
+
template <typename T>
- Trajectory(const std::vector<T>& data, const Ice::DoubleSeq timestamps = Ice::DoubleSeq(), const std::string& dimensionName = "", typename std::enable_if_t<std::is_arithmetic_v< T > >* t = 0)
+ Trajectory(const std::vector<T>& data,
+ const Ice::DoubleSeq timestamps = Ice::DoubleSeq(),
+ const std::string& dimensionName = "",
+ typename std::enable_if_t<std::is_arithmetic_v<T>>* t = 0)
{
if (data.size() == 0)
{
@@ -214,25 +242,29 @@ namespace armarx
*
*/
template <typename T>
- Trajectory(const std::vector<std::vector<T>>& data, const Ice::DoubleSeq timestamps = Ice::DoubleSeq(), const Ice::StringSeq& dimensionNames = {})
+ Trajectory(const std::vector<std::vector<T>>& data,
+ const Ice::DoubleSeq timestamps = Ice::DoubleSeq(),
+ const Ice::StringSeq& dimensionNames = {})
{
if (data.size() == 0)
{
return;
}
- const auto& tempTimestamps = timestamps.size() > 0 ? timestamps : GenerateTimestamps(data.at(0));
+ const auto& tempTimestamps =
+ timestamps.size() > 0 ? timestamps : GenerateTimestamps(data.at(0));
size_t i = 0;
for (const auto& subvec : data)
{
Ice::DoubleSeq dvec(subvec.begin(), subvec.end());
- addDimension(dvec, tempTimestamps, i < dimensionNames.size() ? dimensionNames.at(i++) : "");
+ addDimension(
+ dvec, tempTimestamps, i < dimensionNames.size() ? dimensionNames.at(i++) : "");
}
}
-
-
template <typename T>
- Trajectory(const std::vector<std::vector<std::vector<T> > >& data, const Ice::DoubleSeq timestamps = Ice::DoubleSeq(), const Ice::StringSeq& dimensionNames = {})
+ Trajectory(const std::vector<std::vector<std::vector<T>>>& data,
+ const Ice::DoubleSeq timestamps = Ice::DoubleSeq(),
+ const Ice::StringSeq& dimensionNames = {})
{
this->dataVec = data;
this->timestamps = timestamps;
@@ -247,9 +279,14 @@ namespace armarx
Trajectory& operator=(const Trajectory& source);
// Basic Manipulators
- size_t addDimension(const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps = Ice::DoubleSeq(), const std::string name = "");
- void addPositionsToDimension(size_t dimension, const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps);
- void addDerivationsToDimension(size_t dimension, const double t, const Ice::DoubleSeq& derivs);
+ size_t addDimension(const Ice::DoubleSeq& values,
+ const Ice::DoubleSeq& timestamps = Ice::DoubleSeq(),
+ const std::string name = "");
+ void addPositionsToDimension(size_t dimension,
+ const Ice::DoubleSeq& values,
+ const Ice::DoubleSeq& timestamps);
+ void
+ addDerivationsToDimension(size_t dimension, const double t, const Ice::DoubleSeq& derivs);
Trajectory& operator+=(const Trajectory traj);
void removeDimension(size_t dimension);
void removeDerivation(size_t derivation);
@@ -273,8 +310,10 @@ namespace armarx
std::vector<DoubleSeqPtr>& getStates(double t);
std::vector<DoubleSeqPtr> getStates(double t) const;
Ice::DoubleSeq getStates(double t, size_t derivation) const;
- template<typename T>
- std::map<std::string, T> getStatesMap(double t, size_t derivation = 0) const
+
+ template <typename T>
+ std::map<std::string, T>
+ getStatesMap(double t, size_t derivation = 0) const
{
std::map<std::string, T> result;
size_t dimensions = dim();
@@ -289,8 +328,8 @@ namespace armarx
double getState(double t, size_t dim = 0, size_t derivation = 0);
double getState(double t, size_t dim = 0, size_t derivation = 0) const;
- std::vector<Ice::DoubleSeq > getAllStates(double t, int maxDeriv = 1);
- Ice::DoubleSeq getDerivations(double t, size_t dimension, size_t derivations) const;
+ std::vector<Ice::DoubleSeq> getAllStates(double t, int maxDeriv = 1);
+ Ice::DoubleSeq getDerivations(double t, size_t dimension, size_t derivations) const;
std::string getDimensionName(size_t dim) const;
const Ice::StringSeq& getDimensionNames() const;
@@ -304,10 +343,14 @@ namespace armarx
*/
Ice::DoubleSeq getDimensionData(size_t dimension, size_t derivation = 0) const;
Eigen::VectorXd getDimensionDataAsEigen(size_t dimension, size_t derivation) const;
- Eigen::VectorXd getDimensionDataAsEigen(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ Eigen::VectorXd getDimensionDataAsEigen(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const;
Eigen::MatrixXd toEigen(size_t derivation, double startTime, double endTime) const;
Eigen::MatrixXd toEigen(size_t derivation = 0) const;
- TrajectoryPtr getPart(double startTime, double endTime, size_t numberOfDerivations = 0) const;
+ TrajectoryPtr
+ getPart(double startTime, double endTime, size_t numberOfDerivations = 0) const;
/**
* @brief dim returns the trajectory dimension count for this trajectory (e.g. taskspace w/o orientation would be 3)
* @return
@@ -331,13 +374,23 @@ namespace armarx
double getLength(size_t derivation = 0) const;
double getLength(size_t derivation, double startTime, double endTime) const;
double getLength(size_t dimension, size_t derivation) const;
- double getLength(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ double
+ getLength(size_t dimension, size_t derivation, double startTime, double endTime) const;
double getSquaredLength(size_t dimension, size_t derivation) const;
- double getSquaredLength(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ double getSquaredLength(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const;
double getMax(size_t dimension, size_t derivation, double startTime, double endTime) const;
double getMin(size_t dimension, size_t derivation, double startTime, double endTime) const;
- double getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t derivation, double startTime, double endTime) const;
- double getAmplitude(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ double getWithFunc(const double& (*foo)(const double&, const double&),
+ double initValue,
+ size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const;
+ double
+ getAmplitude(size_t dimension, size_t derivation, double startTime, double endTime) const;
/**
* @brief Calculate *all* minima.
@@ -347,20 +400,35 @@ namespace armarx
* @param endTime
* @return
*/
- Ice::DoubleSeq getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const;
- Ice::DoubleSeq getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const;
- Ice::DoubleSeq getMinimaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const;
- Ice::DoubleSeq getMaximaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ Ice::DoubleSeq
+ getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ Ice::DoubleSeq
+ getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ Ice::DoubleSeq getMinimaTimestamps(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const;
+ Ice::DoubleSeq getMaximaTimestamps(size_t dimension,
+ size_t derivation,
+ double startTime,
+ double endTime) const;
// calculations
- Ice::DoubleSeq getDiscreteDifferentiationForDim(size_t trajDimension, size_t derivation) const;
- static Ice::DoubleSeq DifferentiateDiscretly(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& values, int derivationCount = 1);
- double getDiscretDifferentiationForDimAtT(double t, size_t trajDimension, size_t derivation) const;
+ Ice::DoubleSeq getDiscreteDifferentiationForDim(size_t trajDimension,
+ size_t derivation) const;
+ static Ice::DoubleSeq DifferentiateDiscretly(const Ice::DoubleSeq& timestamps,
+ const Ice::DoubleSeq& values,
+ int derivationCount = 1);
+ double
+ getDiscretDifferentiationForDimAtT(double t, size_t trajDimension, size_t derivation) const;
void differentiateDiscretlyForDim(size_t trajDimension, size_t derivation);
void differentiateDiscretly(size_t derivation);
- void reconstructFromDerivativeForDim(double valueAtFirstTimestamp, size_t trajDimension, size_t sourceDimOfSystemState, size_t targetDimOfSystemState);
+ void reconstructFromDerivativeForDim(double valueAtFirstTimestamp,
+ size_t trajDimension,
+ size_t sourceDimOfSystemState,
+ size_t targetDimOfSystemState);
/**
* @brief negateDim changes the sign of all values of the given dimension.
@@ -374,14 +442,27 @@ namespace armarx
*/
void gaussianFilter(double filterRadius);
- static Trajectory NormalizeTimestamps(const Trajectory& traj, const double startTime = 0.0, const double endTime = 1.0);
- TrajectoryPtr normalize(const double startTime = 0.0, const double endTime = 1.0);
- TrajectoryPtr calculateTimeOptimalTrajectory(double maxVelocity, double maxAcceleration, double maxDeviation, IceUtil::Time const& timestep);
- TrajectoryPtr calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities, const Eigen::VectorXd& maxAccelerations, double maxDeviation, IceUtil::Time const& timestep);
- static Ice::DoubleSeq NormalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime = 0.0, const double endTime = 1.0);
- static Ice::DoubleSeq GenerateTimestamps(double startTime = 0.0, double endTime = 1.0, double stepSize = 0.001);
+ static Trajectory NormalizeTimestamps(const Trajectory& traj,
+ const double startTime = 0.0,
+ const double endTime = 1.0);
+ TrajectoryPtr normalize(const double startTime = 0.0, const double endTime = 1.0);
+ TrajectoryPtr calculateTimeOptimalTrajectory(double maxVelocity,
+ double maxAcceleration,
+ double maxDeviation,
+ IceUtil::Time const& timestep);
+ TrajectoryPtr calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities,
+ const Eigen::VectorXd& maxAccelerations,
+ double maxDeviation,
+ IceUtil::Time const& timestep);
+ static Ice::DoubleSeq NormalizeTimestamps(const Ice::DoubleSeq& timestamps,
+ const double startTime = 0.0,
+ const double endTime = 1.0);
+ static Ice::DoubleSeq
+ GenerateTimestamps(double startTime = 0.0, double endTime = 1.0, double stepSize = 0.001);
+
template <typename T>
- static Ice::DoubleSeq GenerateTimestamps(const std::vector<T>& values)
+ static Ice::DoubleSeq
+ GenerateTimestamps(const std::vector<T>& values)
{
return GenerateTimestamps(0, 1, 1.0 / (values.size() - 1));
}
@@ -400,7 +481,8 @@ namespace armarx
bool dataExists(double t, size_t dimension = 0, size_t derivation = 0) const;
- std::map<double, Ice::DoubleSeq> getStatesAround(double t, size_t derivation, size_t extend) const;
+ std::map<double, Ice::DoubleSeq>
+ getStatesAround(double t, size_t derivation, size_t extend) const;
// Object interface
@@ -408,8 +490,10 @@ namespace armarx
void ice_postUnmarshal() override;
// Serializable interface
- void serialize(const ObjectSerializerBasePtr& obj, const Ice::Current& = Ice::emptyCurrent) const override;
- void deserialize(const ObjectSerializerBasePtr&, const Ice::Current& = Ice::emptyCurrent) override;
+ void serialize(const ObjectSerializerBasePtr& obj,
+ const Ice::Current& = Ice::emptyCurrent) const override;
+ void deserialize(const ObjectSerializerBasePtr&,
+ const Ice::Current& = Ice::emptyCurrent) override;
// VariantDataClass interface
VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
@@ -420,7 +504,8 @@ namespace armarx
Ice::ObjectPtr ice_clone() const override;
- void setDimensionNames(const Ice::StringSeq dimNames)
+ void
+ setDimensionNames(const Ice::StringSeq dimNames)
{
dimensionNames = dimNames;
}
@@ -431,19 +516,25 @@ namespace armarx
protected:
void __addDimension();
void __fillAllEmptyFields();
- double __interpolate(typename ordered_view::const_iterator itMap, size_t dimension, size_t derivation) const;
- double __gaussianFilter(double filterRadius, typename ordered_view::iterator centerIt, size_t trajNum, size_t dim);
+ double __interpolate(typename ordered_view::const_iterator itMap,
+ size_t dimension,
+ size_t derivation) const;
+ double __gaussianFilter(double filterRadius,
+ typename ordered_view::iterator centerIt,
+ size_t trajNum,
+ size_t dim);
timestamp_view::iterator __fillBaseDataAtTimestamp(const double& t);
std::vector<DoubleSeqPtr> __calcBaseDataAtTimestamp(const double& t) const;
TrajDataContainer dataMap;
- virtual double __interpolate(double t, ordered_view::const_iterator itPrev, ordered_view::const_iterator itNext, size_t dimension, size_t derivation) const;
+ virtual double __interpolate(double t,
+ ordered_view::const_iterator itPrev,
+ ordered_view::const_iterator itNext,
+ size_t dimension,
+ size_t derivation) const;
double __interpolate(double t, size_t dimension, size_t derivation) const;
-
-
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
index 5328d534a2b6ce76915a343e2cdad3014a4ee1b4..87f17273a76a43bc2345edfa90cef5d0f60a56d2 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.cpp
+++ b/source/RobotAPI/libraries/core/TrajectoryController.cpp
@@ -22,13 +22,19 @@
* GNU General Public License
*/
#include "TrajectoryController.h"
+
#include <RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
namespace armarx
{
- TrajectoryController::TrajectoryController(const TrajectoryPtr& traj, float kp, float ki, float kd, bool threadSafe, float maxIntegral) :
+ TrajectoryController::TrajectoryController(const TrajectoryPtr& traj,
+ float kp,
+ float ki,
+ float kd,
+ bool threadSafe,
+ float maxIntegral) :
traj(traj)
{
std::vector<bool> limitless;
@@ -36,7 +42,13 @@ namespace armarx
{
limitless.push_back(ls.enabled);
}
- pid.reset(new MultiDimPIDController(kp, ki, kd, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), threadSafe, limitless));
+ pid.reset(new MultiDimPIDController(kp,
+ ki,
+ kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ threadSafe,
+ limitless));
pid->maxIntegral = maxIntegral;
pid->preallocate(traj->dim());
ARMARX_CHECK_EXPRESSION(traj);
@@ -51,14 +63,15 @@ namespace armarx
currentError.resize(traj->dim(), 1);
}
- const Eigen::VectorXf& TrajectoryController::update(double deltaT, const Eigen::VectorXf& currentPosition)
+ const Eigen::VectorXf&
+ TrajectoryController::update(double deltaT, const Eigen::VectorXf& currentPosition)
{
ARMARX_CHECK_EXPRESSION(pid);
ARMARX_CHECK_EXPRESSION(traj);
ARMARX_CHECK_EXPRESSION(traj->size() > 0);
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(currentPosition.rows()), traj->dim());
size_t dim = traj->dim();
- currentTimestamp = currentTimestamp + deltaT;
+ currentTimestamp = currentTimestamp + deltaT;
const Trajectory& traj = *this->traj;
if (currentTimestamp < 0.0)
{
@@ -70,7 +83,8 @@ namespace armarx
{
positions(i) = traj.getState(currentTimestamp, i, 0);
- veloctities(i) = (std::signbit(deltaT) ? -1.0 : 1.0) * traj.getState(currentTimestamp, i, 1);
+ veloctities(i) =
+ (std::signbit(deltaT) ? -1.0 : 1.0) * traj.getState(currentTimestamp, i, 1);
//pids.at(i)->update(std::abs(deltaT), currentPosition(i), positions(i));
//veloctities(i) += pids.at(i)->getControlValue();
}
@@ -93,7 +107,6 @@ namespace armarx
{
currentError(i) = positions(i) - currentPosition(i);
}
-
}
pid->update(std::abs(deltaT), currentPosition, positions);
@@ -112,17 +125,20 @@ namespace armarx
pid = value;
}*/
- double TrajectoryController::getCurrentTimestamp() const
+ double
+ TrajectoryController::getCurrentTimestamp() const
{
return currentTimestamp;
}
- const TrajectoryPtr& TrajectoryController::getTraj() const
+ const TrajectoryPtr&
+ TrajectoryController::getTraj() const
{
return traj;
}
- void TrajectoryController::UnfoldLimitlessJointPositions(TrajectoryPtr traj)
+ void
+ TrajectoryController::UnfoldLimitlessJointPositions(TrajectoryPtr traj)
{
if (traj->size() == 0)
{
@@ -151,14 +167,15 @@ namespace armarx
if (limitlessStates.at(i).enabled)
{
- state.getData().at(i)->at(0) = linTraj.at(i).update(state.getData().at(i)->at(0));
+ state.getData().at(i)->at(0) =
+ linTraj.at(i).update(state.getData().at(i)->at(0));
}
-
}
}
}
- void TrajectoryController::FoldLimitlessJointPositions(TrajectoryPtr traj)
+ void
+ TrajectoryController::FoldLimitlessJointPositions(TrajectoryPtr traj)
{
if (traj->size() == 0)
{
@@ -176,19 +193,22 @@ namespace armarx
continue;
}
double pos = state.getPosition(i);
- double center = (limitlessStates.at(i).limitHi + limitlessStates.at(i).limitLo) * 0.5;
+ double center =
+ (limitlessStates.at(i).limitHi + limitlessStates.at(i).limitLo) * 0.5;
double newPos = math::MathUtils::angleModX(pos, center);
state.getData().at(i)->at(0) = newPos;
}
}
}
- const Eigen::VectorXf& TrajectoryController::getCurrentError() const
+ const Eigen::VectorXf&
+ TrajectoryController::getCurrentError() const
{
return currentError;
}
- const Eigen::VectorXf& TrajectoryController::getPositions() const
+ const Eigen::VectorXf&
+ TrajectoryController::getPositions() const
{
return positions;
}
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.h b/source/RobotAPI/libraries/core/TrajectoryController.h
index 170729a05046e2e87e2ea2d7a4a4a259d9fa9858..3c7a9f21fce7a6466bbdec9ecc61d48c9cdf4dce 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.h
+++ b/source/RobotAPI/libraries/core/TrajectoryController.h
@@ -32,7 +32,12 @@ namespace armarx
class TrajectoryController
{
public:
- TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f, bool threadSafe = true, float maxIntegral = std::numeric_limits<float>::max());
+ TrajectoryController(const TrajectoryPtr& traj,
+ float kp,
+ float ki = 0.0f,
+ float kd = 0.0f,
+ bool threadSafe = true,
+ float maxIntegral = std::numeric_limits<float>::max());
const Eigen::VectorXf& update(double deltaT, const Eigen::VectorXf& currentPosition);
//const MultiDimPIDControllerPtr& getPid() const;
//void setPid(const MultiDimPIDControllerPtr& value);
@@ -56,9 +61,8 @@ namespace armarx
Eigen::VectorXf positions;
Eigen::VectorXf veloctities;
Eigen::VectorXf currentError;
-
};
+
using TrajectoryControllerPtr = std::shared_ptr<TrajectoryController>;
} // namespace armarx
-
diff --git a/source/RobotAPI/libraries/core/VectorHelpers.h b/source/RobotAPI/libraries/core/VectorHelpers.h
index 8ebb0f359a71770532d2627146a2b7fb014f732a..f6c19a46d6f2b9ca2fec9031bfa6f1b126c04f03 100644
--- a/source/RobotAPI/libraries/core/VectorHelpers.h
+++ b/source/RobotAPI/libraries/core/VectorHelpers.h
@@ -23,18 +23,20 @@
*/
#pragma once
-#include <ArmarXCore/core/exceptions/Exception.h>
-
-#include <Eigen/Geometry>
#include <cmath>
+#include <limits>
#include <map>
#include <vector>
-#include <limits>
+
+#include <Eigen/Geometry>
+
+#include <ArmarXCore/core/exceptions/Exception.h>
namespace armarx
{
- template<class T>
- inline void checkValue(const T& value)
+ template <class T>
+ inline void
+ checkValue(const T& value)
{
if (std::numeric_limits<T>::infinity() == value)
{
@@ -47,8 +49,9 @@ namespace armarx
}
}
- template<class T>
- inline void checkValues(const std::vector<T >& values)
+ template <class T>
+ inline void
+ checkValues(const std::vector<T>& values)
{
for (size_t i = 0; i < values.size(); ++i)
{
@@ -56,9 +59,9 @@ namespace armarx
}
}
-
-
- template <class T> std::vector<T> operator -(const std::vector<T>& v1, const std::vector<T>& v2)
+ template <class T>
+ std::vector<T>
+ operator-(const std::vector<T>& v1, const std::vector<T>& v2)
{
std::vector<T> res(std::min(v1.size(), v2.size()), T());
int j = 0;
@@ -72,7 +75,9 @@ namespace armarx
return res;
}
- template <class T> std::vector<T> operator +(const std::vector<T>& v1, const std::vector<T>& v2)
+ template <class T>
+ std::vector<T>
+ operator+(const std::vector<T>& v1, const std::vector<T>& v2)
{
std::vector<T> res(std::min(v1.size(), v2.size()), T());
int j = 0;
@@ -86,7 +91,9 @@ namespace armarx
return res;
}
- template <class T> std::vector<T>& operator +=(std::vector<T>& v1, const std::vector<T>& v2)
+ template <class T>
+ std::vector<T>&
+ operator+=(std::vector<T>& v1, const std::vector<T>& v2)
{
int j = 0;
@@ -99,8 +106,9 @@ namespace armarx
return v1;
}
-
- template <class T> std::vector<T>& operator -=(std::vector<T>& v1, const std::vector<T>& v2)
+ template <class T>
+ std::vector<T>&
+ operator-=(std::vector<T>& v1, const std::vector<T>& v2)
{
int j = 0;
@@ -113,7 +121,9 @@ namespace armarx
return v1;
}
- template <class T> std::vector<T>& operator *=(std::vector<T>& v1, double c)
+ template <class T>
+ std::vector<T>&
+ operator*=(std::vector<T>& v1, double c)
{
int j = 0;
@@ -126,9 +136,9 @@ namespace armarx
return v1;
}
-
-
- template <class T> std::vector<T>& operator /=(std::vector<T>& v1, double c)
+ template <class T>
+ std::vector<T>&
+ operator/=(std::vector<T>& v1, double c)
{
int j = 0;
@@ -141,7 +151,9 @@ namespace armarx
return v1;
}
- template <class T> std::vector<T> operator *(double c, const std::vector<T>& v)
+ template <class T>
+ std::vector<T>
+ operator*(double c, const std::vector<T>& v)
{
std::vector<T> res(v.size(), T());
int j = 0;
@@ -155,13 +167,16 @@ namespace armarx
return res;
}
- template <class T> double operator *(const std::vector<T>& v, const std::vector<T>& v2)
+ template <class T>
+ double
+ operator*(const std::vector<T>& v, const std::vector<T>& v2)
{
double res = 0;
if (v.size() != v2.size())
{
- throw LocalException("vectors do not match in size: ") << v.size() << " vs. " << v2.size();
+ throw LocalException("vectors do not match in size: ")
+ << v.size() << " vs. " << v2.size();
}
int j = 0;
@@ -175,8 +190,9 @@ namespace armarx
return res;
}
-
- template <class T> std::vector<T> operator +(const std::vector<T>& v, double s)
+ template <class T>
+ std::vector<T>
+ operator+(const std::vector<T>& v, double s)
{
std::vector<T> res(v.size(), T());
int j = 0;
@@ -190,7 +206,9 @@ namespace armarx
return res;
}
- template <class T> std::vector<T> operator -(const std::vector<T>& v, double s)
+ template <class T>
+ std::vector<T>
+ operator-(const std::vector<T>& v, double s)
{
std::vector<T> res(v.size(), T());
int j = 0;
@@ -204,19 +222,23 @@ namespace armarx
return res;
}
- template <class T> double vecLength(const std::vector<T>& v)
+ template <class T>
+ double
+ vecLength(const std::vector<T>& v)
{
double result = 0.0;
for (typename std::vector<T>::const_iterator it = v.begin(); it != v.end(); it++)
{
- result += *it** it;
+ result += *it * *it;
}
return sqrt(result);
}
- template <class T> double vecSum(const std::vector<T>& v)
+ template <class T>
+ double
+ vecSum(const std::vector<T>& v)
{
double result = 0.0;
@@ -228,7 +250,9 @@ namespace armarx
return result;
}
- template <class T> std::vector<T> normalizedVec(const std::vector<T>& v)
+ template <class T>
+ std::vector<T>
+ normalizedVec(const std::vector<T>& v)
{
double length = vecLength(v);
std::vector<T> result = v;
@@ -236,9 +260,11 @@ namespace armarx
return result;
}
- template <class T> std::vector<T> operator /(const std::vector<T>& v, double c)
+ template <class T>
+ std::vector<T>
+ operator/(const std::vector<T>& v, double c)
{
- std::vector<T> res(v.size(), T());
+ std::vector<T> res(v.size(), T());
int j = 0;
for (typename std::vector<T>::iterator i = res.begin(); i != res.end(); ++i)
@@ -250,7 +276,9 @@ namespace armarx
return res;
}
- template <class T> std::vector<T> abs(const std::vector<T>& v)
+ template <class T>
+ std::vector<T>
+ abs(const std::vector<T>& v)
{
std::vector<T> res(v.size(), T());
int j = 0;
@@ -264,7 +292,9 @@ namespace armarx
return res;
}
- template <class T> std::vector<T> max(const std::vector<T>& v1, const std::vector<T>& v2)
+ template <class T>
+ std::vector<T>
+ max(const std::vector<T>& v1, const std::vector<T>& v2)
{
std::vector<T> res(std::min(v1.size(), v2.size()), T());
int j = 0;
@@ -278,20 +308,23 @@ namespace armarx
return res;
}
- template <class T> T max(const std::vector<T>& v1)
+ template <class T>
+ T
+ max(const std::vector<T>& v1)
{
T maxValue = std::numeric_limits<T>::min();
for (typename std::vector<T>::const_iterator i = v1.begin(); i != v1.end(); ++i)
{
maxValue = std::max(maxValue, *i);
-
}
return maxValue;
}
- template <class T> std::vector<T> min(const std::vector<T>& v1, const std::vector<T>& v2)
+ template <class T>
+ std::vector<T>
+ min(const std::vector<T>& v1, const std::vector<T>& v2)
{
std::vector<T> res(std::min(v1.size(), v2.size()));
int j = 0;
@@ -305,16 +338,17 @@ namespace armarx
return res;
}
- template <class T> T min(const std::vector<T>& v1)
+ template <class T>
+ T
+ min(const std::vector<T>& v1)
{
T minValue = std::numeric_limits<T>::max();
for (typename std::vector<T>::const_iterator i = v1.begin(); i != v1.end(); ++i)
{
minValue = std::min(minValue, *i);
-
}
return minValue;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPose.h b/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPose.h
index 7c95aa50ba38f6a18f6fb4f6182c0f1e39825d4a..88ba89e9a564290cf13c2ffe737cd3af42170637 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPose.h
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPose.h
@@ -23,14 +23,13 @@
*/
#pragma once
+#include <ArmarXCore/core/FramedPose.h>
#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/observers/ConditionCheck.h>
-#include <ArmarXCore/core/FramedPose.h>
namespace armarx
{
- class ARMARXCORE_IMPORT_EXPORT ConditionCheckEqualsPose :
- public ConditionCheck
+ class ARMARXCORE_IMPORT_EXPORT ConditionCheckEqualsPose : public ConditionCheck
{
public:
ConditionCheckEqualsPose()
@@ -43,12 +42,14 @@ namespace armarx
addSupportedType(VariantType::Pose, createParameterTypeList(1, VariantType::Pose);
}
- ConditionCheck* clone()
+ ConditionCheck*
+ clone()
{
return new ConditionCheckEqualsPose(*this);
}
- bool evaluate(const StringVariantMap& dataFields)
+ bool
+ evaluate(const StringVariantMap& dataFields)
{
if (dataFields.size() != 1)
{
@@ -60,37 +61,59 @@ namespace armarx
VariantTypeId type = value.getType();
if (type == VariantType::Vector3)
- return (sqrt(((value.x - getParameter(0).getClass<Vector3>().x) * (value.x - getParameter(0).getClass<Vector3>().x)) +
- ((value.y - getParameter(0).getClass<Vector3>().y) * (value.y - getParameter(0).getClass<Vector3>().y)) +
- ((value.z - getParameter(0).getClass<Vector3>().x) * (value.x - getParameter(0).getClass<Vector3>().z))) == 0);
+ return (sqrt(((value.x - getParameter(0).getClass<Vector3>().x) *
+ (value.x - getParameter(0).getClass<Vector3>().x)) +
+ ((value.y - getParameter(0).getClass<Vector3>().y) *
+ (value.y - getParameter(0).getClass<Vector3>().y)) +
+ ((value.z - getParameter(0).getClass<Vector3>().x) *
+ (value.x - getParameter(0).getClass<Vector3>().z))) == 0);
if (type == VariantType::FramedPosition)
- return (sqrt(((value.x - getParameter(0).getClass<FramedPosition>().x) * (value.x - getParameter(0).getClass<FramedPosition>().x)) +
- ((value.y - getParameter(0).getClass<FramedPosition>().y) * (value.y - getParameter(0).getClass<FramedPosition>().y)) +
- ((value.z - getParameter(0).getClass<FramedPosition>().x) * (value.x - getParameter(0).getClass<FramedPosition>().z))) == 0);
+ return (sqrt(((value.x - getParameter(0).getClass<FramedPosition>().x) *
+ (value.x - getParameter(0).getClass<FramedPosition>().x)) +
+ ((value.y - getParameter(0).getClass<FramedPosition>().y) *
+ (value.y - getParameter(0).getClass<FramedPosition>().y)) +
+ ((value.z - getParameter(0).getClass<FramedPosition>().x) *
+ (value.x - getParameter(0).getClass<FramedPosition>().z))) == 0);
if (type == VariantType::Quaternion)
- return (sqrt(((value.qw - getParameter(0).getClass<Quaternion>().qw) * (value.qw - getParameter(0).getClass<Quaternion>().qw)) +
- ((value.qx - getParameter(0).getClass<Quaternion>().qx) * (value.qx - getParameter(0).getClass<Quaternion>().qx)) +
- ((value.qy - getParameter(0).getClass<Quaternion>().qy) * (value.qy - getParameter(0).getClass<Quaternion>().qy)) +
- ((value.qz - getParameter(0).getClass<Quaternion>().qx) * (value.qx - getParameter(0).getClass<Quaternion>().qz))) == 0);
+ return (sqrt(((value.qw - getParameter(0).getClass<Quaternion>().qw) *
+ (value.qw - getParameter(0).getClass<Quaternion>().qw)) +
+ ((value.qx - getParameter(0).getClass<Quaternion>().qx) *
+ (value.qx - getParameter(0).getClass<Quaternion>().qx)) +
+ ((value.qy - getParameter(0).getClass<Quaternion>().qy) *
+ (value.qy - getParameter(0).getClass<Quaternion>().qy)) +
+ ((value.qz - getParameter(0).getClass<Quaternion>().qx) *
+ (value.qx - getParameter(0).getClass<Quaternion>().qz))) == 0);
if (type == VariantType::FramedOrientation)
- return (sqrt(((value.qw - getParameter(0).getClass<FramedOrientation>().qw) * (value.qw - getParameter(0).getClass<FramedOrientation>().qw)) +
- ((value.qx - getParameter(0).getClass<FramedOrientation>().qx) * (value.qx - getParameter(0).getClass<FramedOrientation>().qx)) +
- ((value.qy - getParameter(0).getClass<FramedOrientation>().qy) * (value.qy - getParameter(0).getClass<FramedOrientation>().qy)) +
- ((value.qz - getParameter(0).getClass<FramedOrientation>().qx) * (value.qx - getParameter(0).getClass<FramedOrientation>().qz))) == 0);
+ return (sqrt(((value.qw - getParameter(0).getClass<FramedOrientation>().qw) *
+ (value.qw - getParameter(0).getClass<FramedOrientation>().qw)) +
+ ((value.qx - getParameter(0).getClass<FramedOrientation>().qx) *
+ (value.qx - getParameter(0).getClass<FramedOrientation>().qx)) +
+ ((value.qy - getParameter(0).getClass<FramedOrientation>().qy) *
+ (value.qy - getParameter(0).getClass<FramedOrientation>().qy)) +
+ ((value.qz - getParameter(0).getClass<FramedOrientation>().qx) *
+ (value.qx - getParameter(0).getClass<FramedOrientation>().qz))) == 0);
if (type == VariantType::Pose)
- return (sqrt(((value.x - getParameter(0).getClass<Pose>().x) * (value.x - getParameter(0).getClass<Pose>().x)) +
- ((value.y - getParameter(0).getClass<Pose>().y) * (value.y - getParameter(0).getClass<Pose>().y)) +
- ((value.z - getParameter(0).getClass<Pose>().x) * (value.x - getParameter(0).getClass<Pose>().z))) +
- sqrt(((value.qw - getParameter(0).getClass<Pose>().qw) * (value.qw - getParameter(0).getClass<Pose>().qw)) +
- ((value.qx - getParameter(0).getClass<Pose>().qx) * (value.qx - getParameter(0).getClass<Pose>().qx)) +
- ((value.qy - getParameter(0).getClass<Pose>().qy) * (value.qy - getParameter(0).getClass<Pose>().qy)) +
- ((value.qz - getParameter(0).getClass<Pose>().qx) * (value.qx - getParameter(0).getClass<Pose>().qz))) == 0);
+ return (sqrt(((value.x - getParameter(0).getClass<Pose>().x) *
+ (value.x - getParameter(0).getClass<Pose>().x)) +
+ ((value.y - getParameter(0).getClass<Pose>().y) *
+ (value.y - getParameter(0).getClass<Pose>().y)) +
+ ((value.z - getParameter(0).getClass<Pose>().x) *
+ (value.x - getParameter(0).getClass<Pose>().z))) +
+ sqrt(((value.qw - getParameter(0).getClass<Pose>().qw) *
+ (value.qw - getParameter(0).getClass<Pose>().qw)) +
+ ((value.qx - getParameter(0).getClass<Pose>().qx) *
+ (value.qx - getParameter(0).getClass<Pose>().qx)) +
+ ((value.qy - getParameter(0).getClass<Pose>().qy) *
+ (value.qy - getParameter(0).getClass<Pose>().qy)) +
+ ((value.qz - getParameter(0).getClass<Pose>().qx) *
+ (value.qx - getParameter(0).getClass<Pose>().qz))) ==
+ 0);
return false;
}
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h b/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h
index e11a91fcf81a32ef21c656897c6104eae5e1ca34..9dcae32a94e429381390e870976799fed87f6a30 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckEqualsPoseWithTolerance.h
@@ -25,30 +25,42 @@
#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/observers/ConditionCheck.h>
+
#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx
{
- class ARMARXCORE_IMPORT_EXPORT ConditionCheckApproxPose :
- public ConditionCheck
+ class ARMARXCORE_IMPORT_EXPORT ConditionCheckApproxPose : public ConditionCheck
{
public:
ConditionCheckApproxPose()
{
setNumberParameters(3);
- addSupportedType(VariantType::FramedPosition, createParameterTypeList(2, VariantType::FramedPosition, VariantType::Float));
- addSupportedType(VariantType::FramedOrientation, createParameterTypeList(2, VariantType::FramedOrientation, VariantType::Float));
- addSupportedType(VariantType::Vector3, createParameterTypeList(2, VariantType::Vector3, VariantType::Float));
- addSupportedType(VariantType::Quaternion, createParameterTypeList(2, VariantType::Quaternion, VariantType::Float));
- addSupportedType(VariantType::FramedPose, createParameterTypeList(3, VariantType::FramedPose, VariantType::Float, VariantType::Float));
+ addSupportedType(
+ VariantType::FramedPosition,
+ createParameterTypeList(2, VariantType::FramedPosition, VariantType::Float));
+ addSupportedType(
+ VariantType::FramedOrientation,
+ createParameterTypeList(2, VariantType::FramedOrientation, VariantType::Float));
+ addSupportedType(VariantType::Vector3,
+ createParameterTypeList(2, VariantType::Vector3, VariantType::Float));
+ addSupportedType(
+ VariantType::Quaternion,
+ createParameterTypeList(2, VariantType::Quaternion, VariantType::Float));
+ addSupportedType(
+ VariantType::FramedPose,
+ createParameterTypeList(
+ 3, VariantType::FramedPose, VariantType::Float, VariantType::Float));
}
- ConditionCheck* clone() override
+ ConditionCheck*
+ clone() override
{
return new ConditionCheckApproxPose(*this);
}
- bool evaluate(const StringVariantMap& dataFields) override
+ bool
+ evaluate(const StringVariantMap& dataFields) override
{
if (dataFields.size() != 1)
{
@@ -61,17 +73,18 @@ namespace armarx
if (type == VariantType::Vector3)
{
- const Vector3Ptr& typedValue = value.getClass<Vector3>();
- const Vector3Ptr& param = getParameter(0).getClass<Vector3>();
+ const Vector3Ptr& typedValue = value.getClass<Vector3>();
+ const Vector3Ptr& param = getParameter(0).getClass<Vector3>();
return (sqrt(((typedValue->x - param->x) * (typedValue->x - param->x)) +
((typedValue->y - param->y) * (typedValue->y - param->y)) +
- ((typedValue->z - param->x) * (typedValue->x - param->z))) < getParameter(1).getFloat());
+ ((typedValue->z - param->x) * (typedValue->x - param->z))) <
+ getParameter(1).getFloat());
}
if (type == VariantType::Quaternion)
{
- const QuaternionPtr& typedValue = value.getClass<Quaternion>();
- const QuaternionPtr& param = getParameter(0).getClass<Quaternion>();
+ const QuaternionPtr& typedValue = value.getClass<Quaternion>();
+ const QuaternionPtr& param = getParameter(0).getClass<Quaternion>();
Eigen::Matrix3f diffRot = typedValue->toEigen() * param->toEigen().transpose();
Eigen::AngleAxisf aa(diffRot);
return fabs(aa.angle()) < getParameter(1).getFloat();
@@ -80,19 +93,20 @@ namespace armarx
if (type == VariantType::FramedPosition)
{
- const FramedPositionPtr& typedValue = value.getClass<FramedPosition>();
- const FramedPositionPtr& param = getParameter(0).getClass<FramedPosition>();
- return param->getFrame() == typedValue->getFrame()
- && (sqrt(((typedValue->x - param->x) * (typedValue->x - param->x)) +
- ((typedValue->y - param->y) * (typedValue->y - param->y)) +
- ((typedValue->z - param->x) * (typedValue->x - param->z))) < getParameter(1).getFloat());
+ const FramedPositionPtr& typedValue = value.getClass<FramedPosition>();
+ const FramedPositionPtr& param = getParameter(0).getClass<FramedPosition>();
+ return param->getFrame() == typedValue->getFrame() &&
+ (sqrt(((typedValue->x - param->x) * (typedValue->x - param->x)) +
+ ((typedValue->y - param->y) * (typedValue->y - param->y)) +
+ ((typedValue->z - param->x) * (typedValue->x - param->z))) <
+ getParameter(1).getFloat());
}
if (type == VariantType::FramedOrientation)
{
- const FramedOrientationPtr& typedValue = value.getClass<FramedOrientation>();
- const FramedOrientationPtr& param = getParameter(0).getClass<FramedOrientation>();
+ const FramedOrientationPtr& typedValue = value.getClass<FramedOrientation>();
+ const FramedOrientationPtr& param = getParameter(0).getClass<FramedOrientation>();
Eigen::Matrix3f diffRot = typedValue->toEigen() * param->toEigen().transpose();
Eigen::AngleAxisf aa(diffRot);
return fabs(aa.angle()) < getParameter(1).getFloat();
@@ -101,22 +115,24 @@ namespace armarx
if (type == VariantType::FramedPose)
{
- const PosePtr& typedValue = value.getClass<FramedPose>();
- const PosePtr& param = getParameter(0).getClass<FramedPose>();
- bool positionOk = (sqrt(((typedValue->position->x - param->position->x) * (typedValue->position->x - param->position->x)) +
- ((typedValue->position->y - param->position->y) * (typedValue->position->y - param->position->y)) +
- ((typedValue->position->z - param->position->x) * (typedValue->position->x - param->position->z)))
- < getParameter(1).getFloat());
-
- Eigen::Matrix3f diffRot = typedValue->toEigen().block<3, 3>(0, 0) * param->toEigen().block<3, 3>(0, 0).transpose();
+ const PosePtr& typedValue = value.getClass<FramedPose>();
+ const PosePtr& param = getParameter(0).getClass<FramedPose>();
+ bool positionOk = (sqrt(((typedValue->position->x - param->position->x) *
+ (typedValue->position->x - param->position->x)) +
+ ((typedValue->position->y - param->position->y) *
+ (typedValue->position->y - param->position->y)) +
+ ((typedValue->position->z - param->position->x) *
+ (typedValue->position->x - param->position->z))) <
+ getParameter(1).getFloat());
+
+ Eigen::Matrix3f diffRot = typedValue->toEigen().block<3, 3>(0, 0) *
+ param->toEigen().block<3, 3>(0, 0).transpose();
Eigen::AngleAxisf aa(diffRot);
- bool orientationOk =
- fabs(aa.angle()) < getParameter(2).getFloat();
+ bool orientationOk = fabs(aa.angle()) < getParameter(2).getFloat();
return positionOk && orientationOk;
}
return false;
}
};
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
index 9f1ee0943cdbc899358d2f074bac5151932e3703..873349566a4589d3c626ccf38fbbd48ef795dbed 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.cpp
@@ -25,7 +25,6 @@
#include <ArmarXCore/core/util/StringHelpers.h>
-
namespace armarx
{
@@ -33,22 +32,27 @@ namespace armarx
{
setNumberParameters(1);
addSupportedType(VariantType::Vector3, createParameterTypeList(1, VariantType::Float));
- addSupportedType(VariantType::FramedDirection, createParameterTypeList(1, VariantType::Float));
- addSupportedType(VariantType::LinkedDirection, createParameterTypeList(1, VariantType::Float));
-
+ addSupportedType(VariantType::FramedDirection,
+ createParameterTypeList(1, VariantType::Float));
+ addSupportedType(VariantType::LinkedDirection,
+ createParameterTypeList(1, VariantType::Float));
}
- ConditionCheck* ConditionCheckMagnitudeLarger::clone()
+ ConditionCheck*
+ ConditionCheckMagnitudeLarger::clone()
{
return new ConditionCheckMagnitudeLarger(*this);
}
- bool ConditionCheckMagnitudeLarger::evaluate(const StringVariantMap& dataFields)
+ bool
+ ConditionCheckMagnitudeLarger::evaluate(const StringVariantMap& dataFields)
{
if (dataFields.size() != 1)
{
- throw InvalidConditionException("Wrong number of datafields for condition magnitude larger: expected 1 actual: " + ValueToString(dataFields.size()));
+ throw InvalidConditionException(
+ "Wrong number of datafields for condition magnitude larger: expected 1 actual: " +
+ ValueToString(dataFields.size()));
}
const Variant& value = dataFields.begin()->second;
@@ -78,22 +82,24 @@ namespace armarx
return false;
}
-
ConditionCheckMagnitudeSmaller::ConditionCheckMagnitudeSmaller()
{
setNumberParameters(1);
addSupportedType(VariantType::Vector3, createParameterTypeList(1, VariantType::Float));
- addSupportedType(VariantType::FramedDirection, createParameterTypeList(1, VariantType::Float));
- addSupportedType(VariantType::LinkedDirection, createParameterTypeList(1, VariantType::Float));
-
+ addSupportedType(VariantType::FramedDirection,
+ createParameterTypeList(1, VariantType::Float));
+ addSupportedType(VariantType::LinkedDirection,
+ createParameterTypeList(1, VariantType::Float));
}
- ConditionCheck* ConditionCheckMagnitudeSmaller::clone()
+ ConditionCheck*
+ ConditionCheckMagnitudeSmaller::clone()
{
return new ConditionCheckMagnitudeSmaller(*this);
}
- bool ConditionCheckMagnitudeSmaller::evaluate(const StringVariantMap& dataFields)
+ bool
+ ConditionCheckMagnitudeSmaller::evaluate(const StringVariantMap& dataFields)
{
if (dataFields.size() != 1)
@@ -131,18 +137,22 @@ namespace armarx
ConditionCheckMagnitudeInRange::ConditionCheckMagnitudeInRange()
{
setNumberParameters(2);
- addSupportedType(VariantType::Vector3, createParameterTypeList(2, VariantType::Float, VariantType::Float));
- addSupportedType(VariantType::FramedDirection, createParameterTypeList(2, VariantType::Float, VariantType::Float));
- addSupportedType(VariantType::LinkedDirection, createParameterTypeList(2, VariantType::Float, VariantType::Float));
-
+ addSupportedType(VariantType::Vector3,
+ createParameterTypeList(2, VariantType::Float, VariantType::Float));
+ addSupportedType(VariantType::FramedDirection,
+ createParameterTypeList(2, VariantType::Float, VariantType::Float));
+ addSupportedType(VariantType::LinkedDirection,
+ createParameterTypeList(2, VariantType::Float, VariantType::Float));
}
- ConditionCheck* ConditionCheckMagnitudeInRange::clone()
+ ConditionCheck*
+ ConditionCheckMagnitudeInRange::clone()
{
return new ConditionCheckMagnitudeInRange(*this);
}
- bool ConditionCheckMagnitudeInRange::evaluate(const StringVariantMap& dataFields)
+ bool
+ ConditionCheckMagnitudeInRange::evaluate(const StringVariantMap& dataFields)
{
if (dataFields.size() != 1)
{
@@ -156,24 +166,27 @@ namespace armarx
if (type == VariantType::Vector3)
{
Eigen::Vector3f vec = value.getClass<Vector3>()->toEigen();
- return ((vec).norm() > getParameter(0).getFloat()) && ((vec).norm() < getParameter(1).getFloat());
+ return ((vec).norm() > getParameter(0).getFloat()) &&
+ ((vec).norm() < getParameter(1).getFloat());
}
if (type == VariantType::FramedDirection)
{
// FramedDirectionPtr fV1 = value.getClass<FramedDirection>();
Eigen::Vector3f vec = value.getClass<FramedDirection>()->toEigen();
- return ((vec).norm() > getParameter(0).getFloat()) && ((vec).norm() < getParameter(1).getFloat());
+ return ((vec).norm() > getParameter(0).getFloat()) &&
+ ((vec).norm() < getParameter(1).getFloat());
}
if (type == VariantType::LinkedDirection)
{
// LinkedDirectionPtr lV1 = value.getClass<LinkedDirection>();
Eigen::Vector3f vec = value.getClass<LinkedDirection>()->toEigen();
- return ((vec).norm() > getParameter(0).getFloat()) && ((vec).norm() < getParameter(1).getFloat());
+ return ((vec).norm() > getParameter(0).getFloat()) &&
+ ((vec).norm() < getParameter(1).getFloat());
}
return false;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
index 6c936c777acfbdd0b1d066ca0f5b2fcbbfef4072..a762d1f5200d4644d6d7245f96abd565feda2dd5 100644
--- a/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
+++ b/source/RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h
@@ -23,12 +23,11 @@
*/
#pragma once
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/LinkedPose.h>
-
-#include <ArmarXCore/observers/ConditionCheck.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/observers/ConditionCheck.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/LinkedPose.h>
namespace armarx
{
@@ -60,4 +59,4 @@ namespace armarx
bool evaluate(const StringVariantMap& dataFields) override;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/math/ColorUtils.h b/source/RobotAPI/libraries/core/math/ColorUtils.h
index ab333c362da42a5fb877f7a7a91be66a68098ed7..7b4f79a1c900a9b06715f5f33bead04a3b137486 100644
--- a/source/RobotAPI/libraries/core/math/ColorUtils.h
+++ b/source/RobotAPI/libraries/core/math/ColorUtils.h
@@ -24,21 +24,24 @@
#pragma once
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+
#include "MathUtils.h"
+
namespace armarx::colorutils
{
- DrawColor24Bit HsvToRgb(const HsvColor& in)
+ DrawColor24Bit
+ HsvToRgb(const HsvColor& in)
{
- double hh, p, q, t, ff;
- long i;
+ double hh, p, q, t, ff;
+ long i;
double r, g, b;
- if (in.s <= 0.0) // < is bogus, just shuts up warnings
+ if (in.s <= 0.0) // < is bogus, just shuts up warnings
{
r = in.v;
g = in.v;
b = in.v;
- return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
+ return DrawColor24Bit{(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
}
hh = in.h;
if (hh >= 360.0)
@@ -88,24 +91,25 @@ namespace armarx::colorutils
break;
}
- return DrawColor24Bit {(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
+ return DrawColor24Bit{(Ice::Byte)(r * 255), (Ice::Byte)(g * 255), (Ice::Byte)(b * 255)};
}
- HsvColor RgbToHsv(const DrawColor24Bit& in)
+ HsvColor
+ RgbToHsv(const DrawColor24Bit& in)
{
double r = 0.00392156862 * in.r;
double g = 0.00392156862 * in.g;
double b = 0.00392156862 * in.b;
- HsvColor out;
- double min, max, delta;
+ HsvColor out;
+ double min, max, delta;
min = r < g ? r : g;
- min = min < b ? min : b;
+ min = min < b ? min : b;
max = r > g ? r : g;
- max = max > b ? max : b;
+ max = max > b ? max : b;
- out.v = max; // v
+ out.v = max; // v
delta = max - min;
if (delta < 0.00001)
{
@@ -113,32 +117,32 @@ namespace armarx::colorutils
out.h = 0; // undefined, maybe nan?
return out;
}
- if (max > 0.0) // NOTE: if Max is == 0, this divide would cause a crash
+ if (max > 0.0) // NOTE: if Max is == 0, this divide would cause a crash
{
- out.s = (delta / max); // s
+ out.s = (delta / max); // s
}
else
{
// if max is 0, then r = g = b = 0
// s = 0, h is undefined
out.s = 0.0;
- out.h = 0; // its now undefined
+ out.h = 0; // its now undefined
return out;
}
- if (r >= max) // > is bogus, just keeps compilor happy
+ if (r >= max) // > is bogus, just keeps compilor happy
{
- out.h = (g - b) / delta; // between yellow & magenta
+ out.h = (g - b) / delta; // between yellow & magenta
}
else if (g >= max)
{
- out.h = 2.0 + (b - r) / delta; // between cyan & yellow
+ out.h = 2.0 + (b - r) / delta; // between cyan & yellow
}
else
{
- out.h = 4.0 + (r - g) / delta; // between magenta & cyan
+ out.h = 4.0 + (r - g) / delta; // between magenta & cyan
}
- out.h *= 60.0; // degrees
+ out.h *= 60.0; // degrees
if (out.h < 0.0)
{
@@ -146,8 +150,6 @@ namespace armarx::colorutils
}
return out;
-
-
}
/**
@@ -155,26 +157,28 @@ namespace armarx::colorutils
* @param percentage value between 0..1
* @return color on a heatmap corresponding to parameter. 0 -> blue, 1 -> red. Color has full (255) saturation and value.
*/
- HsvColor HeatMapColor(float percentage)
+ HsvColor
+ HeatMapColor(float percentage)
{
percentage = math::MathUtils::LimitMinMax(0.0f, 1.0f, percentage);
- return HsvColor {((1.0f - percentage) * 240.f), 1.0f, 1.0f};
+ return HsvColor{((1.0f - percentage) * 240.f), 1.0f, 1.0f};
}
- DrawColor24Bit HeatMapRGBColor(float percentage)
+ DrawColor24Bit
+ HeatMapRGBColor(float percentage)
{
return HsvToRgb(HeatMapColor(percentage));
}
- DrawColor HeatMapRGBAColor(float percentage)
+ DrawColor
+ HeatMapRGBAColor(float percentage)
{
auto color = HsvToRgb(HeatMapColor(percentage));
- return DrawColor {0.0039215686f * color.r, //divide by 255
- 0.0039215686f * color.g,
- 0.0039215686f * color.b,
- 1.0
- };
+ return DrawColor{0.0039215686f * color.r, //divide by 255
+ 0.0039215686f * color.g,
+ 0.0039215686f * color.b,
+ 1.0};
}
-}
+} // namespace armarx::colorutils
diff --git a/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp b/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp
index eecf1a0b4c6a8f7d607a02c1e932f70d7223953e..3847744b2d84324adb6e1443e9c863df44771ca3 100644
--- a/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp
+++ b/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.cpp
@@ -22,27 +22,31 @@
*/
#include "LinearizeAngularTrajectory.h"
+
#include "MathUtils.h"
namespace armarx::math
{
- LinearizeAngularTrajectory::LinearizeAngularTrajectory(float initialLinearValue)
- : linearValue(initialLinearValue)
+ LinearizeAngularTrajectory::LinearizeAngularTrajectory(float initialLinearValue) :
+ linearValue(initialLinearValue)
{
}
- float LinearizeAngularTrajectory::update(float angle)
+ float
+ LinearizeAngularTrajectory::update(float angle)
{
linearValue = linearValue + MathUtils::angleModPI(angle - linearValue);
return linearValue;
}
- float LinearizeAngularTrajectory::getLinearValue()
+ float
+ LinearizeAngularTrajectory::getLinearValue()
{
return linearValue;
}
- std::vector<float> LinearizeAngularTrajectory::Linearize(const std::vector<float>& data)
+ std::vector<float>
+ LinearizeAngularTrajectory::Linearize(const std::vector<float>& data)
{
std::vector<float> result;
result.reserve(data.size());
@@ -58,7 +62,8 @@ namespace armarx::math
return result;
}
- void LinearizeAngularTrajectory::LinearizeRef(std::vector<float>& data)
+ void
+ LinearizeAngularTrajectory::LinearizeRef(std::vector<float>& data)
{
if (data.size() == 0)
{
@@ -71,7 +76,8 @@ namespace armarx::math
}
}
- std::vector<float> LinearizeAngularTrajectory::Angularize(const std::vector<float>& data, float center)
+ std::vector<float>
+ LinearizeAngularTrajectory::Angularize(const std::vector<float>& data, float center)
{
std::vector<float> result;
result.reserve(data.size());
@@ -82,12 +88,12 @@ namespace armarx::math
return result;
}
- void LinearizeAngularTrajectory::AngularizeRef(std::vector<float>& data, float center)
+ void
+ LinearizeAngularTrajectory::AngularizeRef(std::vector<float>& data, float center)
{
for (size_t i = 0; i < data.size(); i++)
{
data.at(i) = MathUtils::angleModX(data.at(i), center);
}
-
}
-}
+} // namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h b/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h
index f50d963282817427b1d72133a9d01f8641a9f483..396a8eb38ea3fa936d07a1c2eee2489d6fe6ce0f 100644
--- a/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h
+++ b/source/RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h
@@ -23,8 +23,8 @@
#pragma once
-#include <vector>
#include <memory>
+#include <vector>
namespace armarx::math
{
@@ -47,4 +47,4 @@ namespace armarx::math
private:
float linearValue;
};
-}
+} // namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/math/MathUtils.h b/source/RobotAPI/libraries/core/math/MathUtils.h
index 4944430b100596e3336d6feeb2b45870b5258d8c..12cfa791367e5f55f3e09364f76b0216bbf7a131 100644
--- a/source/RobotAPI/libraries/core/math/MathUtils.h
+++ b/source/RobotAPI/libraries/core/math/MathUtils.h
@@ -22,45 +22,61 @@
#pragma once
-#include <Eigen/Core>
-#include <vector>
#include <math.h>
+#include <vector>
+
+#include <Eigen/Core>
+
namespace armarx::math
{
class MathUtils
{
public:
- static int Sign(double value)
+ static int
+ Sign(double value)
{
return value >= 0 ? 1 : -1;
}
- static int LimitMinMax(int min, int max, int value)
+ static int
+ LimitMinMax(int min, int max, int value)
{
return value < min ? min : (value > max ? max : value);
}
- static float LimitMinMax(float min, float max, float value)
+
+ static float
+ LimitMinMax(float min, float max, float value)
{
return value < min ? min : (value > max ? max : value);
}
- static double LimitMinMax(double min, double max, double value)
+
+ static double
+ LimitMinMax(double min, double max, double value)
{
return value < min ? min : (value > max ? max : value);
}
- static Eigen::Vector3f LimitMinMax(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& value)
+
+ static Eigen::Vector3f
+ LimitMinMax(const Eigen::Vector3f& min,
+ const Eigen::Vector3f& max,
+ const Eigen::Vector3f& value)
{
- return Eigen::Vector3f(LimitMinMax(min(0), max(0), value(0)), LimitMinMax(min(1), max(1), value(1)), LimitMinMax(min(2), max(2), value(2)));
+ return Eigen::Vector3f(LimitMinMax(min(0), max(0), value(0)),
+ LimitMinMax(min(1), max(1), value(1)),
+ LimitMinMax(min(2), max(2), value(2)));
}
- static double LimitTo(double value, double absThreshold)
+ static double
+ LimitTo(double value, double absThreshold)
{
return LimitMinMax(-absThreshold, absThreshold, value);
//int sign = (value >= 0) ? 1 : -1;
//return sign * std::min<double>(fabs(value), absThreshold);
}
- static Eigen::Vector3f LimitTo(const Eigen::Vector3f& val, float maxNorm)
+ static Eigen::Vector3f
+ LimitTo(const Eigen::Vector3f& val, float maxNorm)
{
float norm = val.norm();
if (norm > maxNorm)
@@ -70,24 +86,35 @@ namespace armarx::math
return val;
}
- static bool CheckMinMax(int min, int max, int value)
+ static bool
+ CheckMinMax(int min, int max, int value)
{
return value >= min && value <= max;
}
- static bool CheckMinMax(float min, float max, float value)
+
+ static bool
+ CheckMinMax(float min, float max, float value)
{
return value >= min && value <= max;
}
- static bool CheckMinMax(double min, double max, double value)
+
+ static bool
+ CheckMinMax(double min, double max, double value)
{
return value >= min && value <= max;
}
- static bool CheckMinMax(const Eigen::Vector3f& min, const Eigen::Vector3f& max, const Eigen::Vector3f& value)
+
+ static bool
+ CheckMinMax(const Eigen::Vector3f& min,
+ const Eigen::Vector3f& max,
+ const Eigen::Vector3f& value)
{
- return CheckMinMax(min(0), max(0), value(0)) && CheckMinMax(min(1), max(1), value(1)) && CheckMinMax(min(2), max(2), value(2));
+ return CheckMinMax(min(0), max(0), value(0)) && CheckMinMax(min(1), max(1), value(1)) &&
+ CheckMinMax(min(2), max(2), value(2));
}
- static std::vector<float> VectorSubtract(const std::vector<float>& v1, const std::vector<float>& v2)
+ static std::vector<float>
+ VectorSubtract(const std::vector<float>& v1, const std::vector<float>& v2)
{
std::vector<float> result;
@@ -98,7 +125,9 @@ namespace armarx::math
return result;
}
- static std::vector<float> VectorAbsDiff(const std::vector<float>& v1, const std::vector<float>& v2)
+
+ static std::vector<float>
+ VectorAbsDiff(const std::vector<float>& v1, const std::vector<float>& v2)
{
std::vector<float> result;
@@ -110,7 +139,8 @@ namespace armarx::math
return result;
}
- static float VectorMax(const std::vector<float>& vec)
+ static float
+ VectorMax(const std::vector<float>& vec)
{
float max = vec.at(0);
@@ -122,7 +152,8 @@ namespace armarx::math
return max;
}
- static float fmod(float value, float boundLow, float boundHigh)
+ static float
+ fmod(float value, float boundLow, float boundHigh)
{
value = std::fmod(value - boundLow, boundHigh - boundLow) + boundLow;
if (value < boundLow)
@@ -132,51 +163,59 @@ namespace armarx::math
return value;
}
- static float angleMod2PI(float value)
+ static float
+ angleMod2PI(float value)
{
return fmod(value, 0, 2 * M_PI);
}
- static float angleModPI(float value)
+ static float
+ angleModPI(float value)
{
return angleMod2PI(value + M_PI) - M_PI;
}
- static float angleModX(float value, float center)
+ static float
+ angleModX(float value, float center)
{
return angleMod2PI(value + M_PI - center) - M_PI + center;
}
- static float Lerp(float a, float b, float f)
+ static float
+ Lerp(float a, float b, float f)
{
return a * (1 - f) + b * f;
}
- static float LerpClamp(float a, float b, float f)
+ static float
+ LerpClamp(float a, float b, float f)
{
return Lerp(a, b, LimitMinMax(0.f, 1.f, f));
}
- static float ILerp(float a, float b, float f)
+ static float
+ ILerp(float a, float b, float f)
{
return (f - a) / (b - a);
}
- static float ILerpClamp(float a, float b, float f)
+ static float
+ ILerpClamp(float a, float b, float f)
{
return LimitMinMax(0.f, 1.f, ILerp(a, b, f));
}
- static float AngleLerp(float a, float b, float f)
+ static float
+ AngleLerp(float a, float b, float f)
{
b = fmod(b, a - M_PI, a + M_PI);
return Lerp(a, b, f);
}
- static float AngleDelta(float angle1, float angle2)
+ static float
+ AngleDelta(float angle1, float angle2)
{
return angleModPI(angle2 - angle1);
}
-
};
-}
+} // namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/math/MatrixHelpers.h b/source/RobotAPI/libraries/core/math/MatrixHelpers.h
index 80b8fba700950960820621ca13b0db32a5e5314c..3658fd21355503d0022f59da89d7d6defc1f8cfb 100644
--- a/source/RobotAPI/libraries/core/math/MatrixHelpers.h
+++ b/source/RobotAPI/libraries/core/math/MatrixHelpers.h
@@ -23,6 +23,7 @@
#pragma once
#include <math.h>
+
#include <Eigen/Eigen>
namespace armarx::math
@@ -30,7 +31,8 @@ namespace armarx::math
class MatrixHelpers
{
public:
- static void SetRowToValue(Eigen::MatrixXf& matrix, int rowNr, float value)
+ static void
+ SetRowToValue(Eigen::MatrixXf& matrix, int rowNr, float value)
{
for (int i = 0; i < matrix.cols(); i++)
{
@@ -38,7 +40,8 @@ namespace armarx::math
}
}
- static Eigen::Vector3f CalculateCog3D(const Eigen::MatrixXf& points)
+ static Eigen::Vector3f
+ CalculateCog3D(const Eigen::MatrixXf& points)
{
Eigen::Vector3f sum(0, 0, 0);
@@ -50,7 +53,8 @@ namespace armarx::math
return sum / points.cols();
}
- static Eigen::MatrixXf SubtractVectorFromAllColumns3D(const Eigen::MatrixXf& points, const Eigen::Vector3f& vec)
+ static Eigen::MatrixXf
+ SubtractVectorFromAllColumns3D(const Eigen::MatrixXf& points, const Eigen::Vector3f& vec)
{
Eigen::MatrixXf matrix(3, points.cols());
@@ -61,8 +65,5 @@ namespace armarx::math
return matrix;
}
-
};
-}
-
-
+} // namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/math/SVD.h b/source/RobotAPI/libraries/core/math/SVD.h
index 4160bc058da7bc25ad0466fb484e07c9999d40db..7372163fc78a817b156fc90cbc2c718dd3baf621 100644
--- a/source/RobotAPI/libraries/core/math/SVD.h
+++ b/source/RobotAPI/libraries/core/math/SVD.h
@@ -23,6 +23,7 @@
#pragma once
#include <math.h>
+
#include <Eigen/Eigen>
namespace armarx::math
@@ -31,28 +32,29 @@ namespace armarx::math
{
private:
Eigen::JacobiSVD<Eigen::MatrixXf> svd;
+
public:
Eigen::MatrixXf matrixU;
Eigen::MatrixXf matrixV;
Eigen::VectorXf singularValues;
- SVD(Eigen::MatrixXf matrix)
- : svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV)
+
+ SVD(Eigen::MatrixXf matrix) : svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV)
{
matrixU = svd.matrixU();
matrixV = svd.matrixV();
singularValues = svd.singularValues();
}
- Eigen::Vector3f getLeftSingularVector3D(int nr)
+ Eigen::Vector3f
+ getLeftSingularVector3D(int nr)
{
return matrixU.block<3, 1>(0, nr);
}
- float getLeftSingularValue(int nr)
+
+ float
+ getLeftSingularValue(int nr)
{
return singularValues(nr);
}
-
};
-}
-
-
+} // namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h b/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
index 17f9c0a7082022ee57f851458ab03e9962bb3c8e..7bc207d261212588d2b834012e2116802f4a3371 100644
--- a/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
+++ b/source/RobotAPI/libraries/core/math/SlidingWindowVectorMedian.h
@@ -22,13 +22,14 @@
#pragma once
-#include "StatUtils.h"
-
#include <math.h>
+
#include <vector>
#include <ArmarXCore/core/exceptions/Exception.h>
+#include "StatUtils.h"
+
namespace armarx::math
{
class SlidingWindowVectorMedian;
@@ -44,20 +45,22 @@ namespace armarx::math
bool fullCycle;
public:
- SlidingWindowVectorMedian(size_t vectorSize, size_t windowSize)
- : windowSize(windowSize),
- vectorSize(vectorSize),
- data(vectorSize * windowSize, 0), // initialize all data to 0
- currentIndex(0),
- fullCycle(false)
+ SlidingWindowVectorMedian(size_t vectorSize, size_t windowSize) :
+ windowSize(windowSize),
+ vectorSize(vectorSize),
+ data(vectorSize * windowSize, 0), // initialize all data to 0
+ currentIndex(0),
+ fullCycle(false)
{
}
- void addEntry(const std::vector<float>& entry)
+ void
+ addEntry(const std::vector<float>& entry)
{
if (entry.size() != vectorSize)
{
- throw LocalException("Vector of wrong size added. Execting: ") << vectorSize << "; Actual: " << entry.size();
+ throw LocalException("Vector of wrong size added. Execting: ")
+ << vectorSize << "; Actual: " << entry.size();
}
for (size_t i = 0; i < entry.size(); i++)
@@ -69,7 +72,8 @@ namespace armarx::math
fullCycle = fullCycle || currentIndex == 0;
}
- std::vector<float> getMedian()
+ std::vector<float>
+ getMedian()
{
std::vector<float> median;
@@ -88,8 +92,5 @@ namespace armarx::math
return median;
}
-
};
-}
-
-
+} // namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/math/StatUtils.h b/source/RobotAPI/libraries/core/math/StatUtils.h
index 679878188285c3ef977746d7f7e1431ea7e00319..a45775c3b3d5802dd4b1a4464eff20647d961cda 100644
--- a/source/RobotAPI/libraries/core/math/StatUtils.h
+++ b/source/RobotAPI/libraries/core/math/StatUtils.h
@@ -22,17 +22,18 @@
#pragma once
-#include <ArmarXCore/core/exceptions/LocalException.h>
-
#include <cmath>
#include <vector>
+#include <ArmarXCore/core/exceptions/LocalException.h>
+
namespace armarx::math
{
class StatUtils
{
public:
- static float GetPercentile(const std::vector<float>& sortedData, float percentile)
+ static float
+ GetPercentile(const std::vector<float>& sortedData, float percentile)
{
if (sortedData.size() == 0)
{
@@ -51,7 +52,9 @@ namespace armarx::math
return sortedData.at(index) * (1 - f) + sortedData.at(index + 1) * f;
}
- static std::vector<float> GetPercentiles(const std::vector<float>& sortedData, int percentiles)
+
+ static std::vector<float>
+ GetPercentiles(const std::vector<float>& sortedData, int percentiles)
{
std::vector<float> result;
result.push_back(sortedData.at(0));
@@ -64,11 +67,11 @@ namespace armarx::math
result.push_back(sortedData.at(sortedData.size() - 1));
return result;
}
- static float GetMedian(const std::vector<float>& sortedData)
+
+ static float
+ GetMedian(const std::vector<float>& sortedData)
{
return GetPercentile(sortedData, 0.5f);
}
};
-}
-
-
+} // namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp b/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp
index 6b1607730ab21a9ab904df146742609ae50ede76..4cef37663e90a4885e0bb09390e05d27e0c05b2b 100644
--- a/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp
+++ b/source/RobotAPI/libraries/core/math/TimeSeriesUtils.cpp
@@ -21,17 +21,22 @@
* GNU General Public License
*/
-#include "MathUtils.h"
#include "TimeSeriesUtils.h"
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "MathUtils.h"
+
namespace armarx::math
{
TimeSeriesUtils::TimeSeriesUtils()
{
}
- std::vector<float> TimeSeriesUtils::Resample(const std::vector<float>& timestamps, const std::vector<float>& data, const std::vector<float>& newTimestamps)
+ std::vector<float>
+ TimeSeriesUtils::Resample(const std::vector<float>& timestamps,
+ const std::vector<float>& data,
+ const std::vector<float>& newTimestamps)
{
ARMARX_CHECK_EQUAL(data.size(), timestamps.size());
ARMARX_CHECK_EQUAL(data.size(), newTimestamps.size());
@@ -57,7 +62,8 @@ namespace armarx::math
{
i++;
}
- float f = math::MathUtils::ILerp(timestamps.at(i), timestamps.at(i + 1), newTimestamps.at(j));
+ float f =
+ math::MathUtils::ILerp(timestamps.at(i), timestamps.at(i + 1), newTimestamps.at(j));
result.push_back(math::MathUtils::LerpClamp(data.at(i), data.at(i + 1), f));
j++;
}
@@ -65,7 +71,10 @@ namespace armarx::math
return result;
}
- std::vector<float> TimeSeriesUtils::ApplyFilter(const std::vector<float>& data, const std::vector<float>& filter, BorderMode mode)
+ std::vector<float>
+ TimeSeriesUtils::ApplyFilter(const std::vector<float>& data,
+ const std::vector<float>& filter,
+ BorderMode mode)
{
std::vector<float> result;
size_t start = filter.size() / 2;
@@ -92,14 +101,18 @@ namespace armarx::math
return result;
}
-
- std::vector<float> TimeSeriesUtils::ApplyGaussianFilter(const std::vector<float>& data, float sigma, float sampleTime, BorderMode mode)
+ std::vector<float>
+ TimeSeriesUtils::ApplyGaussianFilter(const std::vector<float>& data,
+ float sigma,
+ float sampleTime,
+ BorderMode mode)
{
std::vector<float> filter = CreateGaussianFilter(sigma, sampleTime);
return ApplyFilter(data, filter, mode);
}
- std::vector<float> TimeSeriesUtils::CreateGaussianFilter(const float sigma, float sampleTime, float truncate)
+ std::vector<float>
+ TimeSeriesUtils::CreateGaussianFilter(const float sigma, float sampleTime, float truncate)
{
std::vector<float> filter;
int range = (int)(truncate * sigma / sampleTime);
@@ -111,7 +124,8 @@ namespace armarx::math
return filter;
}
- std::vector<float> TimeSeriesUtils::MakeTimestamps(float start, float end, size_t count)
+ std::vector<float>
+ TimeSeriesUtils::MakeTimestamps(float start, float end, size_t count)
{
std::vector<float> result;
for (size_t i = 0; i < count; i++)
@@ -120,4 +134,4 @@ namespace armarx::math
}
return result;
}
-}
+} // namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/math/TimeSeriesUtils.h b/source/RobotAPI/libraries/core/math/TimeSeriesUtils.h
index d56d2f656e3ba5fb8a892ac87d19690948a378e9..bfe07017b28c11838832b2a7914db51c3503aec9 100644
--- a/source/RobotAPI/libraries/core/math/TimeSeriesUtils.h
+++ b/source/RobotAPI/libraries/core/math/TimeSeriesUtils.h
@@ -24,13 +24,13 @@
#pragma once
#include <memory>
+#include <vector>
namespace armarx::math
{
class TimeSeriesUtils;
using TimeSeriesUtilsPtr = std::shared_ptr<TimeSeriesUtils>;
-
class TimeSeriesUtils
{
public:
@@ -40,14 +40,20 @@ namespace armarx::math
};
TimeSeriesUtils();
- static std::vector<float> Resample(const std::vector<float>& timestamps, const std::vector<float>& data, const std::vector<float>& newTimestamps);
- static std::vector<float> ApplyFilter(const std::vector<float>& data, const std::vector<float>& filter, BorderMode mode);
- static std::vector<float> ApplyGaussianFilter(const std::vector<float>& data, float sigma, float sampleTime, BorderMode mode);
- static std::vector<float> CreateGaussianFilter(const float sigma, float sampleTime, float truncate = 4);
+ static std::vector<float> Resample(const std::vector<float>& timestamps,
+ const std::vector<float>& data,
+ const std::vector<float>& newTimestamps);
+ static std::vector<float> ApplyFilter(const std::vector<float>& data,
+ const std::vector<float>& filter,
+ BorderMode mode);
+ static std::vector<float> ApplyGaussianFilter(const std::vector<float>& data,
+ float sigma,
+ float sampleTime,
+ BorderMode mode);
+ static std::vector<float>
+ CreateGaussianFilter(const float sigma, float sampleTime, float truncate = 4);
static std::vector<float> MakeTimestamps(float start, float end, size_t count);
private:
};
-}
-
-
+} // namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/math/Trigonometry.h b/source/RobotAPI/libraries/core/math/Trigonometry.h
index 1fd7ad174b75a71328ce957c6a7aa827729f9e8f..dbc3e378d464a0b7bb1af3cba26b2c16f4080dc6 100644
--- a/source/RobotAPI/libraries/core/math/Trigonometry.h
+++ b/source/RobotAPI/libraries/core/math/Trigonometry.h
@@ -29,28 +29,34 @@ namespace armarx::math
class Trigonometry
{
public:
- static float Deg2RadF(const float angle)
+ static float
+ Deg2RadF(const float angle)
{
return angle / 180.0f * M_PI;
}
- static double Deg2RadD(const double angle)
+
+ static double
+ Deg2RadD(const double angle)
{
return angle / 180.0 * M_PI;
}
- static float Rad2DegF(const float rad)
+
+ static float
+ Rad2DegF(const float rad)
{
return rad / M_PI * 180.0f;
}
- static double Rad2DegD(const float rad)
+
+ static double
+ Rad2DegD(const float rad)
{
return rad / M_PI * 180.0;
}
- static double GetAngleFromVectorXY(const Eigen::Vector3f& vector)
+ static double
+ GetAngleFromVectorXY(const Eigen::Vector3f& vector)
{
return atan2(vector(1), vector(0));
}
};
-}
-
-
+} // namespace armarx::math
diff --git a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
index 569afc8d18e400dac922e9793b8ba0dc256e1cc5..4a0aa2f6a9b3115e3649e66f3668a3b619f79a1c 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MatrixFilters.h
@@ -22,20 +22,19 @@
#pragma once
-#include <RobotAPI/interface/observers/ObserverFilters.h>
+#include <algorithm>
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/filters/DatafieldFilter.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
-#include <ArmarXCore/core/logging/Logging.h>
-#include <algorithm>
+#include <RobotAPI/interface/observers/ObserverFilters.h>
namespace armarx::filters
{
- class MatrixMaxFilter :
- public MatrixMaxFilterBase,
- public DatafieldFilter
+ class MatrixMaxFilter : public MatrixMaxFilterBase, public DatafieldFilter
{
public:
MatrixMaxFilter()
@@ -43,7 +42,8 @@ namespace armarx::filters
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current&) const override
+ VariantBasePtr
+ calculate(const Ice::Current&) const override
{
std::unique_lock lock(historyMutex);
@@ -56,7 +56,9 @@ namespace armarx::filters
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
return new Variant(matrix->toEigen().maxCoeff());
}
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+
+ ParameterTypeList
+ getSupportedTypes(const Ice::Current&) const override
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
@@ -64,9 +66,7 @@ namespace armarx::filters
}
};
- class MatrixMinFilter :
- public MatrixMinFilterBase,
- public DatafieldFilter
+ class MatrixMinFilter : public MatrixMinFilterBase, public DatafieldFilter
{
public:
MatrixMinFilter()
@@ -74,7 +74,8 @@ namespace armarx::filters
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current&) const override
+ VariantBasePtr
+ calculate(const Ice::Current&) const override
{
std::unique_lock lock(historyMutex);
@@ -87,7 +88,9 @@ namespace armarx::filters
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
return new Variant(matrix->toEigen().minCoeff());
}
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+
+ ParameterTypeList
+ getSupportedTypes(const Ice::Current&) const override
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
@@ -95,9 +98,7 @@ namespace armarx::filters
}
};
- class MatrixAvgFilter :
- public MatrixAvgFilterBase,
- public DatafieldFilter
+ class MatrixAvgFilter : public MatrixAvgFilterBase, public DatafieldFilter
{
public:
MatrixAvgFilter()
@@ -105,7 +106,8 @@ namespace armarx::filters
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current&) const override
+ VariantBasePtr
+ calculate(const Ice::Current&) const override
{
std::unique_lock lock(historyMutex);
@@ -118,7 +120,9 @@ namespace armarx::filters
MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
return new Variant(matrix->toEigen().mean());
}
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+
+ ParameterTypeList
+ getSupportedTypes(const Ice::Current&) const override
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
@@ -126,22 +130,22 @@ namespace armarx::filters
}
};
- class MatrixPercentileFilter :
- public MatrixPercentileFilterBase,
- public DatafieldFilter
+ class MatrixPercentileFilter : public MatrixPercentileFilterBase, public DatafieldFilter
{
public:
MatrixPercentileFilter()
{
this->windowFilterSize = 1;
}
+
MatrixPercentileFilter(float percentile)
{
this->percentile = percentile;
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current&) const override
+ VariantBasePtr
+ calculate(const Ice::Current&) const override
{
std::unique_lock lock(historyMutex);
@@ -156,14 +160,17 @@ namespace armarx::filters
std::sort(vector.begin(), vector.end());
return new Variant(GetPercentile(vector, percentile));
}
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+
+ ParameterTypeList
+ getSupportedTypes(const Ice::Current&) const override
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
return result;
}
- static float GetPercentile(const std::vector<float>& sortedData, float percentile)
+ static float
+ GetPercentile(const std::vector<float>& sortedData, float percentile)
{
if (sortedData.size() == 0)
{
@@ -184,9 +191,7 @@ namespace armarx::filters
}
};
- class MatrixPercentilesFilter :
- public MatrixPercentilesFilterBase,
- public DatafieldFilter
+ class MatrixPercentilesFilter : public MatrixPercentilesFilterBase, public DatafieldFilter
{
public:
MatrixPercentilesFilter()
@@ -194,13 +199,15 @@ namespace armarx::filters
this->windowFilterSize = 1;
this->percentiles = 10;
}
+
MatrixPercentilesFilter(int percentiles)
{
this->percentiles = percentiles;
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current&) const override
+ VariantBasePtr
+ calculate(const Ice::Current&) const override
{
std::unique_lock lock(historyMutex);
@@ -219,13 +226,16 @@ namespace armarx::filters
for (int i = 1; i < percentiles; i++)
{
- result.push_back(MatrixPercentileFilter::GetPercentile(vector, 1.f / percentiles * i));
+ result.push_back(
+ MatrixPercentileFilter::GetPercentile(vector, 1.f / percentiles * i));
}
result.push_back(vector.at(vector.size() - 1));
return new Variant(new MatrixFloat(1, result.size(), result));
}
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+
+ ParameterTypeList
+ getSupportedTypes(const Ice::Current&) const override
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
@@ -242,6 +252,7 @@ namespace armarx::filters
{
this->windowFilterSize = 1;
}
+
MatrixCumulativeFrequencyFilter(float min, float max, int bins)
{
this->min = min;
@@ -249,7 +260,9 @@ namespace armarx::filters
this->bins = bins;
this->windowFilterSize = 1;
}
- VariantBasePtr calculate(const Ice::Current&) const override
+
+ VariantBasePtr
+ calculate(const Ice::Current&) const override
{
std::unique_lock lock(historyMutex);
@@ -272,13 +285,17 @@ namespace armarx::filters
return new Variant(new MatrixFloat(1, resultF.size(), resultF));
}
- ParameterTypeList getSupportedTypes(const Ice::Current&) const override
+
+ ParameterTypeList
+ getSupportedTypes(const Ice::Current&) const override
{
ParameterTypeList result;
result.push_back(VariantType::MatrixFloat);
return result;
}
- static std::vector<int> Calculate(const std::vector<float>& sortedData, float min, float max, int bins)
+
+ static std::vector<int>
+ Calculate(const std::vector<float>& sortedData, float min, float max, int bins)
{
std::vector<int> result;
float val = min;
@@ -303,8 +320,5 @@ namespace armarx::filters
return result;
}
-
};
-}
-
-
+} // namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
index 699dc495760c1fb1862fe4eb0a28986361448ce8..0e53d9a622061892668c6d511331f9f73dfd593b 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.cpp
@@ -30,8 +30,9 @@
using namespace armarx;
using namespace filters;
-armarx::filters::MedianDerivativeFilterV3::MedianDerivativeFilterV3(std::size_t offsetWindowSize, std::size_t windowSize)
- : MedianFilter(windowSize)
+armarx::filters::MedianDerivativeFilterV3::MedianDerivativeFilterV3(std::size_t offsetWindowSize,
+ std::size_t windowSize) :
+ MedianFilter(windowSize)
{
this->valueIndex = 0;
this->offsetIndex = 0;
@@ -41,7 +42,8 @@ armarx::filters::MedianDerivativeFilterV3::MedianDerivativeFilterV3(std::size_t
this->offsetWindowSize = offsetWindowSize;
}
-armarx::VariantBasePtr armarx::filters::MedianDerivativeFilterV3::calculate(const Ice::Current& c) const
+armarx::VariantBasePtr
+armarx::filters::MedianDerivativeFilterV3::calculate(const Ice::Current& c) const
{
std::unique_lock lock(historyMutex);
@@ -75,10 +77,10 @@ armarx::VariantBasePtr armarx::filters::MedianDerivativeFilterV3::calculate(cons
ARMARX_WARNING_S << "Unsupported Variant Type: " << var->getTypeName();
return NULL;
}
-
}
-armarx::ParameterTypeList armarx::filters::MedianDerivativeFilterV3::getSupportedTypes(const Ice::Current& c) const
+armarx::ParameterTypeList
+armarx::filters::MedianDerivativeFilterV3::getSupportedTypes(const Ice::Current& c) const
{
ParameterTypeList result = MedianFilter::getSupportedTypes(c);
result.push_back(VariantType::Vector3);
@@ -87,13 +89,17 @@ armarx::ParameterTypeList armarx::filters::MedianDerivativeFilterV3::getSupporte
return result;
}
-float armarx::filters::MedianDerivativeFilterV3::median(std::vector<float>& values)
+float
+armarx::filters::MedianDerivativeFilterV3::median(std::vector<float>& values)
{
std::sort(values.begin(), values.end());
- return values.size() % 2 == 0 ? (values.at(values.size() / 2 - 1) + values.at(values.size() / 2)) / 2 : values.at(values.size() / 2);
+ return values.size() % 2 == 0
+ ? (values.at(values.size() / 2 - 1) + values.at(values.size() / 2)) / 2
+ : values.at(values.size() / 2);
}
-Eigen::Vector3f armarx::filters::MedianDerivativeFilterV3::calculateMedian(const std::vector<Eigen::Vector3f>& data)
+Eigen::Vector3f
+armarx::filters::MedianDerivativeFilterV3::calculateMedian(const std::vector<Eigen::Vector3f>& data)
{
// increase cache-efficiency by iterating over data one time, storing all three vectors
std::vector<float> values[3];
@@ -117,10 +123,14 @@ Eigen::Vector3f armarx::filters::MedianDerivativeFilterV3::calculateMedian(const
return result;
}
-void armarx::filters::MedianDerivativeFilterV3::update(Ice::Long timestamp, const armarx::VariantBasePtr& value, const Ice::Current& c)
+void
+armarx::filters::MedianDerivativeFilterV3::update(Ice::Long timestamp,
+ const armarx::VariantBasePtr& value,
+ const Ice::Current& c)
{
VariantTypeId type = value->getType();
- if (type == VariantType::Vector3 || type == VariantType::FramedDirection || type == VariantType::FramedPosition)
+ if (type == VariantType::Vector3 || type == VariantType::FramedDirection ||
+ type == VariantType::FramedPosition)
{
Eigen::Vector3f inputValue = VariantPtr::dynamicCast(value)->get<Vector3>()->toEigen();
diff --git a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h
index 382e5c3d6334d2d3c24ccf8186b68226232c6dcd..10c1614a0ae23e60835aa24f8db869accce72159 100644
--- a/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h
+++ b/source/RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h
@@ -24,8 +24,9 @@
#pragma once
#include <ArmarXCore/observers/filters/MedianFilter.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
+
#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::filters
{
@@ -65,8 +66,8 @@ namespace armarx::filters
Eigen::Vector3f calculateMedian(std::vector<Eigen::Vector3f> const& values);
public:
- void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
-
+ void
+ update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
};
-}
+} // namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp
index 6830dbc16972143111bdacecaf52907bf99d8427..60b9981cf54e46ea37c25a512d8f634cfe41c936 100644
--- a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.cpp
@@ -1,10 +1,9 @@
#include "OffsetFilter.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
-
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::filters
{
@@ -15,8 +14,8 @@ namespace armarx::filters
windowFilterSize = 1;
}
-
- VariantBasePtr OffsetFilter::calculate(const Ice::Current&) const
+ VariantBasePtr
+ OffsetFilter::calculate(const Ice::Current&) const
{
std::unique_lock lock(historyMutex);
@@ -29,7 +28,9 @@ namespace armarx::filters
if (currentValue->getType() != initialValue->getType())
{
- ARMARX_ERROR_S << "Types in OffsetFilter are different: " << Variant::typeToString(currentValue->getType()) << " and " << Variant::typeToString(initialValue->getType());
+ ARMARX_ERROR_S << "Types in OffsetFilter are different: "
+ << Variant::typeToString(currentValue->getType()) << " and "
+ << Variant::typeToString(initialValue->getType());
return nullptr;
}
@@ -45,43 +46,51 @@ namespace armarx::filters
}
else if (type == VariantType::Float)
{
- float newValue = dataHistory.rbegin()->second->getFloat() - initialValue->getFloat();
+ float newValue =
+ dataHistory.rbegin()->second->getFloat() - initialValue->getFloat();
newVariant = new Variant(newValue);
}
else if (type == VariantType::Double)
{
- double newValue = dataHistory.rbegin()->second->getDouble() - initialValue->getDouble();
+ double newValue =
+ dataHistory.rbegin()->second->getDouble() - initialValue->getDouble();
newVariant = new Variant(newValue);
}
else if (type == VariantType::FramedDirection)
{
- FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(currentValue->get<FramedDirection>());
- FramedDirectionPtr intialVec = FramedDirectionPtr::dynamicCast(initialValue->get<FramedDirection>());
- Eigen::Vector3f newValue = vec->toEigen() - intialVec->toEigen();
+ FramedDirectionPtr vec =
+ FramedDirectionPtr::dynamicCast(currentValue->get<FramedDirection>());
+ FramedDirectionPtr intialVec =
+ FramedDirectionPtr::dynamicCast(initialValue->get<FramedDirection>());
+ Eigen::Vector3f newValue = vec->toEigen() - intialVec->toEigen();
newVariant = new Variant(new FramedDirection(newValue, vec->frame, vec->agent));
}
else if (type == VariantType::FramedPosition)
{
- FramedPositionPtr pos = FramedPositionPtr::dynamicCast(currentValue->get<FramedPosition>());
- FramedPositionPtr intialPos = FramedPositionPtr::dynamicCast(initialValue->get<FramedPosition>());
- Eigen::Vector3f newValue = pos->toEigen() - intialPos->toEigen();
+ FramedPositionPtr pos =
+ FramedPositionPtr::dynamicCast(currentValue->get<FramedPosition>());
+ FramedPositionPtr intialPos =
+ FramedPositionPtr::dynamicCast(initialValue->get<FramedPosition>());
+ Eigen::Vector3f newValue = pos->toEigen() - intialPos->toEigen();
newVariant = new Variant(new FramedPosition(newValue, pos->frame, pos->agent));
}
else if (type == VariantType::MatrixFloat)
{
- MatrixFloatPtr matrix = MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
- MatrixFloatPtr initialMatrix = MatrixFloatPtr::dynamicCast(initialValue->get<MatrixFloat>());
+ MatrixFloatPtr matrix =
+ MatrixFloatPtr::dynamicCast(currentValue->get<MatrixFloat>());
+ MatrixFloatPtr initialMatrix =
+ MatrixFloatPtr::dynamicCast(initialValue->get<MatrixFloat>());
Eigen::MatrixXf newMatrix = matrix->toEigen() - initialMatrix->toEigen();
newVariant = new Variant(new MatrixFloat(newMatrix));
}
}
return newVariant;
-
}
- ParameterTypeList OffsetFilter::getSupportedTypes(const Ice::Current&) const
+ ParameterTypeList
+ OffsetFilter::getSupportedTypes(const Ice::Current&) const
{
ParameterTypeList result;
result.push_back(VariantType::Int);
@@ -94,7 +103,8 @@ namespace armarx::filters
return result;
}
- void OffsetFilter::update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c)
+ void
+ OffsetFilter::update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c)
{
DatafieldFilter::update(timestamp, value, c);
@@ -112,4 +122,4 @@ namespace armarx::filters
}
}
-}
+} // namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
index 4af1082d7fb7ac6fb8aeea67f1491d7f94d2f39d..796f899e3ce6116d18bc08968c2ebe3b99dc06ef 100644
--- a/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/OffsetFilter.h
@@ -22,10 +22,9 @@
#pragma once
-#include <RobotAPI/interface/observers/ObserverFilters.h>
-
#include <ArmarXCore/observers/filters/DatafieldFilter.h>
+#include <RobotAPI/interface/observers/ObserverFilters.h>
namespace armarx::filters
{
@@ -38,18 +37,14 @@ namespace armarx::filters
* E.g. this is useful for Forces which should be nulled
* at a specific moment.
*/
- class OffsetFilter :
- public ::armarx::OffsetFilterBase,
- public DatafieldFilter
+ class OffsetFilter : public ::armarx::OffsetFilterBase, public DatafieldFilter
{
public:
-
OffsetFilter();
// DatafieldFilterBase interface
public:
-
VariantBasePtr calculate(const Ice::Current& = Ice::emptyCurrent) const override;
ParameterTypeList getSupportedTypes(const Ice::Current&) const override;
@@ -62,9 +57,7 @@ namespace armarx::filters
// DatafieldFilterBase interface
public:
- void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
-
+ void
+ update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
};
-}
-
-
+} // namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
index e0d61134cfb5e85319d9a45af55418cf9f1a9a07..8e2b8f75712c7aa15c15db59d386f61b2c13c83e 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.cpp
@@ -25,11 +25,11 @@
#include <ArmarXCore/core/logging/Logging.h>
-
namespace armarx::filters
{
- VariantBasePtr PoseAverageFilter::calculate(const Ice::Current& c) const
+ VariantBasePtr
+ PoseAverageFilter::calculate(const Ice::Current& c) const
{
std::unique_lock lock(historyMutex);
@@ -40,7 +40,8 @@ namespace armarx::filters
VariantTypeId type = dataHistory.begin()->second->getType();
- if ((type == VariantType::Vector3) || (type == VariantType::FramedDirection) || (type == VariantType::FramedPosition))
+ if ((type == VariantType::Vector3) || (type == VariantType::FramedDirection) ||
+ (type == VariantType::FramedPosition))
{
Eigen::Vector3f vec;
@@ -112,5 +113,4 @@ namespace armarx::filters
}
-
-}
+} // namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h
index 6ba6fa1e563fddc06b4b0579221706af9d3f6fef..9950b8bc0cb7f53caa2c5bd90fb7a1a594bac920 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseAverageFilter.h
@@ -27,14 +27,10 @@
#include <RobotAPI/libraries/core/FramedPose.h>
-
-
namespace armarx::filters
{
- class PoseAverageFilter :
- public ::armarx::PoseAverageFilterBase,
- public AverageFilter
+ class PoseAverageFilter : public ::armarx::PoseAverageFilterBase, public AverageFilter
{
public:
PoseAverageFilter(int windowSize = 11)
@@ -50,7 +46,8 @@ namespace armarx::filters
* @brief This filter supports: Vector3, FramedDirection, FramedPosition
* @return List of VariantTypes
*/
- ParameterTypeList getSupportedTypes(const Ice::Current& c) const override
+ ParameterTypeList
+ getSupportedTypes(const Ice::Current& c) const override
{
ParameterTypeList result = AverageFilter::getSupportedTypes(c);
result.push_back(VariantType::Vector3);
@@ -60,6 +57,4 @@ namespace armarx::filters
}
};
-}
-
-
+} // namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.cpp
index 72c6e8e8e798c43046f14c3dbcabc73233ab9a0d..1be55cb478ca0386cc12e1cdf7c048784998ae71 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.cpp
@@ -1,10 +1,9 @@
#include "PoseMedianFilter.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/interface/core/PoseBase.h>
-
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::filters
{
@@ -14,8 +13,8 @@ namespace armarx::filters
this->windowFilterSize = windowSize;
}
-
- VariantBasePtr PoseMedianFilter::calculate(const Ice::Current& c) const
+ VariantBasePtr
+ PoseMedianFilter::calculate(const Ice::Current& c) const
{
std::unique_lock lock(historyMutex);
@@ -26,9 +25,8 @@ namespace armarx::filters
VariantTypeId type = dataHistory.begin()->second->getType();
- if (type == VariantType::Vector3
- or type == VariantType::FramedDirection
- or type == VariantType::FramedPosition)
+ if (type == VariantType::Vector3 or type == VariantType::FramedDirection or
+ type == VariantType::FramedPosition)
{
Eigen::Vector3f vec;
vec.setZero();
@@ -99,8 +97,8 @@ namespace armarx::filters
return MedianFilter::calculate(c);
}
-
- ParameterTypeList PoseMedianFilter::getSupportedTypes(const Ice::Current& c) const
+ ParameterTypeList
+ PoseMedianFilter::getSupportedTypes(const Ice::Current& c) const
{
ParameterTypeList result = MedianFilter::getSupportedTypes(c);
result.push_back(VariantType::Vector3);
@@ -109,4 +107,4 @@ namespace armarx::filters
return result;
}
-}
+} // namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
index bc03128714c8685f07b45b09e1e41996437e697f..c8e54c21cb551e2845f323d01485a8dae646aef5 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianFilter.h
@@ -23,10 +23,9 @@
*/
#pragma once
-#include <RobotAPI/interface/core/FramedPoseBase.h>
-
#include <ArmarXCore/observers/filters/MedianFilter.h>
+#include <RobotAPI/interface/core/FramedPoseBase.h>
namespace armarx::filters
{
@@ -37,9 +36,7 @@ namespace armarx::filters
* @brief The MedianFilter class provides an implementation
* for a median for datafields of type float, int and double.
*/
- class PoseMedianFilter :
- public ::armarx::PoseMedianFilterBase,
- public MedianFilter
+ class PoseMedianFilter : public ::armarx::PoseMedianFilterBase, public MedianFilter
{
public:
PoseMedianFilter(int windowSize = 11);
@@ -53,9 +50,6 @@ namespace armarx::filters
* @return List of VariantTypes
*/
ParameterTypeList getSupportedTypes(const Ice::Current& c) const override;
-
};
-} // namespace Filters
-
-
+} // namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
index 47e3606321c73477abeb0a31bb4cf9990469c075..85c5b586a93ecec134c5c552e65dd31911f8e546 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.cpp
@@ -29,8 +29,8 @@
using namespace armarx;
using namespace filters;
-armarx::filters::PoseMedianOffsetFilter::PoseMedianOffsetFilter(int windowSize)
- : MedianFilter(windowSize)
+armarx::filters::PoseMedianOffsetFilter::PoseMedianOffsetFilter(int windowSize) :
+ MedianFilter(windowSize)
{
this->windowFilterSize = windowSize;
this->dataIndex = -windowSize;
@@ -38,7 +38,8 @@ armarx::filters::PoseMedianOffsetFilter::PoseMedianOffsetFilter(int windowSize)
this->currentValue = Eigen::Vector3f::Zero();
}
-armarx::VariantBasePtr armarx::filters::PoseMedianOffsetFilter::calculate(const Ice::Current& c) const
+armarx::VariantBasePtr
+armarx::filters::PoseMedianOffsetFilter::calculate(const Ice::Current& c) const
{
std::unique_lock lock(historyMutex);
@@ -72,10 +73,10 @@ armarx::VariantBasePtr armarx::filters::PoseMedianOffsetFilter::calculate(const
ARMARX_WARNING_S << "Unsupported Variant Type: " << var->getTypeName();
return NULL;
}
-
}
-armarx::ParameterTypeList armarx::filters::PoseMedianOffsetFilter::getSupportedTypes(const Ice::Current& c) const
+armarx::ParameterTypeList
+armarx::filters::PoseMedianOffsetFilter::getSupportedTypes(const Ice::Current& c) const
{
ParameterTypeList result = MedianFilter::getSupportedTypes(c);
result.push_back(VariantType::Vector3);
@@ -84,13 +85,17 @@ armarx::ParameterTypeList armarx::filters::PoseMedianOffsetFilter::getSupportedT
return result;
}
-float armarx::filters::PoseMedianOffsetFilter::median(std::vector<float>& values)
+float
+armarx::filters::PoseMedianOffsetFilter::median(std::vector<float>& values)
{
std::sort(values.begin(), values.end());
- return values.size() % 2 == 0 ? (values.at(values.size() / 2 - 1) + values.at(values.size() / 2)) / 2 : values.at(values.size() / 2);
+ return values.size() % 2 == 0
+ ? (values.at(values.size() / 2 - 1) + values.at(values.size() / 2)) / 2
+ : values.at(values.size() / 2);
}
-Eigen::Vector3f armarx::filters::PoseMedianOffsetFilter::calculateMedian()
+Eigen::Vector3f
+armarx::filters::PoseMedianOffsetFilter::calculateMedian()
{
Eigen::Vector3f result;
for (int i = 0; i < 3; ++i)
@@ -107,10 +112,14 @@ Eigen::Vector3f armarx::filters::PoseMedianOffsetFilter::calculateMedian()
return result;
}
-void armarx::filters::PoseMedianOffsetFilter::update(Ice::Long timestamp, const armarx::VariantBasePtr& value, const Ice::Current& c)
+void
+armarx::filters::PoseMedianOffsetFilter::update(Ice::Long timestamp,
+ const armarx::VariantBasePtr& value,
+ const Ice::Current& c)
{
VariantTypeId type = value->getType();
- if (type == VariantType::Vector3 || type == VariantType::FramedDirection || type == VariantType::FramedPosition)
+ if (type == VariantType::Vector3 || type == VariantType::FramedDirection ||
+ type == VariantType::FramedPosition)
{
Eigen::Vector3f currentValue = VariantPtr::dynamicCast(value)->get<Vector3>()->toEigen();
if (dataIndex < 0)
diff --git a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
index 7e8cc3e23e7fb69753232ffc64e3ce4435b6a11b..a49f3ebde8beef6094bba95a0f6003e5917ec86f 100644
--- a/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
+++ b/source/RobotAPI/libraries/core/observerfilters/PoseMedianOffsetFilter.h
@@ -24,8 +24,9 @@
#pragma once
#include <ArmarXCore/observers/filters/MedianFilter.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
+
#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::filters
{
@@ -36,9 +37,7 @@ namespace armarx::filters
* @brief The MedianFilter class provides an implementation
* for a median for datafields of type float, int and double.
*/
- class PoseMedianOffsetFilter :
- public ::armarx::PoseMedianOffsetFilterBase,
- public MedianFilter
+ class PoseMedianOffsetFilter : public ::armarx::PoseMedianOffsetFilterBase, public MedianFilter
{
public:
PoseMedianOffsetFilter(int windowSize = 11);
@@ -63,8 +62,8 @@ namespace armarx::filters
Eigen::Vector3f calculateMedian();
public:
- void update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
-
+ void
+ update(Ice::Long timestamp, const VariantBasePtr& value, const Ice::Current& c) override;
};
-} // namespace Filters
+} // namespace armarx::filters
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 4383ae2e53fafb87f6713bd0a341a380e073cf54..c00c65a5e80486f1ea216bee86a5c4c6cc814059 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -23,32 +23,31 @@
*/
#include "RemoteRobot.h"
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/RobotConfig.h>
-#include <VirtualRobot/RobotFactory.h>
+#include <Eigen/Geometry>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Nodes/RobotNodeFixed.h>
#include <VirtualRobot/Nodes/RobotNodeFixedFactory.h>
+#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
#include <VirtualRobot/Nodes/RobotNodePrismaticFactory.h>
+#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Nodes/RobotNodeRevoluteFactory.h>
-#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/RobotFactory.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <RobotAPI/interface/core/RobotState.h>
-#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
-#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
-#include <VirtualRobot/Nodes/RobotNodeFixed.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
-
-#include <Eigen/Geometry>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
//#define DMES(Obj) cout << format("[%1%|%2%|%3%] %4%") % __FILE__ % __LINE__ % __func__ % Obj;
#define DMES(Obj) ;
@@ -61,9 +60,7 @@ namespace armarx
std::recursive_mutex RemoteRobot::m;
- RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) :
- Robot(),
- _robot(robot)
+ RemoteRobot::RemoteRobot(SharedRobotInterfacePrx robot) : Robot(), _robot(robot)
{
_robot->ref();
}
@@ -82,10 +79,10 @@ namespace armarx
{
ARMARX_DEBUG_S << "Unref of SharedRobot failed: reason unknown";
}
-
}
- RobotNodePtr RemoteRobot::getRootNode() const
+ RobotNodePtr
+ RemoteRobot::getRootNode() const
{
// lazy initialization needed since shared_from_this() must not be called in constructor
if (!_root)
@@ -95,7 +92,8 @@ namespace armarx
return _root;
}
- bool RemoteRobot::hasRobotNode(const std::string& robotNodeName) const
+ bool
+ RemoteRobot::hasRobotNode(const std::string& robotNodeName) const
{
if (_cachedNodes.find(name) == _cachedNodes.end())
{
@@ -107,8 +105,8 @@ namespace armarx
}
}
-
- bool RemoteRobot::hasRobotNode(RobotNodePtr robotNode) const
+ bool
+ RemoteRobot::hasRobotNode(RobotNodePtr robotNode) const
{
return this->hasRobotNode(robotNode->getName());
@@ -131,16 +129,16 @@ namespace armarx
*/
}
-
- RobotNodePtr RemoteRobot::getRobotNode(const std::string& robotNodeName) const
+ RobotNodePtr
+ RemoteRobot::getRobotNode(const std::string& robotNodeName) const
{
DMES((format("Node: %1%") % robotNodeName));
auto it = _cachedNodes.find(robotNodeName);
if (it == _cachedNodes.end() || it->second == NULL)
{
DMES("No cache hit");
- _cachedNodes[robotNodeName] = RemoteRobot::createRemoteRobotNode(_robot->getRobotNode(robotNodeName),
- shared_from_this());
+ _cachedNodes[robotNodeName] = RemoteRobot::createRemoteRobotNode(
+ _robot->getRobotNode(robotNodeName), shared_from_this());
return _cachedNodes[robotNodeName];
}
else
@@ -150,7 +148,8 @@ namespace armarx
}
}
- void RemoteRobot::getRobotNodes(std::vector< RobotNodePtr >& storeNodes, bool clearVector) const
+ void
+ RemoteRobot::getRobotNodes(std::vector<RobotNodePtr>& storeNodes, bool clearVector) const
{
if (clearVector)
{
@@ -164,21 +163,23 @@ namespace armarx
}
}
- bool RemoteRobot::hasRobotNodeSet(const std::string& name) const
+ bool
+ RemoteRobot::hasRobotNodeSet(const std::string& name) const
{
return _robot->hasRobotNodeSet(name);
}
- RobotNodeSetPtr RemoteRobot::getRobotNodeSet(const std::string& nodeSetName) const
+ RobotNodeSetPtr
+ RemoteRobot::getRobotNodeSet(const std::string& nodeSetName) const
{
std::vector<RobotNodePtr> storeNodes;
RobotNodeSetInfoPtr info = _robot->getRobotNodeSet(nodeSetName);
return RobotNodeSet::createRobotNodeSet(
- shared_from_this(), nodeSetName, info->names, info->rootName, info->tcpName);
+ shared_from_this(), nodeSetName, info->names, info->rootName, info->tcpName);
}
-
- void RemoteRobot::getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet) const
+ void
+ RemoteRobot::getRobotNodeSets(std::vector<RobotNodeSetPtr>& storeNodeSet) const
{
NameList sets = _robot->getRobotNodeSets();
@@ -188,28 +189,37 @@ namespace armarx
}
}
- Matrix4f RemoteRobot::getGlobalPose() const
+ Matrix4f
+ RemoteRobot::getGlobalPose() const
{
PosePtr p = PosePtr::dynamicCast(_robot->getGlobalPose());
return p->toEigen(); // convert to eigen first
}
- float RemoteRobot::getScaling()
+ float
+ RemoteRobot::getScaling()
{
return _robot->getScaling();
}
- SharedRobotInterfacePrx RemoteRobot::getSharedRobot() const
+ SharedRobotInterfacePrx
+ RemoteRobot::getSharedRobot() const
{
return this->_robot;
}
- std::string RemoteRobot::getName() const
+ std::string
+ RemoteRobot::getName() const
{
return _robot->getName();
}
- VirtualRobot::RobotNodePtr RemoteRobot::createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> >& childrenMap, RobotPtr robo)
+ VirtualRobot::RobotNodePtr
+ RemoteRobot::createLocalNode(
+ SharedRobotNodeInterfacePrx remoteNode,
+ std::vector<VirtualRobot::RobotNodePtr>& allNodes,
+ std::map<VirtualRobot::RobotNodePtr, std::vector<std::string>>& childrenMap,
+ RobotPtr robo)
{
std::scoped_lock cloneLock(m);
static int nonameCounter = 0;
@@ -220,9 +230,15 @@ namespace armarx
return VirtualRobot::RobotNodePtr();
}
- VirtualRobot::RobotNodeFactoryPtr revoluteNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeRevoluteFactory::getName(), NULL);
- VirtualRobot::RobotNodeFactoryPtr fixedNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeFixedFactory::getName(), NULL);
- VirtualRobot::RobotNodeFactoryPtr prismaticNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodePrismaticFactory::getName(), NULL);
+ VirtualRobot::RobotNodeFactoryPtr revoluteNodeFactory =
+ VirtualRobot::RobotNodeFactory::fromName(
+ VirtualRobot::RobotNodeRevoluteFactory::getName(), NULL);
+ VirtualRobot::RobotNodeFactoryPtr fixedNodeFactory =
+ VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeFixedFactory::getName(),
+ NULL);
+ VirtualRobot::RobotNodeFactoryPtr prismaticNodeFactory =
+ VirtualRobot::RobotNodeFactory::fromName(
+ VirtualRobot::RobotNodePrismaticFactory::getName(), NULL);
Eigen::Vector3f idVec3 = Eigen::Vector3f::Zero();
std::string name = remoteNode->getName();
@@ -243,7 +259,7 @@ namespace armarx
//float jv = remoteNode->getJointValue();
float jvLo = remoteNode->getJointLimitLow();
float jvHi = remoteNode->getJointLimitHigh();
- float jointOffset = 0;//remoteNode->getJointOffset();
+ float jointOffset = 0; //remoteNode->getJointOffset();
JointType jt = remoteNode->getType();
@@ -261,7 +277,8 @@ namespace armarx
{
case ePrismatic:
{
- Vector3Ptr axisBase = Vector3Ptr::dynamicCast(remoteNode->getJointTranslationDirection());
+ Vector3Ptr axisBase =
+ Vector3Ptr::dynamicCast(remoteNode->getJointTranslationDirection());
Eigen::Vector3f axis = axisBase->toEigen();
// convert axis to local coord system
Eigen::Vector4f result4f = Eigen::Vector4f::Zero();
@@ -270,14 +287,32 @@ namespace armarx
result4f = gp->toEigen().inverse() * result4f;
axis = result4f.block(0, 0, 3, 1);
- result = prismaticNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
- jvLo, jvHi, jointOffset, localTransform, idVec3, axis, physics);
+ result = prismaticNodeFactory->createRobotNode(robo,
+ name,
+ VirtualRobot::VisualizationNodePtr(),
+ VirtualRobot::CollisionModelPtr(),
+ jvLo,
+ jvHi,
+ jointOffset,
+ localTransform,
+ idVec3,
+ axis,
+ physics);
}
break;
case eFixed:
- result = fixedNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(), 0,
- 0, 0, localTransform, idVec3, idVec3, physics);
+ result = fixedNodeFactory->createRobotNode(robo,
+ name,
+ VirtualRobot::VisualizationNodePtr(),
+ VirtualRobot::CollisionModelPtr(),
+ 0,
+ 0,
+ 0,
+ localTransform,
+ idVec3,
+ idVec3,
+ physics);
break;
case eRevolute:
@@ -291,8 +326,17 @@ namespace armarx
result4f = gp->toEigen().inverse() * result4f;
axis = result4f.block(0, 0, 3, 1);
- result = revoluteNodeFactory->createRobotNode(robo, name, VirtualRobot::VisualizationNodePtr(), VirtualRobot::CollisionModelPtr(),
- jvLo, jvHi, jointOffset, localTransform, axis, idVec3, physics);
+ result = revoluteNodeFactory->createRobotNode(robo,
+ name,
+ VirtualRobot::VisualizationNodePtr(),
+ VirtualRobot::CollisionModelPtr(),
+ jvLo,
+ jvHi,
+ jointOffset,
+ localTransform,
+ axis,
+ idVec3,
+ physics);
}
break;
@@ -316,7 +360,8 @@ namespace armarx
if (_robot->hasRobotNode(childrenBase[i]))
{
SharedRobotNodeInterfacePrx rnRemote = _robot->getRobotNode(childrenBase[i]);
- VirtualRobot::RobotNodePtr localNode = createLocalNode(rnRemote, allNodes, childrenMap, robo);
+ VirtualRobot::RobotNodePtr localNode =
+ createLocalNode(rnRemote, allNodes, childrenMap, robo);
if (!localNode)
{
@@ -332,7 +377,8 @@ namespace armarx
return result;
}
- VirtualRobot::RobotPtr RemoteRobot::createLocalClone()
+ VirtualRobot::RobotPtr
+ RemoteRobot::createLocalClone()
{
std::scoped_lock cloneLock(m);
std::string robotType = getName();
@@ -343,11 +389,12 @@ namespace armarx
SharedRobotNodeInterfacePrx root = _robot->getRootNode();
std::vector<VirtualRobot::RobotNodePtr> allNodes;
- std::map< VirtualRobot::RobotNodePtr, std::vector<std::string> > childrenMap;
+ std::map<VirtualRobot::RobotNodePtr, std::vector<std::string>> childrenMap;
VirtualRobot::RobotNodePtr rootLocal = createLocalNode(root, allNodes, childrenMap, robo);
- bool res = VirtualRobot::RobotFactory::initializeRobot(robo, allNodes, childrenMap, rootLocal);
+ bool res =
+ VirtualRobot::RobotFactory::initializeRobot(robo, allNodes, childrenMap, rootLocal);
if (!res)
{
@@ -361,7 +408,8 @@ namespace armarx
for (size_t i = 0; i < rns.size(); i++)
{
RobotNodeSetInfoPtr rnsInfo = _robot->getRobotNodeSet(rns[i]);
- RobotNodeSet::createRobotNodeSet(robo, rnsInfo->name, rnsInfo->names, rnsInfo->rootName, rnsInfo->tcpName, true);
+ RobotNodeSet::createRobotNodeSet(
+ robo, rnsInfo->name, rnsInfo->names, rnsInfo->rootName, rnsInfo->tcpName, true);
}
//ARMARX_IMPORTANT_S << "RemoteRobot local clone end" << flush;
@@ -370,12 +418,25 @@ namespace armarx
return robo;
}
- VirtualRobot::RobotPtr RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename, const Ice::StringSeq packages, VirtualRobot::RobotIO::RobotDescription loadMode)
+ VirtualRobot::RobotPtr
+ RemoteRobot::createLocalClone(RobotStateComponentInterfacePrx robotStatePrx,
+ const std::string& filename,
+ const Ice::StringSeq packages,
+ VirtualRobot::RobotIO::RobotDescription loadMode)
{
- return createLocalClone(robotStatePrx->getSynchronizedRobot(), filename, robotStatePrx->getScaling(), packages, loadMode);
+ return createLocalClone(robotStatePrx->getSynchronizedRobot(),
+ filename,
+ robotStatePrx->getScaling(),
+ packages,
+ loadMode);
}
- RobotPtr RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, std::string filename, float scaling, const Ice::StringSeq packages, VirtualRobot::RobotIO::RobotDescription loadMode)
+ RobotPtr
+ RemoteRobot::createLocalClone(SharedRobotInterfacePrx sharedRobotPrx,
+ std::string filename,
+ float scaling,
+ const Ice::StringSeq packages,
+ VirtualRobot::RobotIO::RobotDescription loadMode)
{
RobotPtr result;
@@ -413,11 +474,13 @@ namespace armarx
CMakePackageFinder project(projectName);
auto pathsString = project.getDataDir();
- ARMARX_DEBUG_S << "Data paths of ArmarX package " << projectName << ": " << pathsString;
+ ARMARX_DEBUG_S << "Data paths of ArmarX package " << projectName << ": "
+ << pathsString;
Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
- ARMARX_DEBUG_S << "Result: Data paths of ArmarX package " << projectName << ": " << projectIncludePaths;
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
-
+ ARMARX_DEBUG_S << "Result: Data paths of ArmarX package " << projectName << ": "
+ << projectIncludePaths;
+ includePaths.insert(
+ includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
}
@@ -445,19 +508,26 @@ namespace armarx
return result;
}
- RobotPtr RemoteRobot::createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, RobotIO::RobotDescription loadMode)
+ RobotPtr
+ RemoteRobot::createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx,
+ RobotIO::RobotDescription loadMode)
{
- return createLocalClone(robotStatePrx, robotStatePrx->getRobotFilename(), robotStatePrx->getArmarXPackages(), loadMode);
+ return createLocalClone(robotStatePrx,
+ robotStatePrx->getRobotFilename(),
+ robotStatePrx->getArmarXPackages(),
+ loadMode);
}
-
-
- bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
+ bool
+ RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot,
+ RobotStateComponentInterfacePrx robotStatePrx)
{
return synchronizeLocalClone(robot, robotStatePrx->getSynchronizedRobot());
}
- bool RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx)
+ bool
+ RemoteRobot::synchronizeLocalClone(VirtualRobot::RobotPtr robot,
+ SharedRobotInterfacePrx sharedRobotPrx)
{
if (!robot)
{
@@ -481,7 +551,8 @@ namespace armarx
if (!c->setConfig(jointName, jointAngle))
{
- ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping...";
+ ARMARX_WARNING << deactivateSpam(10, jointName)
+ << "Joint not known in local copy:" << jointName << ". Skipping...";
}
}
@@ -491,7 +562,10 @@ namespace armarx
return true;
}
- bool RemoteRobot::synchronizeLocalCloneToTimestamp(RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
+ bool
+ RemoteRobot::synchronizeLocalCloneToTimestamp(RobotPtr robot,
+ RobotStateComponentInterfacePrx robotStatePrx,
+ Ice::Long timestamp)
{
ARMARX_CHECK_EXPRESSION(robotStatePrx);
@@ -501,7 +575,8 @@ namespace armarx
return synchronizeLocalCloneToState(robot, state);
}
- bool RemoteRobot::synchronizeLocalCloneToState(RobotPtr robot, const RobotStateConfig& state)
+ bool
+ RemoteRobot::synchronizeLocalCloneToState(RobotPtr robot, const RobotStateConfig& state)
{
ARMARX_CHECK_EXPRESSION(robot);
@@ -511,7 +586,8 @@ namespace armarx
return false;
}
- for (NameValueMap::const_iterator it = state.jointMap.begin(); it != state.jointMap.end(); it++)
+ for (NameValueMap::const_iterator it = state.jointMap.begin(); it != state.jointMap.end();
+ it++)
{
// joint values
const std::string& jointName = it->first;
@@ -519,7 +595,8 @@ namespace armarx
if (!c->setConfig(jointName, jointAngle))
{
- ARMARX_WARNING << deactivateSpam(10, jointName) << "Joint not known in local copy:" << jointName << ". Skipping...";
+ ARMARX_WARNING << deactivateSpam(10, jointName)
+ << "Joint not known in local copy:" << jointName << ". Skipping...";
}
}
@@ -530,29 +607,57 @@ namespace armarx
return true;
}
-
-
// Private (unused methods)
- bool RemoteRobot::hasEndEffector(const std::string& endEffectorName) const
+ bool
+ RemoteRobot::hasEndEffector(const std::string& endEffectorName) const
{
return false;
}
- EndEffectorPtr RemoteRobot::getEndEffector(const std::string& endEffectorName) const
+ EndEffectorPtr
+ RemoteRobot::getEndEffector(const std::string& endEffectorName) const
{
return EndEffectorPtr();
}
- void RemoteRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) const {}
- void RemoteRobot::setRootNode(RobotNodePtr node) {}
- void RemoteRobot::registerRobotNode(RobotNodePtr node) {}
- void RemoteRobot::deregisterRobotNode(RobotNodePtr node) {}
- void RemoteRobot::registerRobotNodeSet(RobotNodeSetPtr nodeSet) {}
- void RemoteRobot::deregisterRobotNodeSet(RobotNodeSetPtr nodeSet) {}
- void RemoteRobot::registerEndEffector(EndEffectorPtr endEffector) {}
+ void
+ RemoteRobot::getEndEffectors(std::vector<EndEffectorPtr>& storeEEF) const
+ {
+ }
+
+ void
+ RemoteRobot::setRootNode(RobotNodePtr node)
+ {
+ }
+
+ void
+ RemoteRobot::registerRobotNode(RobotNodePtr node)
+ {
+ }
+
+ void
+ RemoteRobot::deregisterRobotNode(RobotNodePtr node)
+ {
+ }
+
+ void
+ RemoteRobot::registerRobotNodeSet(RobotNodeSetPtr nodeSet)
+ {
+ }
+
+ void
+ RemoteRobot::deregisterRobotNodeSet(RobotNodeSetPtr nodeSet)
+ {
+ }
+
+ void
+ RemoteRobot::registerEndEffector(EndEffectorPtr endEffector)
+ {
+ }
- void RemoteRobot::setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues)
+ void
+ RemoteRobot::setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues)
{
if (_robot)
{
@@ -560,10 +665,11 @@ namespace armarx
}
}
- CollisionCheckerPtr RemoteRobotNode_getGlobalCollisionChecker()
+ CollisionCheckerPtr
+ RemoteRobotNode_getGlobalCollisionChecker()
{
return VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index c976c53411a43af5837a52fb8d58926401db4bee..09128b1bbade5707bdd4d5f7d466982e09dc4f03 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -23,58 +23,58 @@
*/
#pragma once
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/XML/RobotIO.h>
+#include <mutex>
#include <VirtualRobot/Nodes/RobotNodeFixed.h>
-#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Nodes/RobotNodePrismatic.h>
+#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
#include <RobotAPI/interface/core/RobotState.h>
-#include <mutex>
-
namespace armarx
{
// forward declaration of RemoteRobotNode
- template<class VirtualRobotNodeType> class RemoteRobotNode;
+ template <class VirtualRobotNodeType>
+ class RemoteRobotNode;
/** @brief RemoteRobotNodeInitializer is used to initialize the robot node for a given node type.
* For each robot type to be supported make a specialization of the initialize function.
* Currently supports: RobotNodeRevolute, RobotNodePrismatic, RobotNodeFixed. Node type specific
* initializations go here.
*/
- template<typename VirtualRobotNodeType>
+ template <typename VirtualRobotNodeType>
struct RemoteRobotNodeInitializer
{
static void initialize(RemoteRobotNode<VirtualRobotNodeType>* remoteNode);
};
// specializations
- template<>
- void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeRevolute>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeRevolute>* remoteNode);
- template<>
- void RemoteRobotNodeInitializer<VirtualRobot::RobotNodePrismatic>::initialize(RemoteRobotNode<VirtualRobot::RobotNodePrismatic>* remoteNode);
- template<>
- void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode);
+ template <>
+ void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeRevolute>::initialize(
+ RemoteRobotNode<VirtualRobot::RobotNodeRevolute>* remoteNode);
+ template <>
+ void RemoteRobotNodeInitializer<VirtualRobot::RobotNodePrismatic>::initialize(
+ RemoteRobotNode<VirtualRobot::RobotNodePrismatic>* remoteNode);
+ template <>
+ void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(
+ RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode);
VirtualRobot::CollisionCheckerPtr RemoteRobotNode_getGlobalCollisionChecker();
-
/** @brief Mimics the behaviour of robot nodes while redirecting everything to Ice proxies.
* @tparam VirtualRobotNodeType Must be a descendant of VirtualRobot::RobotNode
* @details This class is for <b> internal use only</b> as classes cannot be referenced!
*/
- template<class VirtualRobotNodeType>
- class RemoteRobotNode :
- public VirtualRobotNodeType
+ template <class VirtualRobotNodeType>
+ class RemoteRobotNode : public VirtualRobotNodeType
{
friend struct RemoteRobotNodeInitializer<VirtualRobotNodeType>;
public:
- RemoteRobotNode(SharedRobotNodeInterfacePrx node, VirtualRobot::RobotPtr vr) :
- _node(node)
+ RemoteRobotNode(SharedRobotNodeInterfacePrx node, VirtualRobot::RobotPtr vr) : _node(node)
{
_node->ref();
this->name = _node->getName();
@@ -100,9 +100,12 @@ namespace armarx
Eigen::Vector3f getPositionInRootFrame() const override;
virtual bool hasChildNode(const std::string& child, bool recursive = false) const;
- std::vector<VirtualRobot::RobotNodePtr> getAllParents(VirtualRobot::RobotNodeSetPtr rns) override;
+ std::vector<VirtualRobot::RobotNodePtr>
+ getAllParents(VirtualRobot::RobotNodeSetPtr rns) override;
virtual VirtualRobot::SceneObjectPtr getParent();
- inline SharedRobotNodeInterfacePrx getSharedRobotNode()
+
+ inline SharedRobotNodeInterfacePrx
+ getSharedRobotNode()
{
return _node;
}
@@ -116,18 +119,20 @@ namespace armarx
virtual void setLocalTransformation(const Eigen::Matrix4f& trafo);
virtual std::string getParentName() const;
- std::vector< VirtualRobot::SceneObjectPtr> getChildren() const override;
+ std::vector<VirtualRobot::SceneObjectPtr> getChildren() const override;
void updateTransformationMatrices() override;
void updateTransformationMatrices(const Eigen::Matrix4f& globalPose) override;
- virtual bool hasChildNode(const VirtualRobot::RobotNodePtr child, bool recursive = false) const;
+ virtual bool hasChildNode(const VirtualRobot::RobotNodePtr child,
+ bool recursive = false) const;
virtual void addChildNode(VirtualRobot::RobotNodePtr child);
virtual bool initialize(VirtualRobot::RobotNodePtr parent, bool initializeChildren = false);
virtual void reset();
void setGlobalPose(const Eigen::Matrix4f& pose) override;
- virtual void setJointValue(float q, bool updateTransformations = true, bool clampToLimits = true);
+ virtual void
+ setJointValue(float q, bool updateTransformations = true, bool clampToLimits = true);
SharedRobotNodeInterfacePrx _node;
};
@@ -149,11 +154,14 @@ namespace armarx
bool hasRobotNode(VirtualRobot::RobotNodePtr) const override;
VirtualRobot::RobotNodePtr getRobotNode(const std::string& robotNodeName) const override;
- void getRobotNodes(std::vector< VirtualRobot::RobotNodePtr >& storeNodes, bool clearVector = true) const override;
+ void getRobotNodes(std::vector<VirtualRobot::RobotNodePtr>& storeNodes,
+ bool clearVector = true) const override;
bool hasRobotNodeSet(const std::string& name) const override;
- VirtualRobot::RobotNodeSetPtr getRobotNodeSet(const std::string& nodeSetName) const override;
- void getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet) const override;
+ VirtualRobot::RobotNodeSetPtr
+ getRobotNodeSet(const std::string& nodeSetName) const override;
+ void
+ getRobotNodeSets(std::vector<VirtualRobot::RobotNodeSetPtr>& storeNodeSet) const override;
/**
*
@@ -164,7 +172,6 @@ namespace armarx
float getScaling();
-
/// Use this method to share the robot instance over Ice.
SharedRobotInterfacePrx getSharedRobot() const;
@@ -180,9 +187,18 @@ namespace armarx
The loadMode specifies in which mode the model should be loaded. Refer to simox for more information (only matters if filename was passed in).
@see createLocalCloneFromFile(), synchronizeLocalClone()
*/
- static VirtualRobot::RobotPtr createLocalClone(RobotStateComponentInterfacePrx robotStatePrx, const std::string& filename = std::string(), const Ice::StringSeq packages = Ice::StringSeq(), VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
-
- static VirtualRobot::RobotPtr createLocalClone(SharedRobotInterfacePrx sharedRobotPrx, std::string filename = std::string(), float scaling = 1.0f, const Ice::StringSeq packages = Ice::StringSeq(), VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
+ static VirtualRobot::RobotPtr createLocalClone(
+ RobotStateComponentInterfacePrx robotStatePrx,
+ const std::string& filename = std::string(),
+ const Ice::StringSeq packages = Ice::StringSeq(),
+ VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
+
+ static VirtualRobot::RobotPtr createLocalClone(
+ SharedRobotInterfacePrx sharedRobotPrx,
+ std::string filename = std::string(),
+ float scaling = 1.0f,
+ const Ice::StringSeq packages = Ice::StringSeq(),
+ VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
/**
* @brief This is a convenience function for createLocalClone, which automatically gets the filename from the RobotStateComponent,
@@ -192,7 +208,9 @@ namespace armarx
* @return new robot clone
* @see createLocalClone(), synchronizeLocalClone()
*/
- static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
+ static VirtualRobot::RobotPtr createLocalCloneFromFile(
+ RobotStateComponentInterfacePrx robotStatePrx,
+ VirtualRobot::RobotIO::RobotDescription loadMode = VirtualRobot::RobotIO::eFull);
/*!
@@ -200,9 +218,11 @@ namespace armarx
The local clone must have the identical structure as the remote robot model, otherwise an error will be reported.
This is the fastest way to get update a local robot.
*/
- static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx);
+ static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot,
+ RobotStateComponentInterfacePrx robotStatePrx);
- static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx);
+ static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot,
+ SharedRobotInterfacePrx sharedRobotPrx);
/**
* @brief Synchronizes a local robot to a robot state at timestamp.
@@ -211,9 +231,12 @@ namespace armarx
* @param timestamp Timestamp to which the local robot should be sychronized. Must be in range of the state history of the RobotStateComponent.
* @return True if successfully synced.
*/
- static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp);
+ static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot,
+ RobotStateComponentInterfacePrx robotStatePrx,
+ Ice::Long timestamp);
- static bool synchronizeLocalCloneToState(VirtualRobot::RobotPtr robot, RobotStateConfig const& state);
+ static bool synchronizeLocalCloneToState(VirtualRobot::RobotPtr robot,
+ RobotStateConfig const& state);
// VirtualRobot::RobotPtr getRobotPtr() { return shared_from_this();} // only for debugging
@@ -221,11 +244,11 @@ namespace armarx
VirtualRobot::RobotPtr createLocalClone();
protected:
-
/// Not implemented yet
bool hasEndEffector(const std::string& endEffectorName) const override;
/// Not implemented yet
- VirtualRobot::EndEffectorPtr getEndEffector(const std::string& endEffectorName) const override;
+ VirtualRobot::EndEffectorPtr
+ getEndEffector(const std::string& endEffectorName) const override;
/// Not implemented yet
void getEndEffectors(std::vector<VirtualRobot::EndEffectorPtr>& storeEEF) const override;
@@ -248,7 +271,11 @@ namespace armarx
*/
void setGlobalPose(const Eigen::Matrix4f& globalPose, bool applyValues = true) override;
- VirtualRobot::RobotNodePtr createLocalNode(SharedRobotNodeInterfacePrx remoteNode, std::vector<VirtualRobot::RobotNodePtr>& allNodes, std::map<VirtualRobot::RobotNodePtr, std::vector<std::string> >& childrenMap, VirtualRobot::RobotPtr robo);
+ VirtualRobot::RobotNodePtr
+ createLocalNode(SharedRobotNodeInterfacePrx remoteNode,
+ std::vector<VirtualRobot::RobotNodePtr>& allNodes,
+ std::map<VirtualRobot::RobotNodePtr, std::vector<std::string>>& childrenMap,
+ VirtualRobot::RobotPtr robo);
protected:
SharedRobotInterfacePrx _robot;
@@ -257,8 +284,9 @@ namespace armarx
static std::recursive_mutex m;
- static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx, VirtualRobot::RobotPtr);
+ static VirtualRobot::RobotNodePtr createRemoteRobotNode(SharedRobotNodeInterfacePrx,
+ VirtualRobot::RobotPtr);
};
using RemoteRobotPtr = std::shared_ptr<RemoteRobot>;
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
index 6e402e30031d962e4e755e50cd2bd63b0131fe8d..a359e3b985e9ceb7fc1657d38eae045227b3ee19 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
@@ -21,17 +21,16 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include "RemoteRobot.h"
-
-#include <RobotAPI/libraries/core/FramedPose.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/BasicTypes.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include "RemoteRobot.h"
namespace armarx
{
@@ -40,7 +39,8 @@ namespace armarx
using namespace VirtualRobot;
using namespace Eigen;
- RobotNodePtr RemoteRobot::createRemoteRobotNode(SharedRobotNodeInterfacePrx node, RobotPtr robot)
+ RobotNodePtr
+ RemoteRobot::createRemoteRobotNode(SharedRobotNodeInterfacePrx node, RobotPtr robot)
{
switch (node->getType())
{
@@ -60,29 +60,37 @@ namespace armarx
return RobotNodePtr();
}
-
- template<>
- void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeRevolute>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeRevolute>* remoteNode)
+ template <>
+ void
+ RemoteRobotNodeInitializer<VirtualRobot::RobotNodeRevolute>::initialize(
+ RemoteRobotNode<VirtualRobot::RobotNodeRevolute>* remoteNode)
{
// set rotation axis
- remoteNode->jointRotationAxis = remoteNode->getGlobalPose().block<3, 3>(0, 0).inverse() * Vector3Ptr::dynamicCast(remoteNode->_node->getJointRotationAxis())->toEigen();
+ remoteNode->jointRotationAxis =
+ remoteNode->getGlobalPose().block<3, 3>(0, 0).inverse() *
+ Vector3Ptr::dynamicCast(remoteNode->_node->getJointRotationAxis())->toEigen();
}
- template<>
- void RemoteRobotNodeInitializer<VirtualRobot::RobotNodePrismatic>::initialize(RemoteRobotNode<VirtualRobot::RobotNodePrismatic>* remoteNode)
+ template <>
+ void
+ RemoteRobotNodeInitializer<VirtualRobot::RobotNodePrismatic>::initialize(
+ RemoteRobotNode<VirtualRobot::RobotNodePrismatic>* remoteNode)
{
// set translation direction
- remoteNode->jointTranslationDirection = remoteNode->getGlobalPose().block<3, 3>(0, 0).inverse() * Vector3Ptr::dynamicCast(remoteNode->_node->getJointTranslationDirection())->toEigen();
+ remoteNode->jointTranslationDirection =
+ remoteNode->getGlobalPose().block<3, 3>(0, 0).inverse() *
+ Vector3Ptr::dynamicCast(remoteNode->_node->getJointTranslationDirection())->toEigen();
}
- template<>
- void RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode)
+ template <>
+ void
+ RemoteRobotNodeInitializer<VirtualRobot::RobotNodeFixed>::initialize(
+ RemoteRobotNode<VirtualRobot::RobotNodeFixed>* remoteNode)
{
// nothing to do for fixed joints
}
-
- template<class RobotNodeType>
+ template <class RobotNodeType>
RemoteRobotNode<RobotNodeType>::~RemoteRobotNode()
{
try
@@ -99,71 +107,86 @@ namespace armarx
}
}
- template<class RobotNodeType>
- float RemoteRobotNode<RobotNodeType>::getJointValue() const
+ template <class RobotNodeType>
+ float
+ RemoteRobotNode<RobotNodeType>::getJointValue() const
{
return _node->getJointValue();
}
- template<class RobotNodeType>
- float RemoteRobotNode<RobotNodeType>::getJointLimitHi() const
+ template <class RobotNodeType>
+ float
+ RemoteRobotNode<RobotNodeType>::getJointLimitHi() const
{
return _node->getJointLimitHigh();
}
- template<class RobotNodeType>
- float RemoteRobotNode<RobotNodeType>::getJointLimitLo() const
+
+ template <class RobotNodeType>
+ float
+ RemoteRobotNode<RobotNodeType>::getJointLimitLo() const
{
return _node->getJointLimitLow();
}
+
/*
template<class RobotNodeType>
Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getPostJointTransformation(){
return PosePtr::dynamicCast(_node->getPostJointTransformation())->toEigen();
}*/
- template<class RobotNodeType>
- Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getLocalTransformation()
+ template <class RobotNodeType>
+ Eigen::Matrix4f
+ RemoteRobotNode<RobotNodeType>::getLocalTransformation()
{
return PosePtr::dynamicCast(_node->getLocalTransformation())->toEigen();
}
- template<class RobotNodeType>
- Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getGlobalPose() const
+ template <class RobotNodeType>
+ Eigen::Matrix4f
+ RemoteRobotNode<RobotNodeType>::getGlobalPose() const
{
return FramedPosePtr::dynamicCast(_node->getGlobalPose())->toEigen();
}
- template<class RobotNodeType>
- Eigen::Matrix4f RemoteRobotNode<RobotNodeType>::getPoseInRootFrame() const
+
+ template <class RobotNodeType>
+ Eigen::Matrix4f
+ RemoteRobotNode<RobotNodeType>::getPoseInRootFrame() const
{
return FramedPosePtr::dynamicCast(_node->getPoseInRootFrame())->toEigen();
}
- template<class RobotNodeType>
- Eigen::Vector3f RemoteRobotNode<RobotNodeType>::getPositionInRootFrame() const
+ template <class RobotNodeType>
+ Eigen::Vector3f
+ RemoteRobotNode<RobotNodeType>::getPositionInRootFrame() const
{
Vector3Ptr pos = Vector3Ptr::dynamicCast(_node->getPoseInRootFrame()->position);
ARMARX_CHECK_EXPRESSION(pos);
return pos->toEigen();
}
- template<class RobotNodeType>
- bool RemoteRobotNode<RobotNodeType>::hasChildNode(const RobotNodePtr child, bool recursive) const
+ template <class RobotNodeType>
+ bool
+ RemoteRobotNode<RobotNodeType>::hasChildNode(const RobotNodePtr child, bool recursive) const
{
return false;
}
- template<class RobotNodeType>
- bool RemoteRobotNode<RobotNodeType>::hasChildNode(const std::string& child, bool recursive) const
+
+ template <class RobotNodeType>
+ bool
+ RemoteRobotNode<RobotNodeType>::hasChildNode(const std::string& child, bool recursive) const
{
return _node->hasChild(child, recursive);
}
- template<class RobotNodeType>
- void RemoteRobotNode<RobotNodeType>::setJointLimits(float lo, float hi)
+ template <class RobotNodeType>
+ void
+ RemoteRobotNode<RobotNodeType>::setJointLimits(float lo, float hi)
{
}
- template<class RobotNodeType>
- vector<RobotNodePtr> RemoteRobotNode<RobotNodeType>::getAllParents(RobotNodeSetPtr rns)
+ template <class RobotNodeType>
+ vector<RobotNodePtr>
+ RemoteRobotNode<RobotNodeType>::getAllParents(RobotNodeSetPtr rns)
{
NameList nodes = _node->getAllParents(rns->getName());
vector<RobotNodePtr> result;
@@ -176,31 +199,40 @@ namespace armarx
return result;
}
- template<class RobotNodeType>
- SceneObjectPtr RemoteRobotNode<RobotNodeType>::getParent()
+ template <class RobotNodeType>
+ SceneObjectPtr
+ RemoteRobotNode<RobotNodeType>::getParent()
{
return this->robot.lock()->getRobotNode(_node->getParent());
}
+
/*
template<class RobotNodeType>
void RemoteRobotNode<RobotNodeType>::setPostJointTransformation(const Eigen::Matrix4f &trafo){
}*/
- template<class RobotNodeType>
- void RemoteRobotNode<RobotNodeType>::setLocalTransformation(const Eigen::Matrix4f& trafo)
+ template <class RobotNodeType>
+ void
+ RemoteRobotNode<RobotNodeType>::setLocalTransformation(const Eigen::Matrix4f& trafo)
{
}
- template<class RobotNodeType>
- std::vector<std::string> RemoteRobotNode<RobotNodeType>::getChildrenNames() const
+
+ template <class RobotNodeType>
+ std::vector<std::string>
+ RemoteRobotNode<RobotNodeType>::getChildrenNames() const
{
return _node->getChildren();
}
- template<class RobotNodeType>
- std::string RemoteRobotNode<RobotNodeType>::getParentName() const
+
+ template <class RobotNodeType>
+ std::string
+ RemoteRobotNode<RobotNodeType>::getParentName() const
{
return _node->getParent();
}
- template<class RobotNodeType>
- std::vector<SceneObjectPtr> RemoteRobotNode<RobotNodeType>::getChildren() const
+
+ template <class RobotNodeType>
+ std::vector<SceneObjectPtr>
+ RemoteRobotNode<RobotNodeType>::getChildren() const
{
NameList nodes = _node->getChildren();
vector<SceneObjectPtr> result;
@@ -211,37 +243,49 @@ namespace armarx
return result;
}
-
- template<class RobotNodeType>
- void RemoteRobotNode<RobotNodeType>::updateTransformationMatrices()
+ template <class RobotNodeType>
+ void
+ RemoteRobotNode<RobotNodeType>::updateTransformationMatrices()
{
}
- template<class RobotNodeType>
- void RemoteRobotNode<RobotNodeType>::updateTransformationMatrices(const Eigen::Matrix4f& globalPose)
+
+ template <class RobotNodeType>
+ void
+ RemoteRobotNode<RobotNodeType>::updateTransformationMatrices(const Eigen::Matrix4f& globalPose)
{
}
-
- template<class RobotNodeType>
- void RemoteRobotNode<RobotNodeType>::addChildNode(RobotNodePtr child)
+ template <class RobotNodeType>
+ void
+ RemoteRobotNode<RobotNodeType>::addChildNode(RobotNodePtr child)
{
}
- template<class RobotNodeType>
- bool RemoteRobotNode<RobotNodeType>::initialize(RobotNodePtr parent, bool initializeChildren)
+
+ template <class RobotNodeType>
+ bool
+ RemoteRobotNode<RobotNodeType>::initialize(RobotNodePtr parent, bool initializeChildren)
{
return false;
}
- template<class RobotNodeType>
- void RemoteRobotNode<RobotNodeType>::reset()
+
+ template <class RobotNodeType>
+ void
+ RemoteRobotNode<RobotNodeType>::reset()
{
}
- template<class RobotNodeType>
- void RemoteRobotNode<RobotNodeType>::setGlobalPose(const Eigen::Matrix4f& pose)
+
+ template <class RobotNodeType>
+ void
+ RemoteRobotNode<RobotNodeType>::setGlobalPose(const Eigen::Matrix4f& pose)
{
}
- template<class RobotNodeType>
- void RemoteRobotNode<RobotNodeType>::setJointValue(float q, bool updateTransformations, bool clampToLimits)
+
+ template <class RobotNodeType>
+ void
+ RemoteRobotNode<RobotNodeType>::setJointValue(float q,
+ bool updateTransformations,
+ bool clampToLimits)
{
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index a140749a911bcf68bf4ba0c84417509ae8b21101..578b8cd4f51563389f249ba2d2722891c575db75 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -21,25 +21,24 @@
*/
#include "RobotStateObserver.h"
-#include <RobotAPI/interface/core/RobotState.h>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotConfig.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/VirtualRobot.h>
+
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
#include <ArmarXCore/observers/checks/ConditionCheckSmaller.h>
-
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
-#include <VirtualRobot/RobotConfig.h>
-#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#define TCP_POSE_CHANNEL "TCPPose"
#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
@@ -53,10 +52,10 @@ using namespace VirtualRobot;
// ********************************************************************
RobotStateObserver::RobotStateObserver()
{
-
}
-void RobotStateObserver::onInitObserver()
+void
+RobotStateObserver::onInitObserver()
{
// register all checks
@@ -66,7 +65,8 @@ void RobotStateObserver::onInitObserver()
offerConditionCheck("smaller", new ConditionCheckSmaller());
}
-void RobotStateObserver::onConnectObserver()
+void
+RobotStateObserver::onConnectObserver()
{
@@ -79,9 +79,8 @@ void RobotStateObserver::onConnectObserver()
// ********************************************************************
-
-
-void RobotStateObserver::updatePoses()
+void
+RobotStateObserver::updatePoses()
{
if (getState() < eManagedIceObjectStarting)
{
@@ -96,24 +95,24 @@ void RobotStateObserver::updatePoses()
std::unique_lock lock(dataMutex);
ReadLockPtr lock2 = robot->getReadLock();
FramedPoseBaseMap tcpPoses;
- std::string rootFrame = robot->getRootNode()->getName();
+ std::string rootFrame = robot->getRootNode()->getName();
//IceUtil::Time start = IceUtil::Time::now();
for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
VirtualRobot::RobotNodePtr& node = nodesToReport.at(i).first;
- const std::string& tcpName = node->getName();
+ const std::string& tcpName = node->getName();
const Eigen::Matrix4f& currentPose = node->getPoseInRootFrame();
tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robot->getName());
- FramedPosePtr::dynamicCast(tcpPoses[tcpName])->changeFrame(robot, nodesToReport.at(i).second);
-
+ FramedPosePtr::dynamicCast(tcpPoses[tcpName])
+ ->changeFrame(robot, nodesToReport.at(i).second);
}
udpatePoseDatafields(tcpPoses);
-
}
-void RobotStateObserver::udpatePoseDatafields(const FramedPoseBaseMap& poseMap)
+void
+RobotStateObserver::udpatePoseDatafields(const FramedPoseBaseMap& poseMap)
{
// ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
FramedPoseBaseMap::const_iterator it = poseMap.begin();
@@ -126,7 +125,8 @@ void RobotStateObserver::udpatePoseDatafields(const FramedPoseBaseMap& poseMap)
if (!existsDataField(TCP_POSE_CHANNEL, tcpName))
{
- offerDataFieldWithDefault(TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ offerDataFieldWithDefault(
+ TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
}
else
{
@@ -141,43 +141,47 @@ void RobotStateObserver::udpatePoseDatafields(const FramedPoseBaseMap& poseMap)
offerDataFieldWithDefault(tcpName, "x", Variant(vec->position->x), "X axis");
offerDataFieldWithDefault(tcpName, "y", Variant(vec->position->y), "Y axis");
offerDataFieldWithDefault(tcpName, "z", Variant(vec->position->z), "Z axis");
- offerDataFieldWithDefault(tcpName, "qx", Variant(vec->orientation->qx), "Quaternion part x");
- offerDataFieldWithDefault(tcpName, "qy", Variant(vec->orientation->qy), "Quaternion part y");
- offerDataFieldWithDefault(tcpName, "qz", Variant(vec->orientation->qz), "Quaternion part z");
- offerDataFieldWithDefault(tcpName, "qw", Variant(vec->orientation->qw), "Quaternion part w");
+ offerDataFieldWithDefault(
+ tcpName, "qx", Variant(vec->orientation->qx), "Quaternion part x");
+ offerDataFieldWithDefault(
+ tcpName, "qy", Variant(vec->orientation->qy), "Quaternion part y");
+ offerDataFieldWithDefault(
+ tcpName, "qz", Variant(vec->orientation->qz), "Quaternion part z");
+ offerDataFieldWithDefault(
+ tcpName, "qw", Variant(vec->orientation->qw), "Quaternion part w");
offerDataFieldWithDefault(tcpName, "frame", Variant(vec->frame), "Reference Frame");
}
else
{
StringVariantBaseMap newValues;
- newValues["x"] = new Variant(vec->position->x);
- newValues["y"] = new Variant(vec->position->y);
- newValues["z"] = new Variant(vec->position->z);
- newValues["qx"] = new Variant(vec->orientation->qx);
- newValues["qy"] = new Variant(vec->orientation->qy);
- newValues["qz"] = new Variant(vec->orientation->qz);
- newValues["qw"] = new Variant(vec->orientation->qw);
- newValues["frame"] = new Variant(vec->frame);
+ newValues["x"] = new Variant(vec->position->x);
+ newValues["y"] = new Variant(vec->position->y);
+ newValues["z"] = new Variant(vec->position->z);
+ newValues["qx"] = new Variant(vec->orientation->qx);
+ newValues["qy"] = new Variant(vec->orientation->qy);
+ newValues["qz"] = new Variant(vec->orientation->qz);
+ newValues["qw"] = new Variant(vec->orientation->qw);
+ newValues["frame"] = new Variant(vec->frame);
setDataFieldsFlatCopy(tcpName, newValues);
}
updateChannel(tcpName);
-
}
}
-void RobotStateObserver::getPoseDatafield_async(
+void
+RobotStateObserver::getPoseDatafield_async(
const AMD_RobotStateObserverInterface_getPoseDatafieldPtr& amd,
const std::string& nodeName,
const Ice::Current&) const
{
- addWorkerJob("RobotStateObserver::getPoseDatafield", [this, amd, nodeName]
- {
- return getDatafieldRefByName(TCP_POSE_CHANNEL, nodeName);
- });
+ addWorkerJob("RobotStateObserver::getPoseDatafield",
+ [this, amd, nodeName]
+ { return getDatafieldRefByName(TCP_POSE_CHANNEL, nodeName); });
}
-void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long timestampMicroSeconds)
+void
+RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long timestampMicroSeconds)
{
if (jointVel.empty())
{
@@ -206,17 +210,17 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long
// ARMARX_INFO << jointVel; FramedPoseBaseMap tcpPoses;
FramedDirectionMap tcpTranslationVelocities;
FramedDirectionMap tcpOrientationVelocities;
- std::string rootFrame = robot->getRootNode()->getName();
+ std::string rootFrame = robot->getRootNode()->getName();
NameValueMap tempJointAngles = robot->getConfig()->getRobotNodeJointValueMap();
FramedPoseBaseMap tcpPoses;
for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
RobotNodePtr node = nodesToReport.at(i).first;
- const std::string& tcpName = node->getName();
- const Eigen::Matrix4f& currentPose = velocityReportRobot->getRobotNode(tcpName)->getGlobalPose();
+ const std::string& tcpName = node->getName();
+ const Eigen::Matrix4f& currentPose =
+ velocityReportRobot->getRobotNode(tcpName)->getGlobalPose();
tcpPoses[tcpName] = new FramedPose(currentPose, GlobalFrame, "");
-
}
@@ -229,7 +233,8 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long
auto keys = armarx::getMapKeys(jointVel);
Eigen::VectorXf jointVelocities(jointVel.size());
auto rootFrameName = velocityReportRobot->getRootNode()->getName();
- RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(velocityReportRobot, "All_Nodes", keys, rootFrameName);
+ RobotNodeSetPtr rns =
+ RobotNodeSet::createRobotNodeSet(velocityReportRobot, "All_Nodes", keys, rootFrameName);
for (size_t i = 0; i < rns->getSize(); ++i)
{
jointVelocities(i) = jointVel.at(rns->getNode(i)->getName());
@@ -244,16 +249,18 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long
Eigen::MatrixXf jac = j->getJacobianMatrix(node, IKSolver::All);
Eigen::VectorXf nodeVel = jac * jointVelocities;
- const std::string tcpName = node->getName();
- tcpTranslationVelocities[tcpName] = new FramedDirection(nodeVel.block<3, 1>(0, 0), rootFrameName, robotName);
- tcpOrientationVelocities[tcpName] = new FramedDirection(nodeVel.block<3, 1>(3, 0), rootFrameName, robotName);
-
-
+ const std::string tcpName = node->getName();
+ tcpTranslationVelocities[tcpName] =
+ new FramedDirection(nodeVel.block<3, 1>(0, 0), rootFrameName, robotName);
+ tcpOrientationVelocities[tcpName] =
+ new FramedDirection(nodeVel.block<3, 1>(3, 0), rootFrameName, robotName);
}
updateVelocityDatafields(tcpTranslationVelocities, tcpOrientationVelocities);
}
-void RobotStateObserver::updateVelocityDatafields(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities)
+void
+RobotStateObserver::updateVelocityDatafields(const FramedDirectionMap& tcpTranslationVelocities,
+ const FramedDirectionMap& tcpOrientationVelocities)
{
FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();
@@ -275,7 +282,8 @@ void RobotStateObserver::updateVelocityDatafields(const FramedDirectionMap& tcpT
if (!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
{
- offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ offerDataFieldWithDefault(
+ TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
}
else
{
@@ -294,39 +302,39 @@ void RobotStateObserver::updateVelocityDatafields(const FramedDirectionMap& tcpT
offerDataFieldWithDefault(channelName, "roll", Variant(vecOri->x), "Roll");
offerDataFieldWithDefault(channelName, "pitch", Variant(vecOri->y), "Pitch");
offerDataFieldWithDefault(channelName, "yaw", Variant(vecOri->z), "Yaw");
- offerDataFieldWithDefault(channelName, "frame", Variant(vecOri->frame), "Reference Frame");
+ offerDataFieldWithDefault(
+ channelName, "frame", Variant(vecOri->frame), "Reference Frame");
}
else
{
StringVariantBaseMap newValues;
- newValues["x"] = new Variant(vec->x);
- newValues["y"] = new Variant(vec->y);
- newValues["z"] = new Variant(vec->z);
- newValues["roll"] = new Variant(vecOri->x);
- newValues["pitch"] = new Variant(vecOri->y);
- newValues["yaw"] = new Variant(vecOri->z);
- newValues["frame"] = new Variant(vec->frame);
+ newValues["x"] = new Variant(vec->x);
+ newValues["y"] = new Variant(vec->y);
+ newValues["z"] = new Variant(vec->z);
+ newValues["roll"] = new Variant(vecOri->x);
+ newValues["pitch"] = new Variant(vecOri->y);
+ newValues["yaw"] = new Variant(vecOri->z);
+ newValues["frame"] = new Variant(vec->frame);
setDataFieldsFlatCopy(channelName, newValues);
}
updateChannel(channelName);
-
}
}
-
-PropertyDefinitionsPtr RobotStateObserver::createPropertyDefinitions()
+PropertyDefinitionsPtr
+RobotStateObserver::createPropertyDefinitions()
{
- return PropertyDefinitionsPtr(new RobotStateObserverPropertyDefinitions(
- getConfigIdentifier()));
+ return PropertyDefinitionsPtr(new RobotStateObserverPropertyDefinitions(getConfigIdentifier()));
}
-void RobotStateObserver::setRobot(RobotPtr robot)
+void
+RobotStateObserver::setRobot(RobotPtr robot)
{
std::unique_lock lock(dataMutex);
this->robot = robot;
- std::vector< VirtualRobot::RobotNodeSetPtr > robotNodes;
+ std::vector<VirtualRobot::RobotNodeSetPtr> robotNodes;
robotNodes = robot->getRobotNodeSets();
std::string nodesetsString = getProperty<std::string>("TCPsToReport").getValue();
@@ -342,7 +350,8 @@ void RobotStateObserver::setRobot(RobotPtr robot)
{
if (set->getTCP())
{
- nodesToReport.push_back(std::make_pair(set->getTCP(), robot->getRootNode()->getName()));
+ nodesToReport.push_back(
+ std::make_pair(set->getTCP(), robot->getRootNode()->getName()));
}
}
}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
index 8f0899834450db1b5674bdebf0bfa2443ab4de8e..74bdb253926591fd04e2b13ca506e8f40702c36c 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
@@ -22,18 +22,19 @@
#pragma once
-#include <ArmarXCore/core/system/ImportExport.h>
+#include <string>
+
+#include <VirtualRobot/VirtualRobot.h>
+
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/system/ImportExport.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/interface/core/RobotStateObserverInterface.h>
-#include <RobotAPI/interface/core/RobotState.h>
#include <ArmarXCore/observers/ConditionCheck.h>
#include <ArmarXCore/observers/Observer.h>
-#include <VirtualRobot/VirtualRobot.h>
-
-#include <string>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/core/RobotStateObserverInterface.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
namespace armarx
{
@@ -41,18 +42,21 @@ namespace armarx
/**
* RobotStatePropertyDefinition Property Definitions
*/
- class RobotStateObserverPropertyDefinitions:
- public ObserverPropertyDefinitions
+ class RobotStateObserverPropertyDefinitions : public ObserverPropertyDefinitions
{
public:
- RobotStateObserverPropertyDefinitions(std::string prefix):
+ RobotStateObserverPropertyDefinitions(std::string prefix) :
ObserverPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("TCPsToReport", "", "comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none");
+ defineOptionalProperty<std::string>(
+ "TCPsToReport",
+ "",
+ "comma seperated list of nodesets' endeffectors, which poses and velocities that "
+ "should be reported. * for all, empty for none");
}
};
- using FramedPoseBaseMap = ::std::map< ::std::string, ::armarx::FramedPoseBasePtr>;
+ using FramedPoseBaseMap = ::std::map<::std::string, ::armarx::FramedPoseBasePtr>;
/**
* ArmarX RobotStateObserver.
@@ -77,34 +81,35 @@ namespace armarx
void setRobot(VirtualRobot::RobotPtr robot);
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotStateObserver";
}
void updatePoses();
void updateNodeVelocities(const NameValueMap& jointVel, long timestampMicroSeconds);
- protected:
- void updateVelocityDatafields(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities);
+ protected:
+ void updateVelocityDatafields(const FramedDirectionMap& tcpTranslationVelocities,
+ const FramedDirectionMap& tcpOrientationVelocities);
void udpatePoseDatafields(const FramedPoseBaseMap& poseMap);
+
private:
std::string robotNodeSetName;
RobotStateComponentInterfacePrx server;
- VirtualRobot::RobotPtr robot, velocityReportRobot;
- std::vector<std::pair<VirtualRobot::RobotNodePtr, std::string> > nodesToReport;
+ VirtualRobot::RobotPtr robot, velocityReportRobot;
+ std::vector<std::pair<VirtualRobot::RobotNodePtr, std::string>> nodesToReport;
std::recursive_mutex dataMutex;
IceUtil::Time lastVelocityUpdate;
// RobotStateObserverInterface interface
public:
- void getPoseDatafield_async(
- const AMD_RobotStateObserverInterface_getPoseDatafieldPtr& amd,
- const std::string& nodeName,
- const Ice::Current&) const override;
+ void getPoseDatafield_async(const AMD_RobotStateObserverInterface_getPoseDatafieldPtr& amd,
+ const std::string& nodeName,
+ const Ice::Current&) const override;
};
using RobotStateObserverPtr = IceInternal::Handle<RobotStateObserver>;
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp b/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp
index 6454d71cf3be8e080f86c329130598f889afd306..be144d554a67effca18661f08a8de80e4093cff7 100644
--- a/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/test/ArmarPoseTest.cpp
@@ -24,13 +24,14 @@
#define BOOST_TEST_MODULE RobotAPI::FramedPose::Test
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
+
#include <ArmarXCore/util/json/JSONObject.h>
+
#include <RobotAPI/libraries/core/FramedPose.h>
using namespace armarx;
-
BOOST_AUTO_TEST_CASE(complexVariantToDict)
{
armarx::JSONObjectPtr json = new JSONObject();
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
index 9c2f0fc609d47dfae5993d61d4d887be05e82b5b..4e3a63100c210aea2be1e06efabd5ab51af4c48c 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
@@ -22,33 +22,32 @@
#define BOOST_TEST_MODULE RobotAPI::CartesianVelocityController::Test
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
+#include "../CartesianVelocityController.h"
-#include <ArmarXCore/core/test/IceTestHelper.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <RobotAPI/Test.h>
-#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/IK/IKSolver.h>
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include "../CartesianVelocityController.h"
+#include <ArmarXCore/core/test/IceTestHelper.h>
using namespace armarx;
-void check_close(float a, float b, float tolerance)
+void
+check_close(float a, float b, float tolerance)
{
if (fabs(a - b) > tolerance)
{
- ARMARX_ERROR << "fabs(a-b) > tolerance. a=" << a << " b=" << b << " tolerance=" << tolerance;
+ ARMARX_ERROR << "fabs(a-b) > tolerance. a=" << a << " b=" << b
+ << " tolerance=" << tolerance;
BOOST_FAIL("");
}
}
-
-
-
BOOST_AUTO_TEST_CASE(test1)
{
const std::string project = "RobotAPI";
@@ -65,7 +64,8 @@ BOOST_AUTO_TEST_CASE(test1)
std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
ArmarXDataPath::getAbsolutePath(fn, fn);
std::string robotFilename = fn;
- VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
+ VirtualRobot::RobotPtr robot =
+ VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("HipYawRightArm");
CartesianVelocityControllerPtr h(new CartesianVelocityController(rns));
Eigen::VectorXf jointVel(8);
@@ -75,12 +75,12 @@ BOOST_AUTO_TEST_CASE(test1)
targetTcpVel << 0, 0, 0, 0, 0, 0;
ARMARX_IMPORTANT << rns->getJointValues();
- Eigen::VectorXf vel = h->calculate(targetTcpVel, jointVel, VirtualRobot::IKSolver::CartesianSelection::All);
+ Eigen::VectorXf vel =
+ h->calculate(targetTcpVel, jointVel, VirtualRobot::IKSolver::CartesianSelection::All);
ARMARX_IMPORTANT << vel.transpose();
Eigen::VectorXf tcpVel = (h->jacobi * vel).transpose();
ARMARX_IMPORTANT << tcpVel;
BOOST_CHECK_LE((targetTcpVel - tcpVel).norm(), 0.01f);
-
}
BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
@@ -99,9 +99,11 @@ BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
ArmarXDataPath::getAbsolutePath(fn, fn);
std::string robotFilename = fn;
- VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
+ VirtualRobot::RobotPtr robot =
+ VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("TorsoRightArm");
- CartesianVelocityControllerPtr h(new CartesianVelocityController(rns, nullptr, VirtualRobot::JacobiProvider::eSVDDamped));
+ CartesianVelocityControllerPtr h(
+ new CartesianVelocityController(rns, nullptr, VirtualRobot::JacobiProvider::eSVDDamped));
const Eigen::VectorXf oldJV = rns->getJointValuesEigen();
ARMARX_INFO << VAROUT(oldJV);
Eigen::VectorXf cartesianVel(6);
@@ -111,14 +113,17 @@ BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
{
- cartesianVel << rand() % 100, rand() % 100, rand() % 100, 0.01 * (rand() % 100), 0.01 * (rand() % 100), 0.01 * (rand() % 100);
+ cartesianVel << rand() % 100, rand() % 100, rand() % 100, 0.01 * (rand() % 100),
+ 0.01 * (rand() % 100), 0.01 * (rand() % 100);
// cartesianVel << 100,0,0;//, 0.01*(rand()%100), 0.01*(rand()%100), 0.01*(rand()%100);
auto mode = VirtualRobot::IKSolver::CartesianSelection::All;
Eigen::MatrixXf jacobi = h->ik->getJacobianMatrix(rns->getTCP(), mode);
- Eigen::MatrixXf inv = h->ik->computePseudoInverseJacobianMatrix(jacobi, h->ik->getJacobiRegularization(mode));
+ Eigen::MatrixXf inv =
+ h->ik->computePseudoInverseJacobianMatrix(jacobi, h->ik->getJacobiRegularization(mode));
int jointIndex = rand() % rns->getSize();
jacobi.block(0, jointIndex, jacobi.rows(), 1) = Eigen::VectorXf::Zero(jacobi.rows());
- Eigen::MatrixXf inv2 = h->ik->computePseudoInverseJacobianMatrix(jacobi, h->ik->getJacobiRegularization(mode));
+ Eigen::MatrixXf inv2 =
+ h->ik->computePseudoInverseJacobianMatrix(jacobi, h->ik->getJacobiRegularization(mode));
// ARMARX_INFO << "diff\n" << (inv-inv2);
rns->setJointValues(oldJV);
@@ -128,13 +133,14 @@ BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
Eigen::VectorXf jointVel = inv * cartesianVel;
rns->setJointValues(oldJV + jointVel * accuracy);
- Eigen::VectorXf resultCartesianVel = ((h->_tcp->getPositionInRootFrame() - posBefore) / accuracy);
-
+ Eigen::VectorXf resultCartesianVel =
+ ((h->_tcp->getPositionInRootFrame() - posBefore) / accuracy);
Eigen::VectorXf jointVel2 = inv2 * cartesianVel;
rns->setJointValues(oldJV + jointVel2 * accuracy);
- Eigen::VectorXf resultCartesianVel2 = ((h->_tcp->getPositionInRootFrame() - posBefore) / accuracy);
+ Eigen::VectorXf resultCartesianVel2 =
+ ((h->_tcp->getPositionInRootFrame() - posBefore) / accuracy);
Eigen::Vector3f diff = (resultCartesianVel - cartesianVel.head<3>());
Eigen::Vector3f diff2 = (resultCartesianVel2 - cartesianVel.head<3>());
@@ -145,8 +151,12 @@ BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
ARMARX_INFO << "jacobi\n" << jacobi;
ARMARX_INFO << "inv\n" << inv;
ARMARX_INFO << "inv2\n" << inv2;
- ARMARX_INFO << "\n\nResulting joint cart velocity: " << resultCartesianVel << "\n jointVel:\n" << jointVel;
- ARMARX_INFO << "\n\nResulting joint cart velocity with joint limit avoidance: " << resultCartesianVel2 << "\n jointVel:\n" << jointVel2;
+ ARMARX_INFO << "\n\nResulting joint cart velocity: " << resultCartesianVel
+ << "\n jointVel:\n"
+ << jointVel;
+ ARMARX_INFO << "\n\nResulting joint cart velocity with joint limit avoidance: "
+ << resultCartesianVel2 << "\n jointVel:\n"
+ << jointVel2;
ARMARX_INFO << "Diff:\n" << diff;
ARMARX_INFO << "Diff2:\n" << diff2;
@@ -158,6 +168,5 @@ BOOST_AUTO_TEST_CASE(testJointLimitAwareness)
BOOST_REQUIRE((diff - diff2).norm() < 1 || diff2.norm() < diff.norm());
-
}
}
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
index a4dcccdcec0d71c592dd0198f91e0e7beb2ebac7..def5f3e1ce444dffdc32b413cfb011dd4304f2c9 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
@@ -24,10 +24,13 @@
#define BOOST_TEST_MODULE RobotAPI::CartesianVelocityController::Test
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
-#include <ArmarXCore/core/test/IceTestHelper.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/test/IceTestHelper.h>
+
#include "../CartesianVelocityControllerWithRamp.h"
using namespace armarx;
@@ -47,16 +50,13 @@ BOOST_AUTO_TEST_CASE(CartesianVelocityRampTest)
target << 200, 0, 0, 3, 0, 0;
-
for (size_t i = 0; i < 30; i++)
{
state = c.update(target, dt);
ARMARX_IMPORTANT << "state:" << state.transpose();
}
-
}
-
BOOST_AUTO_TEST_CASE(test1)
{
const std::string project = "RobotAPI";
@@ -73,7 +73,8 @@ BOOST_AUTO_TEST_CASE(test1)
std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
ArmarXDataPath::getAbsolutePath(fn, fn);
std::string robotFilename = fn;
- VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
+ VirtualRobot::RobotPtr robot =
+ VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eStructure);
VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSet("HipYawRightArm");
@@ -84,8 +85,10 @@ BOOST_AUTO_TEST_CASE(test1)
Eigen::VectorXf targetTcpVel(6);
targetTcpVel << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
- CartesianVelocityControllerWithRampPtr h(new CartesianVelocityControllerWithRamp(rns, currentjointVel, VirtualRobot::IKSolver::CartesianSelection::All, 100, 1, 0.1));
- Eigen::MatrixXf jacobi = h->controller.ik->getJacobianMatrix(rns->getTCP(), VirtualRobot::IKSolver::CartesianSelection::All);
+ CartesianVelocityControllerWithRampPtr h(new CartesianVelocityControllerWithRamp(
+ rns, currentjointVel, VirtualRobot::IKSolver::CartesianSelection::All, 100, 1, 0.1));
+ Eigen::MatrixXf jacobi = h->controller.ik->getJacobianMatrix(
+ rns->getTCP(), VirtualRobot::IKSolver::CartesianSelection::All);
ARMARX_IMPORTANT << "Initial TCP Vel: " << (jacobi * currentjointVel).transpose();
@@ -108,5 +111,4 @@ BOOST_AUTO_TEST_CASE(test1)
ARMARX_IMPORTANT << tcpVel;
BOOST_CHECK_LE((targetTcpVel - tcpVel).norm(), 0.01f);
*/
-
}
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp
index 5bfd4e36b8216ae4367ea582e18f382e1d3cb06f..3225346ad9a870fb2fd34104a611c191ef88d4f1 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityRampTest.cpp
@@ -22,18 +22,20 @@
#define BOOST_TEST_MODULE RobotAPI::CartesianVelocityRamp::Test
#define ARMARX_BOOST_TEST
+#include "../CartesianVelocityRamp.h"
+
#include <RobotAPI/Test.h>
-#include <ArmarXCore/core/test/IceTestHelper.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include "../CartesianVelocityRamp.h"
+#include <ArmarXCore/core/test/IceTestHelper.h>
+
#include <RobotAPI/libraries/core/EigenHelpers.h>
using namespace armarx;
-
-
BOOST_AUTO_TEST_CASE(testBoth)
{
Eigen::VectorXf state = MakeVectorXf(0, 0, 0, 0, 0, 0);
@@ -49,5 +51,4 @@ BOOST_AUTO_TEST_CASE(testBoth)
Eigen::VectorXf expected = MakeVectorXf(10, 0, 0, 0, 0, 0);
BOOST_CHECK_LE((state - expected).norm(), 0.01f);
-
}
diff --git a/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
index bd36e530746f0fecd307e6b449a25f463c69a543..8f64c7b09ee8a54ee1fc7dfe3229f0f7e44e36be 100644
--- a/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
+++ b/source/RobotAPI/libraries/core/test/DebugDrawerTopicTest.cpp
@@ -35,7 +35,6 @@
using namespace armarx;
-
// PCL-like dummy types.
struct PointXYZ
@@ -61,49 +60,62 @@ private:
using VectorT = std::vector<PointT>;
public:
+ PointCloud()
+ {
+ }
- PointCloud() {}
- PointCloud(const VectorT& points) : points(points) {}
+ PointCloud(const VectorT& points) : points(points)
+ {
+ }
// Container methods.
- std::size_t size() const
+ std::size_t
+ size() const
{
return points.size();
}
- PointT& operator[](std::size_t i)
+ PointT&
+ operator[](std::size_t i)
{
return points[i];
}
- const PointT& operator[](std::size_t i) const
+
+ const PointT&
+ operator[](std::size_t i) const
{
return points[i];
}
// Iterators.
- typename VectorT::iterator begin()
+ typename VectorT::iterator
+ begin()
{
return points.begin();
}
- typename VectorT::const_iterator begin() const
+
+ typename VectorT::const_iterator
+ begin() const
{
return points.begin();
}
- typename VectorT::iterator end()
+
+ typename VectorT::iterator
+ end()
{
return points.end();
}
- typename VectorT::const_iterator end() const
+
+ typename VectorT::const_iterator
+ end() const
{
return points.end();
}
-
/// The points.
VectorT points;
};
-
/* These test do not actually check any behaviour,
* but check whether this code compiles.
*/
@@ -115,7 +127,7 @@ struct Fixture
{
}
- const DebugDrawerTopic::VisuID id {"layer", "name"};
+ const DebugDrawerTopic::VisuID id{"layer", "name"};
const int pointSize = 10;
DebugDrawerTopic drawer;
@@ -124,50 +136,50 @@ struct Fixture
const PointCloud<PointT>& pointCloud = pointCloudMutable;
};
-
BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZ, Fixture<PointXYZ>)
{
- pointCloudMutable.points = { {1, 2, 3}, {2, 3, 4}, {3, 4, 5} };
+ pointCloudMutable.points = {{1, 2, 3}, {2, 3, 4}, {3, 4, 5}};
drawer.drawPointCloud(id, pointCloud);
- drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
-
- drawer.drawPointCloud(id, pointCloud,
- [](const PointXYZ&)
- {
- return DrawColor{0, 0.5, 1, 1};
- }, pointSize);
+ drawer.drawPointCloud(id, pointCloud.points, DrawColor{0, 0.5, 1, 1});
+
+ drawer.drawPointCloud(
+ id,
+ pointCloud,
+ [](const PointXYZ&) {
+ return DrawColor{0, 0.5, 1, 1};
+ },
+ pointSize);
}
-
BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZRGBA, Fixture<PointXYZRGBA>)
{
drawer.drawPointCloud(id, pointCloud);
- drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
+ drawer.drawPointCloud(id, pointCloud.points, DrawColor{0, 0.5, 1, 1});
- drawer.drawPointCloud(id, pointCloud,
- [](const PointXYZRGBA&)
- {
- return DrawColor{0, 0.5, 1, 1};
- }, pointSize);
+ drawer.drawPointCloud(
+ id,
+ pointCloud,
+ [](const PointXYZRGBA&) {
+ return DrawColor{0, 0.5, 1, 1};
+ },
+ pointSize);
drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
}
-
BOOST_FIXTURE_TEST_CASE(test_drawPointCloud_PointXYZRGBL, Fixture<PointXYZRGBL>)
{
drawer.drawPointCloud(id, pointCloud);
- drawer.drawPointCloud(id, pointCloud.points, DrawColor {0, 0.5, 1, 1});
+ drawer.drawPointCloud(id, pointCloud.points, DrawColor{0, 0.5, 1, 1});
- drawer.drawPointCloud(id, pointCloud,
- [](const PointXYZRGBL&)
- {
- return DrawColor{0, 0.5, 1, 1};
- }, pointSize);
+ drawer.drawPointCloud(
+ id,
+ pointCloud,
+ [](const PointXYZRGBL&) {
+ return DrawColor{0, 0.5, 1, 1};
+ },
+ pointSize);
drawer.drawPointCloudRGBA(id, pointCloud, pointSize);
}
-
-
-
diff --git a/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp b/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
index 40e5d9aa6c9006701d6fadaa12d17586a1fd404f..c51464a8145231e3423036cbf366df2cc03070f7 100644
--- a/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
+++ b/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
@@ -22,18 +22,23 @@
#define BOOST_TEST_MODULE RobotAPI::PIDController::Test
#define ARMARX_BOOST_TEST
+#include "../math/MathUtils.h"
+
#include <RobotAPI/Test.h>
+
#include <ArmarXCore/core/test/IceTestHelper.h>
-#include "../math/MathUtils.h"
-#include "../math/ColorUtils.h"
+
#include "../FramedPose.h"
+#include "../math/ColorUtils.h"
using namespace armarx;
-void check_close(float a, float b, float tolerance)
+void
+check_close(float a, float b, float tolerance)
{
if (fabs(a - b) > tolerance)
{
- ARMARX_ERROR << "fabs(a-b) > tolerance. a=" << a << " b=" << b << " tolerance=" << tolerance;
+ ARMARX_ERROR << "fabs(a-b) > tolerance. a=" << a << " b=" << b
+ << " tolerance=" << tolerance;
BOOST_FAIL("");
}
}
@@ -53,16 +58,15 @@ BOOST_AUTO_TEST_CASE(eigenOutputTest)
ARMARX_INFO << "pose:\n" << p.output();
}
-
BOOST_AUTO_TEST_CASE(fmodTest)
{
float min = -22.0f / 180.f * M_PI;
float max = 200.0f / 180.f * M_PI;
- std::vector<float> values = { 190.0f, 185, min, max, 0};
+ std::vector<float> values = {190.0f, 185, min, max, 0};
for (auto value : values)
{
value /= 180.f / M_PI;
- std::vector<float> offsets = { -2 * M_PI, 0, 2 * M_PI};
+ std::vector<float> offsets = {-2 * M_PI, 0, 2 * M_PI};
for (auto offset : offsets)
{
@@ -73,7 +77,6 @@ BOOST_AUTO_TEST_CASE(fmodTest)
check_close(result, value, 0.001);
}
}
-
}
BOOST_AUTO_TEST_CASE(ColorUtilTest)
@@ -81,7 +84,8 @@ BOOST_AUTO_TEST_CASE(ColorUtilTest)
auto testColor = [](DrawColor24Bit rgb)
{
auto hsv = colorutils::RgbToHsv(rgb);
- ARMARX_INFO << VAROUT(rgb.r) << VAROUT(rgb.g) << VAROUT(rgb.b) << " --> " << VAROUT(hsv.h) << VAROUT(hsv.s) << VAROUT(hsv.v);
+ ARMARX_INFO << VAROUT(rgb.r) << VAROUT(rgb.g) << VAROUT(rgb.b) << " --> " << VAROUT(hsv.h)
+ << VAROUT(hsv.s) << VAROUT(hsv.v);
};
testColor({255, 0, 0});
testColor({0, 255, 0});
diff --git a/source/RobotAPI/libraries/core/test/PIDControllerTest.cpp b/source/RobotAPI/libraries/core/test/PIDControllerTest.cpp
index 8f0aff900e75372d0749decf9068ee9376f36351..0b9ae1ad6e3c80645c47b4180bf77e0a9cd1e076 100644
--- a/source/RobotAPI/libraries/core/test/PIDControllerTest.cpp
+++ b/source/RobotAPI/libraries/core/test/PIDControllerTest.cpp
@@ -22,14 +22,15 @@
#define BOOST_TEST_MODULE RobotAPI::PIDController::Test
#define ARMARX_BOOST_TEST
+#include "../PIDController.h"
+
#include <RobotAPI/Test.h>
+
#include <ArmarXCore/core/test/IceTestHelper.h>
#include <ArmarXCore/util/json/JSONObject.h>
-#include "../PIDController.h"
using namespace armarx;
-
BOOST_AUTO_TEST_CASE(PIDControllerTest)
{
PIDController c(1, 0, 0, 0.1, 0.1);
@@ -37,14 +38,15 @@ BOOST_AUTO_TEST_CASE(PIDControllerTest)
ARMARX_INFO << "velocity: " << c.getControlValue();
BOOST_CHECK_LE(c.getControlValue(), 0.1);
BOOST_CHECK_LE(c.controlValueDerivation, 0.11);
-
}
-void check_close(float a, float b, float tolerance)
+void
+check_close(float a, float b, float tolerance)
{
if (fabs(a - b) > tolerance)
{
- ARMARX_ERROR << "fabs(a-b) > tolerance. a=" << a << " b=" << b << " tolerance=" << tolerance;
+ ARMARX_ERROR << "fabs(a-b) > tolerance. a=" << a << " b=" << b
+ << " tolerance=" << tolerance;
BOOST_FAIL("");
}
}
diff --git a/source/RobotAPI/libraries/core/test/RobotTest.cpp b/source/RobotAPI/libraries/core/test/RobotTest.cpp
index b16bf6cbc4528994969fdc60b2f786f44f528252..4705b89d2b1dd733b9ad9f213df6fd7d5e27655a 100644
--- a/source/RobotAPI/libraries/core/test/RobotTest.cpp
+++ b/source/RobotAPI/libraries/core/test/RobotTest.cpp
@@ -25,17 +25,19 @@
#define BOOST_TEST_MODULE RobotAPI::RobotTest::Test
#define ARMARX_BOOST_TEST
#include <RobotAPI/Test.h>
+
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/test/IceTestHelper.h>
#include <ArmarXCore/util/json/JSONObject.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <VirtualRobot/XML/RobotIO.h>
+
#include "../RobotPool.h"
using namespace armarx;
-
BOOST_AUTO_TEST_CASE(RobotPoolTest)
{
const std::string project = "RobotAPI";
@@ -49,7 +51,8 @@ BOOST_AUTO_TEST_CASE(RobotPoolTest)
std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
ArmarXDataPath::getAbsolutePath(fn, fn);
std::string robotFilename = fn;
- VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eCollisionModel);
+ VirtualRobot::RobotPtr robot =
+ VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eCollisionModel);
RobotPool pool(robot, 2);
{
BOOST_CHECK_EQUAL(pool.getPoolSize(), 2);
@@ -70,6 +73,4 @@ BOOST_AUTO_TEST_CASE(RobotPoolTest)
BOOST_CHECK_EQUAL(pool.getRobotsInUseCount(), 0);
BOOST_CHECK_EQUAL(pool.clean(), 2);
BOOST_CHECK_EQUAL(pool.getPoolSize(), 0);
-
}
-
diff --git a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
index ebb9b863d36e761e30551e3960404105986639cd..831830c8c55e12433dd06363cc1582adbeb07fde 100644
--- a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
+++ b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
@@ -22,24 +22,30 @@
#define BOOST_TEST_MODULE RobotAPI::Trajectory::Test
#define ARMARX_BOOST_TEST
+#include "../Trajectory.h"
+
#include <RobotAPI/Test.h>
+
+#include <random>
+
#include <ArmarXCore/core/test/IceTestHelper.h>
#include <ArmarXCore/util/json/JSONObject.h>
-#include "../Trajectory.h"
-#include "../TrajectoryController.h"
+
#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <random>
-using namespace armarx;
+#include "../TrajectoryController.h"
+using namespace armarx;
BOOST_AUTO_TEST_CASE(TrajectoryBasicUsage)
{
//! [TrajectoryDocumentation BasicUsage]
- Ice::DoubleSeq joint1Values {0, 5, 1}; // fill with your values;
- Ice::DoubleSeq joint2Values {1, 3, 1}; // fill with your values;
- Ice::DoubleSeq joint3Values {0, 2, 5}; // fill with your values;
- Ice::DoubleSeq timestamps = Trajectory::GenerateTimestamps(0, 1, 1.0 / (joint1Values.size() - 1)); // if you dont have timestamps
- TrajectoryPtr trajectory(new Trajectory(DoubleSeqSeq {joint1Values, joint2Values}, timestamps, {"Shoulder 0 R", "Shoulder 1 R"}));
+ Ice::DoubleSeq joint1Values{0, 5, 1}; // fill with your values;
+ Ice::DoubleSeq joint2Values{1, 3, 1}; // fill with your values;
+ Ice::DoubleSeq joint3Values{0, 2, 5}; // fill with your values;
+ Ice::DoubleSeq timestamps = Trajectory::GenerateTimestamps(
+ 0, 1, 1.0 / (joint1Values.size() - 1)); // if you dont have timestamps
+ TrajectoryPtr trajectory(new Trajectory(
+ DoubleSeqSeq{joint1Values, joint2Values}, timestamps, {"Shoulder 0 R", "Shoulder 1 R"}));
trajectory->addDimension(joint3Values, timestamps, "Shoulder 2 R");
double timestamp = 0.1;
@@ -54,8 +60,9 @@ BOOST_AUTO_TEST_CASE(TrajectoryBasicUsage)
BOOST_AUTO_TEST_CASE(TrajectoryIteratorUsage)
{
- Ice::DoubleSeq myPositionValues {0, 5, 1}; // fill with your values;
- Ice::DoubleSeq timestamps = Trajectory::GenerateTimestamps(0, 1, 1.0 / (myPositionValues.size() - 1)); // if you dont have timestamps
+ Ice::DoubleSeq myPositionValues{0, 5, 1}; // fill with your values;
+ Ice::DoubleSeq timestamps = Trajectory::GenerateTimestamps(
+ 0, 1, 1.0 / (myPositionValues.size() - 1)); // if you dont have timestamps
TrajectoryPtr trajectory(new Trajectory);
trajectory->addDimension(myPositionValues, timestamps, "Shoulder 1 L");
//! [TrajectoryDocumentation IteratorUsage]
@@ -78,8 +85,8 @@ BOOST_AUTO_TEST_CASE(TrajectoryIteratorUsage)
BOOST_AUTO_TEST_CASE(testInplaceDerivation2)
{
- Ice::DoubleSeq timestamps { 0, 1, 2 };
- Ice::DoubleSeq positions { 1, 2, 5 };
+ Ice::DoubleSeq timestamps{0, 1, 2};
+ Ice::DoubleSeq positions{1, 2, 5};
TrajectoryPtr traj = new Trajectory(positions, timestamps);
TrajectoryPtr traj2 = new Trajectory(positions, timestamps);
@@ -90,8 +97,8 @@ BOOST_AUTO_TEST_CASE(testInplaceDerivation2)
BOOST_AUTO_TEST_CASE(testInplaceDerivation3)
{
- Ice::DoubleSeq timestamps { 0, 1, 2 };
- Ice::DoubleSeq positions { 1, 2, 5 };
+ Ice::DoubleSeq timestamps{0, 1, 2};
+ Ice::DoubleSeq positions{1, 2, 5};
TrajectoryPtr traj = new Trajectory(positions, timestamps);
TrajectoryPtr traj2 = new Trajectory(positions, timestamps);
@@ -102,8 +109,8 @@ BOOST_AUTO_TEST_CASE(testInplaceDerivation3)
BOOST_AUTO_TEST_CASE(testDerivationRemoval)
{
- Ice::DoubleSeq timestamps { 0, 1, 2 };
- Ice::DoubleSeq positions { 1, 2, 5 };
+ Ice::DoubleSeq timestamps{0, 1, 2};
+ Ice::DoubleSeq positions{1, 2, 5};
TrajectoryPtr traj = new Trajectory(positions, timestamps);
// TrajectoryPtr traj2 = new Trajectory(timestamps, positions);
@@ -113,13 +120,11 @@ BOOST_AUTO_TEST_CASE(testDerivationRemoval)
BOOST_CHECK_EQUAL(traj->begin()->getData().at(0)->size(), 1);
}
-
-
BOOST_AUTO_TEST_CASE(TrajectoryMarshalTest)
{
- Ice::DoubleSeq timestamps { 0, 1, 2 };
- Ice::DoubleSeq positions { 1, 2, 5 };
+ Ice::DoubleSeq timestamps{0, 1, 2};
+ Ice::DoubleSeq positions{1, 2, 5};
TrajectoryPtr traj = new Trajectory(positions, timestamps);
traj->ice_preMarshal();
@@ -172,7 +177,8 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
}
}
IceUtil::Time end = IceUtil::Time::now();
- ARMARX_INFO << "time per iteration in microseconds: " << (end - start).toMicroSecondsDouble() / traj->size();
+ ARMARX_INFO << "time per iteration in microseconds: "
+ << (end - start).toMicroSecondsDouble() / traj->size();
int iterationCount = 0;
start = IceUtil::Time::now();
for (float t = 0; t < 1000; t += 0.32)
@@ -184,7 +190,8 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
iterationCount++;
}
end = IceUtil::Time::now();
- ARMARX_INFO << "time per iteration with interpolation in microseconds: " << (end - start).toMicroSecondsDouble() / iterationCount;
+ ARMARX_INFO << "time per iteration with interpolation in microseconds: "
+ << (end - start).toMicroSecondsDouble() / iterationCount;
ARMARX_INFO << VAROUT(dummyVar);
BOOST_CHECK_EQUAL(traj->getState(0), traj2->getState(0));
}
@@ -217,16 +224,15 @@ BOOST_AUTO_TEST_CASE(TrajectoryUnmarshalTest)
traj->getLength(0, 50, 350);
}
-
BOOST_AUTO_TEST_CASE(TrajectorySerializationTest)
{
- FloatSeqSeq positions {{ 1, 2, 5 }, { 10, 2332, 53425 }};
+ FloatSeqSeq positions{{1, 2, 5}, {10, 2332, 53425}};
JSONObjectPtr json = new JSONObject();
- TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
+ TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
traj->serialize(json);
ARMARX_INFO_S << json->asString(true);
- TrajectoryPtr traj2 = new Trajectory();
+ TrajectoryPtr traj2 = new Trajectory();
traj2->deserialize(json);
BOOST_CHECK_CLOSE(traj->getState(0), traj2->getState(0), 0.01);
BOOST_CHECK_CLOSE(traj->getState(0.4, 1), traj2->getState(0.4, 1), 0.01);
@@ -236,7 +242,7 @@ BOOST_AUTO_TEST_CASE(TrajectorySerializationTest)
BOOST_AUTO_TEST_CASE(TrajectoryLengthTest)
{
- FloatSeqSeq jointtrajectories {{ 0, 0, 1 }, { 0, 1, 1 }};
+ FloatSeqSeq jointtrajectories{{0, 0, 1}, {0, 1, 1}};
TrajectoryPtr traj = new Trajectory(jointtrajectories, {}, {"joint1", "joint2"});
BOOST_CHECK_EQUAL(traj->getLength(), 2);
@@ -245,15 +251,15 @@ BOOST_AUTO_TEST_CASE(TrajectoryLengthTest)
BOOST_AUTO_TEST_CASE(TrajectoryCopyIfTest)
{
//! [TrajectoryDocumentation CopyIf]
- FloatSeqSeq positions {{ 0, 0, 1 }, { 0, 1, 1 }};
+ FloatSeqSeq positions{{0, 0, 1}, {0, 1, 1}};
TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
std::vector<Trajectory::TrajData> selection(traj->size());
// select all elements with velocity > 0
- auto it = std::copy_if(traj->begin(), traj->end(), selection.begin(),
- [](const Trajectory::TrajData & elem)
- {
- return fabs(elem.getDeriv(0, 1)) > 0;
- });
+ auto it = std::copy_if(traj->begin(),
+ traj->end(),
+ selection.begin(),
+ [](const Trajectory::TrajData& elem)
+ { return fabs(elem.getDeriv(0, 1)) > 0; });
selection.resize(std::distance(selection.begin(), it));
//! [TrajectoryDocumentation CopyIf]
ARMARX_INFO_S << VAROUT(*traj);
@@ -262,7 +268,7 @@ BOOST_AUTO_TEST_CASE(TrajectoryCopyIfTest)
BOOST_AUTO_TEST_CASE(TrajectoryControllerTest)
{
- FloatSeqSeq positions {{ 0, 0, 1 }, { 0, 1, 1 }};
+ FloatSeqSeq positions{{0, 0, 1}, {0, 1, 1}};
TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
TrajectoryController c(traj, 0, 1);
@@ -276,17 +282,17 @@ BOOST_AUTO_TEST_CASE(TrajectoryControllerTest)
while (t < 1.0f)
{
Eigen::VectorXf vel = c.update(deltaT, pos);
- pos += vel * deltaT * distribution(generator);
+ pos += vel * deltaT * distribution(generator);
ARMARX_INFO /*<< deactivateSpam(0.01)*/ << VAROUT(t) << VAROUT(pos);
t += deltaT;
}
-
-
}
BOOST_AUTO_TEST_CASE(TrajectoryControllerUnfoldingTest)
{
- BOOST_CHECK_LT(std::abs(armarx::math::MathUtils::AngleDelta(1.5 * M_PI, 0.25 * M_PI) / M_PI + armarx::math::MathUtils::AngleDelta(0.25 * M_PI, 1.5 * M_PI) / M_PI), 0.001);
+ BOOST_CHECK_LT(std::abs(armarx::math::MathUtils::AngleDelta(1.5 * M_PI, 0.25 * M_PI) / M_PI +
+ armarx::math::MathUtils::AngleDelta(0.25 * M_PI, 1.5 * M_PI) / M_PI),
+ 0.001);
Ice::FloatSeq waypoints = {3 * M_PI, 2.5 * M_PI, -2 * M_PI};
Ice::FloatSeq trajPoints;
float currentPoint = 0.0f;
@@ -302,8 +308,8 @@ BOOST_AUTO_TEST_CASE(TrajectoryControllerUnfoldingTest)
}
// ARMARX_INFO << VAROUT(trajPoints);
//FloatSeqSeq positions {{ 0, -M_PI * 0.99, -1.5 * M_PI * 0.99, 0, 0.1 * M_PI, - M_PI }/*, { -M_PI, 0, M_PI, 1.5 * M_PI, M_PI, - 2 * M_PI }*/};
- TrajectoryPtr origTraj = new Trajectory(FloatSeqSeq {trajPoints}, {}, {"joint1"/*, "joint2"*/});
- TrajectoryPtr traj = new Trajectory(FloatSeqSeq {trajPoints}, {}, {"joint1"/*, "joint2"*/});
+ TrajectoryPtr origTraj = new Trajectory(FloatSeqSeq{trajPoints}, {}, {"joint1" /*, "joint2"*/});
+ TrajectoryPtr traj = new Trajectory(FloatSeqSeq{trajPoints}, {}, {"joint1" /*, "joint2"*/});
traj->setLimitless({{true, -M_PI, M_PI}});
TrajectoryController::UnfoldLimitlessJointPositions(traj);
@@ -316,12 +322,11 @@ BOOST_AUTO_TEST_CASE(TrajectoryControllerUnfoldingTest)
auto pos = data.getPosition(0);
BOOST_CHECK(std::abs(pos - traj->getState(t, 0, 0)) < 0.01);
}
-
}
BOOST_AUTO_TEST_CASE(TrajectoryShiftTimeTest)
{
- FloatSeqSeq positions {{ 0, 0, 1 }, { 0, 1, 1 }};
+ FloatSeqSeq positions{{0, 0, 1}, {0, 1, 1}};
TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
ARMARX_INFO << VAROUT(traj->getDimensionNames());
auto startTime = traj->begin()->timestamp;
@@ -332,4 +337,3 @@ BOOST_AUTO_TEST_CASE(TrajectoryShiftTimeTest)
ARMARX_INFO << VAROUT(traj->getDimensionNames());
BOOST_CHECK_EQUAL(traj->getDimensionNames().at(0), "joint1");
}
-
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
index d9d6fa6514152ae4395ef9fd8d4574fc1340145a..dcc39aa7f7f8cd1ff38e089a77373b761fe65113 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp
@@ -1,141 +1,163 @@
#include "DebugDrawerTopic.h"
-#include <VirtualRobot/math/Helpers.h>
+#include <SimoxUtility/color/hsv.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
+#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/ManagedIceObject.h>
#include "../Pose.h"
-
#include "GlasbeyLUT.h"
-#include <SimoxUtility/color/hsv.h>
-
-
namespace armarx
{
DebugDrawerTopic::VisuID::VisuID() : VisuID("", "")
- {}
+ {
+ }
DebugDrawerTopic::VisuID::VisuID(const std::string& name) : VisuID("", name)
- {}
+ {
+ }
DebugDrawerTopic::VisuID::VisuID(const std::string& layer, const std::string& name) :
layer(layer), name(name)
- {}
+ {
+ }
- DebugDrawerTopic::VisuID DebugDrawerTopic::VisuID::withName(const std::string& newName) const
+ DebugDrawerTopic::VisuID
+ DebugDrawerTopic::VisuID::withName(const std::string& newName) const
{
return VisuID(this->layer, newName);
}
- std::ostream& operator<<(std::ostream& os, const DebugDrawerTopic::VisuID& rhs)
+ std::ostream&
+ operator<<(std::ostream& os, const DebugDrawerTopic::VisuID& rhs)
{
os << "Visu '" << rhs.name << "' on layer '" << rhs.layer << "'";
return os;
}
-
const std::string DebugDrawerTopic::TOPIC_NAME = "DebugDrawerUpdates";
const std::string DebugDrawerTopic::DEFAULT_LAYER = "debug";
const DebugDrawerTopic::Defaults DebugDrawerTopic::DEFAULTS = {};
- DebugDrawerTopic::DebugDrawerTopic(const std::string& layer) :
- _layer(layer)
- {}
+ DebugDrawerTopic::DebugDrawerTopic(const std::string& layer) : _layer(layer)
+ {
+ }
- DebugDrawerTopic::DebugDrawerTopic(
- const DebugDrawerInterfacePrx& topic, const std::string& layer) :
+ DebugDrawerTopic::DebugDrawerTopic(const DebugDrawerInterfacePrx& topic,
+ const std::string& layer) :
topic(topic), _layer(layer)
- {}
+ {
+ }
- void DebugDrawerTopic::setTopic(const DebugDrawerInterfacePrx& topic)
+ void
+ DebugDrawerTopic::setTopic(const DebugDrawerInterfacePrx& topic)
{
this->topic = topic;
}
- DebugDrawerInterfacePrx DebugDrawerTopic::getTopic() const
+ DebugDrawerInterfacePrx
+ DebugDrawerTopic::getTopic() const
{
return topic;
}
- void DebugDrawerTopic::setEnabled(bool enabled)
+ void
+ DebugDrawerTopic::setEnabled(bool enabled)
{
this->_enabled = enabled;
}
- bool DebugDrawerTopic::enabled() const
+ bool
+ DebugDrawerTopic::enabled() const
{
return _enabled && topic;
}
- void DebugDrawerTopic::offeringTopic(ManagedIceObject& component, const std::string& topicNameOverride) const
+ void
+ DebugDrawerTopic::offeringTopic(ManagedIceObject& component,
+ const std::string& topicNameOverride) const
{
component.offeringTopic(topicNameOverride.empty() ? TOPIC_NAME : topicNameOverride);
}
- void DebugDrawerTopic::getTopic(ManagedIceObject& component, const std::string& topicNameOverride)
+ void
+ DebugDrawerTopic::getTopic(ManagedIceObject& component, const std::string& topicNameOverride)
{
- setTopic(component.getTopic<DebugDrawerInterfacePrx>(topicNameOverride.empty() ? TOPIC_NAME : topicNameOverride));
+ setTopic(component.getTopic<DebugDrawerInterfacePrx>(
+ topicNameOverride.empty() ? TOPIC_NAME : topicNameOverride));
}
- const std::string& DebugDrawerTopic::getLayer() const
+ const std::string&
+ DebugDrawerTopic::getLayer() const
{
return _layer;
}
- void DebugDrawerTopic::setLayer(const std::string& layer)
+ void
+ DebugDrawerTopic::setLayer(const std::string& layer)
{
this->_layer = layer;
}
- float DebugDrawerTopic::getLengthScale() const
+ float
+ DebugDrawerTopic::getLengthScale() const
{
return _lengthScale;
}
- void DebugDrawerTopic::setLengthScale(float scale)
+ void
+ DebugDrawerTopic::setLengthScale(float scale)
{
this->_lengthScale = scale;
}
- void DebugDrawerTopic::setLengthScaleMetersToMillimeters()
+ void
+ DebugDrawerTopic::setLengthScaleMetersToMillimeters()
{
setLengthScale(1000);
}
- void DebugDrawerTopic::setLengthScaleMillimetersToMeters()
+ void
+ DebugDrawerTopic::setLengthScaleMillimetersToMeters()
{
setLengthScale(0.001f);
}
- float DebugDrawerTopic::getPoseScale() const
+ float
+ DebugDrawerTopic::getPoseScale() const
{
return _poseScale;
}
- void DebugDrawerTopic::setPoseScale(float scale)
+ void
+ DebugDrawerTopic::setPoseScale(float scale)
{
this->_poseScale = scale;
}
- void DebugDrawerTopic::setPoseScaleMeters()
+ void
+ DebugDrawerTopic::setPoseScaleMeters()
{
setPoseScale(0.001f);
}
- void DebugDrawerTopic::setPoseScaleMillimeters()
+ void
+ DebugDrawerTopic::setPoseScaleMillimeters()
{
setPoseScale(1);
}
- void DebugDrawerTopic::shortSleep()
+ void
+ DebugDrawerTopic::shortSleep()
{
this->sleepFor(_shortSleepDuration);
}
- void DebugDrawerTopic::clearAll(bool sleep)
+ void
+ DebugDrawerTopic::clearAll(bool sleep)
{
if (enabled())
{
@@ -147,12 +169,14 @@ namespace armarx
}
}
- void DebugDrawerTopic::clearLayer(bool sleep)
+ void
+ DebugDrawerTopic::clearLayer(bool sleep)
{
clearLayer(_layer, sleep);
}
- void DebugDrawerTopic::clearLayer(const std::string& layer, bool sleep)
+ void
+ DebugDrawerTopic::clearLayer(const std::string& layer, bool sleep)
{
if (enabled())
{
@@ -164,10 +188,13 @@ namespace armarx
}
}
- void DebugDrawerTopic::drawText(
- const VisuID& id, const Eigen::Vector3f& position, const std::string& text,
- int size, const DrawColor color,
- bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawText(const VisuID& id,
+ const Eigen::Vector3f& position,
+ const std::string& text,
+ int size,
+ const DrawColor color,
+ bool ignoreLengthScale)
{
if (enabled())
{
@@ -176,45 +203,67 @@ namespace armarx
}
}
- void DebugDrawerTopic::drawBox(
- const VisuID& id, const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
- const Eigen::Vector3f& extents, const DrawColor& color,
- bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawBox(const VisuID& id,
+ const Eigen::Vector3f& position,
+ const Eigen::Quaternionf& orientation,
+ const Eigen::Vector3f& extents,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (enabled())
{
const float scale = lengthScale(ignoreLengthScale);
- topic->setBoxVisu(layer(id), id.name,
+ topic->setBoxVisu(layer(id),
+ id.name,
new Pose(scaled(scale, position), new Quaternion(orientation)),
- scaled(scale, extents), color);
+ scaled(scale, extents),
+ color);
}
}
- void DebugDrawerTopic::drawBox(const VisuID& id, const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
- const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawBox(const VisuID& id,
+ const Eigen::Matrix4f& pose,
+ const Eigen::Vector3f& extents,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
- drawBox(id, ::math::Helpers::Position(pose), Eigen::Quaternionf(::math::Helpers::Orientation(pose)),
- extents, color, ignoreLengthScale);
+ drawBox(id,
+ ::math::Helpers::Position(pose),
+ Eigen::Quaternionf(::math::Helpers::Orientation(pose)),
+ extents,
+ color,
+ ignoreLengthScale);
}
- void DebugDrawerTopic::drawBox(const DebugDrawerTopic::VisuID& id, const VirtualRobot::BoundingBox& boundingBox,
- const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawBox(const DebugDrawerTopic::VisuID& id,
+ const VirtualRobot::BoundingBox& boundingBox,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
drawBox(id, boundingBox, Eigen::Matrix4f::Identity(), color, ignoreLengthScale);
}
- void DebugDrawerTopic::drawBox(
- const DebugDrawerTopic::VisuID& id,
- const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
- const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawBox(const DebugDrawerTopic::VisuID& id,
+ const VirtualRobot::BoundingBox& boundingBox,
+ const Eigen::Matrix4f& pose,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
const Eigen::Vector3f center = .5 * (boundingBox.getMin() + boundingBox.getMax());
- drawBox(id, ::math::Helpers::TransformPosition(pose, center),
+ drawBox(id,
+ ::math::Helpers::TransformPosition(pose, center),
Eigen::Quaternionf(::math::Helpers::Orientation(pose)),
- boundingBox.getMax() - boundingBox.getMin(), color, ignoreLengthScale);
+ boundingBox.getMax() - boundingBox.getMin(),
+ color,
+ ignoreLengthScale);
}
- void DebugDrawerTopic::removeBox(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::removeBox(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
{
@@ -222,19 +271,30 @@ namespace armarx
}
}
- void DebugDrawerTopic::drawBoxEdges(
- const DebugDrawerTopic::VisuID& id,
- const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
- const Eigen::Vector3f& extents,
- float width, const DrawColor& color, bool ignoreLengthScale)
- {
- drawBoxEdges(id, ::math::Helpers::Pose(position, orientation), extents, width, color, ignoreLengthScale);
+ void
+ DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& position,
+ const Eigen::Quaternionf& orientation,
+ const Eigen::Vector3f& extents,
+ float width,
+ const DrawColor& color,
+ bool ignoreLengthScale)
+ {
+ drawBoxEdges(id,
+ ::math::Helpers::Pose(position, orientation),
+ extents,
+ width,
+ color,
+ ignoreLengthScale);
}
- void DebugDrawerTopic::drawBoxEdges(
- const DebugDrawerTopic::VisuID& id,
- const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
- float width, const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Matrix4f& pose,
+ const Eigen::Vector3f& extents,
+ float width,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (!enabled())
{
@@ -245,12 +305,12 @@ namespace armarx
Eigen::Matrix<float, 3, 2> bb;
bb.col(0) = -extents / 2;
- bb.col(1) = extents / 2;
+ bb.col(1) = extents / 2;
auto addLine = [&](int x1, int y1, int z1, int x2, int y2, int z2)
{
- Eigen::Vector3f start = { bb.col(x1).x(), bb.col(y1).y(), bb.col(z1).z() };
- Eigen::Vector3f end = { bb.col(x2).x(), bb.col(y2).y(), bb.col(z2).z() };
+ Eigen::Vector3f start = {bb.col(x1).x(), bb.col(y1).y(), bb.col(z1).z()};
+ Eigen::Vector3f end = {bb.col(x2).x(), bb.col(y2).y(), bb.col(z2).z()};
start = ::math::Helpers::TransformPosition(pose, start);
end = ::math::Helpers::TransformPosition(pose, end);
@@ -292,91 +352,129 @@ namespace armarx
drawLineSet(id, points, width, color, ignoreLengthScale);
}
- void DebugDrawerTopic::drawBoxEdges(
- const DebugDrawerTopic::VisuID& id, const VirtualRobot::BoundingBox& boundingBox,
- float width, const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
+ const VirtualRobot::BoundingBox& boundingBox,
+ float width,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
drawBoxEdges(id, boundingBox, Eigen::Matrix4f::Identity(), width, color, ignoreLengthScale);
}
- void DebugDrawerTopic::drawBoxEdges(
- const DebugDrawerTopic::VisuID& id, const Eigen::Matrix32f& aabb,
- float width, const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Matrix32f& aabb,
+ float width,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
drawBoxEdges(id, aabb, Eigen::Matrix4f::Identity(), width, color, ignoreLengthScale);
}
- void DebugDrawerTopic::drawBoxEdges(
- const DebugDrawerTopic::VisuID& id,
- const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
- float width, const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
+ const VirtualRobot::BoundingBox& boundingBox,
+ const Eigen::Matrix4f& pose,
+ float width,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
const Eigen::Vector3f center = .5 * (boundingBox.getMin() + boundingBox.getMax());
- drawBoxEdges(id, ::math::Helpers::Pose(::math::Helpers::TransformPosition(pose, center),
- ::math::Helpers::Orientation(pose)),
+ drawBoxEdges(id,
+ ::math::Helpers::Pose(::math::Helpers::TransformPosition(pose, center),
+ ::math::Helpers::Orientation(pose)),
boundingBox.getMax() - boundingBox.getMin(),
- width, color, ignoreLengthScale);
+ width,
+ color,
+ ignoreLengthScale);
}
- void DebugDrawerTopic::drawBoxEdges(
- const DebugDrawerTopic::VisuID& id,
- const Eigen::Matrix32f& aabb, const Eigen::Matrix4f& pose,
- float width, const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawBoxEdges(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Matrix32f& aabb,
+ const Eigen::Matrix4f& pose,
+ float width,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
const Eigen::Vector3f center = 0.5 * (aabb.col(0) + aabb.col(1));
- drawBoxEdges(id, ::math::Helpers::Pose(::math::Helpers::TransformPosition(pose, center),
- ::math::Helpers::Orientation(pose)),
+ drawBoxEdges(id,
+ ::math::Helpers::Pose(::math::Helpers::TransformPosition(pose, center),
+ ::math::Helpers::Orientation(pose)),
aabb.col(1) - aabb.col(0),
- width, color, ignoreLengthScale);
+ width,
+ color,
+ ignoreLengthScale);
}
- void DebugDrawerTopic::removeboxEdges(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::removeboxEdges(const DebugDrawerTopic::VisuID& id)
{
removeLineSet(id);
}
-
- void DebugDrawerTopic::drawCylinder(
- const DebugDrawerTopic::VisuID& id,
- const Eigen::Vector3f& center, const Eigen::Vector3f& direction,
- float length, float radius,
- const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawCylinder(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& center,
+ const Eigen::Vector3f& direction,
+ float length,
+ float radius,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (enabled())
{
const float scale = lengthScale(ignoreLengthScale);
- topic->setCylinderVisu(layer(id), id.name, scaled(scale, center), new Vector3(direction),
- scaled(scale, length), scaled(scale, radius), color);
+ topic->setCylinderVisu(layer(id),
+ id.name,
+ scaled(scale, center),
+ new Vector3(direction),
+ scaled(scale, length),
+ scaled(scale, radius),
+ color);
}
}
- void DebugDrawerTopic::drawCylinder(
- const DebugDrawerTopic::VisuID& id,
- const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation,
- float length, float radius,
- const DrawColor& color, bool ignoreLengthScale)
- {
- drawCylinder(id, center, orientation * Eigen::Vector3f::UnitY(), radius, length, color,
+ void
+ DebugDrawerTopic::drawCylinder(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& center,
+ const Eigen::Quaternionf& orientation,
+ float length,
+ float radius,
+ const DrawColor& color,
+ bool ignoreLengthScale)
+ {
+ drawCylinder(id,
+ center,
+ orientation * Eigen::Vector3f::UnitY(),
+ radius,
+ length,
+ color,
ignoreLengthScale);
}
- void DebugDrawerTopic::drawCylinderFromTo(
- const DebugDrawerTopic::VisuID& id,
- const Eigen::Vector3f& from, const Eigen::Vector3f& to, float radius,
- const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawCylinderFromTo(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& from,
+ const Eigen::Vector3f& to,
+ float radius,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (enabled())
{
- const Eigen::Vector3f dir = (to - from); // no need for scaling at this point
+ const Eigen::Vector3f dir = (to - from); // no need for scaling at this point
const float length = dir.norm();
- drawCylinder(id, .5 * (from + to), dir / length, radius, length, color, ignoreLengthScale);
+ drawCylinder(
+ id, .5 * (from + to), dir / length, radius, length, color, ignoreLengthScale);
}
}
-
- void DebugDrawerTopic::removeCylinder(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::removeCylinder(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
{
@@ -384,22 +482,23 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::drawSphere(
- const DebugDrawerTopic::VisuID& id,
- const Eigen::Vector3f& center, float radius,
- const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawSphere(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& center,
+ float radius,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (enabled())
{
const float scale = lengthScale(ignoreLengthScale);
- topic->setSphereVisu(layer(id), id.name, scaled(scale, center), color,
- scaled(scale, radius));
+ topic->setSphereVisu(
+ layer(id), id.name, scaled(scale, center), color, scaled(scale, radius));
}
}
-
- void DebugDrawerTopic::removeSphere(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::removeSphere(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
{
@@ -407,36 +506,46 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::drawArrow(
- const VisuID& id,
- const Eigen::Vector3f& position, const Eigen::Vector3f& direction, float length,
- float width, const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawArrow(const VisuID& id,
+ const Eigen::Vector3f& position,
+ const Eigen::Vector3f& direction,
+ float length,
+ float width,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (enabled())
{
const float scale = lengthScale(ignoreLengthScale);
- topic->setArrowVisu(layer(id), id.name, scaled(scale, position), new Vector3(direction),
- color, scaled(scale, length), scaled(scale, width));
+ topic->setArrowVisu(layer(id),
+ id.name,
+ scaled(scale, position),
+ new Vector3(direction),
+ color,
+ scaled(scale, length),
+ scaled(scale, width));
}
}
-
- void DebugDrawerTopic::drawArrowFromTo(
- const VisuID& id,
- const Eigen::Vector3f& from, const Eigen::Vector3f& to,
- float width, const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawArrowFromTo(const VisuID& id,
+ const Eigen::Vector3f& from,
+ const Eigen::Vector3f& to,
+ float width,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (enabled())
{
- const Eigen::Vector3f dir = (to - from); // no need for scaling at this point
+ const Eigen::Vector3f dir = (to - from); // no need for scaling at this point
const float length = dir.norm();
drawArrow(id, from, dir / length, length, width, color, ignoreLengthScale);
}
}
-
- void DebugDrawerTopic::removeArrow(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::removeArrow(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
{
@@ -444,11 +553,13 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::drawPolygon(const VisuID& id,
- const std::vector<Eigen::Vector3f>& points,
- const DrawColor& colorFace, float lineWidth, const DrawColor& colorEdge,
- bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawPolygon(const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ const DrawColor& colorFace,
+ float lineWidth,
+ const DrawColor& colorEdge,
+ bool ignoreLengthScale)
{
if (enabled())
{
@@ -462,13 +573,12 @@ namespace armarx
polyPoints.push_back(scaled(scale, point));
}
- topic->setPolygonVisu(layer(id), id.name, polyPoints,
- colorFace, colorEdge, lineWidth);
+ topic->setPolygonVisu(layer(id), id.name, polyPoints, colorFace, colorEdge, lineWidth);
}
}
-
- void DebugDrawerTopic::removePolygon(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::removePolygon(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
{
@@ -476,22 +586,25 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::drawLine(
- const VisuID& id, const Eigen::Vector3f& from, const Eigen::Vector3f& to,
- float width, const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawLine(const VisuID& id,
+ const Eigen::Vector3f& from,
+ const Eigen::Vector3f& to,
+ float width,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (enabled())
{
const float scale = lengthScale(ignoreLengthScale);
- topic->setLineVisu(layer(id), id.name, scaled(scale, from), scaled(scale, to),
- width, color);
+ topic->setLineVisu(
+ layer(id), id.name, scaled(scale, from), scaled(scale, to), width, color);
}
}
-
- void DebugDrawerTopic::removeLine(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::removeLine(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
{
@@ -499,9 +612,10 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::drawLineSet(
- const VisuID& id, const DebugDrawerLineSet& lineSet, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawLineSet(const VisuID& id,
+ const DebugDrawerLineSet& lineSet,
+ bool ignoreLengthScale)
{
if (enabled())
{
@@ -525,10 +639,12 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::drawLineSet(
- const VisuID& id, const std::vector<Eigen::Vector3f>& points,
- float width, const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawLineSet(const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ float width,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (enabled())
{
@@ -548,11 +664,14 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::drawLineSet(
- const VisuID& id, const std::vector<Eigen::Vector3f>& points, float width,
- const DrawColor& colorA, const DrawColor& colorB, const std::vector<float>& intensitiesB,
- bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawLineSet(const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ float width,
+ const DrawColor& colorA,
+ const DrawColor& colorB,
+ const std::vector<float>& intensitiesB,
+ bool ignoreLengthScale)
{
if (enabled())
{
@@ -573,8 +692,8 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::removeLineSet(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::removeLineSet(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
{
@@ -582,32 +701,34 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::drawPose(
- const VisuID& id, const Eigen::Matrix4f& pose, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawPose(const VisuID& id,
+ const Eigen::Matrix4f& pose,
+ bool ignoreLengthScale)
{
drawPose(id, pose, _poseScale, ignoreLengthScale);
}
-
- void DebugDrawerTopic::drawPose(
- const VisuID& id,
- const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
- bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawPose(const VisuID& id,
+ const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
+ bool ignoreLengthScale)
{
drawPose(id, ::math::Helpers::Pose(pos, ori), _poseScale, ignoreLengthScale);
}
-
- void DebugDrawerTopic::drawPose(
- const VisuID& id, const Eigen::Matrix4f& pose, float scale,
- bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawPose(const VisuID& id,
+ const Eigen::Matrix4f& pose,
+ float scale,
+ bool ignoreLengthScale)
{
if (enabled())
{
const float lenghtScale = lengthScale(ignoreLengthScale);
- if (scale >= 1 && scale <= 1) // squelch compiler warning that == is unsafe
+ if (scale >= 1 && scale <= 1) // squelch compiler warning that == is unsafe
{
topic->setPoseVisu(layer(id), id.name, scaled(lenghtScale, pose));
}
@@ -616,19 +737,20 @@ namespace armarx
topic->setScaledPoseVisu(layer(id), id.name, scaled(lenghtScale, pose), scale);
}
}
-
}
-
- void DebugDrawerTopic::drawPose(
- const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
- float scale, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawPose(const VisuID& id,
+ const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
+ float scale,
+ bool ignoreLengthScale)
{
drawPose(id, ::math::Helpers::Pose(pos, ori), scale, ignoreLengthScale);
}
-
- void DebugDrawerTopic::removePose(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::removePose(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
{
@@ -636,10 +758,11 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::drawRobot(const DebugDrawerTopic::VisuID& id,
- const std::string& robotFile, const std::string& armarxProject,
- DrawStyle drawStyle)
+ void
+ DebugDrawerTopic::drawRobot(const DebugDrawerTopic::VisuID& id,
+ const std::string& robotFile,
+ const std::string& armarxProject,
+ DrawStyle drawStyle)
{
if (enabled())
{
@@ -647,10 +770,10 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::updateRobotPose(
- const DebugDrawerTopic::VisuID& id,
- const Eigen::Matrix4f& pose, bool ignoreScale)
+ void
+ DebugDrawerTopic::updateRobotPose(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Matrix4f& pose,
+ bool ignoreScale)
{
if (enabled())
{
@@ -658,17 +781,18 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::updateRobotPose(
- const DebugDrawerTopic::VisuID& id,
- const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori, bool ignoreScale)
+ void
+ DebugDrawerTopic::updateRobotPose(const DebugDrawerTopic::VisuID& id,
+ const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
+ bool ignoreScale)
{
updateRobotPose(id, ::math::Helpers::Pose(pos, ori), ignoreScale);
}
-
- void DebugDrawerTopic::updateRobotConfig(
- const DebugDrawerTopic::VisuID& id, const std::map<std::string, float>& config)
+ void
+ DebugDrawerTopic::updateRobotConfig(const DebugDrawerTopic::VisuID& id,
+ const std::map<std::string, float>& config)
{
if (enabled())
{
@@ -676,9 +800,8 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::updateRobotColor(
- const DebugDrawerTopic::VisuID& id, const DrawColor& color)
+ void
+ DebugDrawerTopic::updateRobotColor(const DebugDrawerTopic::VisuID& id, const DrawColor& color)
{
if (enabled())
{
@@ -686,10 +809,10 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::updateRobotNodeColor(
- const DebugDrawerTopic::VisuID& id,
- const std::string& nodeName, const DrawColor& color)
+ void
+ DebugDrawerTopic::updateRobotNodeColor(const DebugDrawerTopic::VisuID& id,
+ const std::string& nodeName,
+ const DrawColor& color)
{
if (enabled())
{
@@ -697,8 +820,8 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::removeRobot(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::removeRobot(const DebugDrawerTopic::VisuID& id)
{
if (enabled())
{
@@ -706,10 +829,11 @@ namespace armarx
}
}
-
- void DebugDrawerTopic::drawTriMesh(
- const VisuID& id, const VirtualRobot::TriMeshModel& triMesh,
- const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawTriMesh(const VisuID& id,
+ const VirtualRobot::TriMeshModel& triMesh,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (!enabled())
{
@@ -724,13 +848,13 @@ namespace armarx
for (const auto& vertex : triMesh.vertices)
{
auto scaled = vertex * scale;
- dd.vertices.push_back({ scaled.x(), scaled.y(), scaled.z() });
+ dd.vertices.push_back({scaled.x(), scaled.y(), scaled.z()});
}
const std::size_t normalBase = dd.vertices.size();
for (const auto& normal : triMesh.normals)
{
- dd.vertices.push_back({ normal.x(), normal.y(), normal.z() });
+ dd.vertices.push_back({normal.x(), normal.y(), normal.z()});
}
for (const auto& face : triMesh.faces)
@@ -744,10 +868,7 @@ namespace armarx
ddf.vertex1.normalID = ddf.vertex2.normalID = ddf.vertex3.normalID = -1;
bool validNormalIDs = true;
- for (const auto& id :
- {
- face.idNormal1, face.idNormal2, face.idNormal3
- })
+ for (const auto& id : {face.idNormal1, face.idNormal2, face.idNormal3})
{
validNormalIDs &= id < triMesh.normals.size();
}
@@ -761,7 +882,7 @@ namespace armarx
else
{
const Eigen::Vector3f& normal = face.normal;
- ddf.normal = { normal.x(), normal.y(), normal.z() };
+ ddf.normal = {normal.x(), normal.y(), normal.z()};
}
dd.faces.push_back(ddf);
@@ -770,21 +891,31 @@ namespace armarx
topic->setTriMeshVisu(layer(id), id.name, dd);
}
-
- void DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
- const VirtualRobot::TriMeshModel& trimesh,
- const DrawColor& colorFace, float lineWidth, const DrawColor& colorEdge,
- bool ignoreLengthScale)
- {
- drawTriMeshAsPolygons(id, trimesh, Eigen::Matrix4f::Identity(),
- colorFace, lineWidth, colorEdge, ignoreLengthScale);
- }
-
-
- void DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
- const VirtualRobot::TriMeshModel& trimesh, const Eigen::Matrix4f& pose,
- const DrawColor& colorFace, float lineWidth, const DrawColor& colorEdge,
- bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh,
+ const DrawColor& colorFace,
+ float lineWidth,
+ const DrawColor& colorEdge,
+ bool ignoreLengthScale)
+ {
+ drawTriMeshAsPolygons(id,
+ trimesh,
+ Eigen::Matrix4f::Identity(),
+ colorFace,
+ lineWidth,
+ colorEdge,
+ ignoreLengthScale);
+ }
+
+ void
+ DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh,
+ const Eigen::Matrix4f& pose,
+ const DrawColor& colorFace,
+ float lineWidth,
+ const DrawColor& colorEdge,
+ bool ignoreLengthScale)
{
if (!enabled())
{
@@ -794,10 +925,8 @@ namespace armarx
const float scale = lengthScale(ignoreLengthScale);
bool isIdentity = pose.isIdentity();
- auto toVector3 = [&scale, &isIdentity, &pose](const Eigen::Vector3f & v)
- {
- return scaled(scale, isIdentity ? v : ::math::Helpers::TransformPosition(pose, v));
- };
+ auto toVector3 = [&scale, &isIdentity, &pose](const Eigen::Vector3f& v)
+ { return scaled(scale, isIdentity ? v : ::math::Helpers::TransformPosition(pose, v)); };
ARMARX_INFO << "Drawing trimesh as polygons";
@@ -805,25 +934,27 @@ namespace armarx
for (std::size_t i = 0; i < trimesh.faces.size(); ++i)
{
const auto& face = trimesh.faces[i];
- PolygonPointList points
- {
- toVector3(trimesh.vertices.at(face.id1)),
- toVector3(trimesh.vertices.at(face.id2)),
- toVector3(trimesh.vertices.at(face.id3))
- };
-
- topic->setPolygonVisu(layer(id), id.name + "_" + std::to_string(counter), points,
- colorFace, colorEdge, lineWidth);
+ PolygonPointList points{toVector3(trimesh.vertices.at(face.id1)),
+ toVector3(trimesh.vertices.at(face.id2)),
+ toVector3(trimesh.vertices.at(face.id3))};
+
+ topic->setPolygonVisu(layer(id),
+ id.name + "_" + std::to_string(counter),
+ points,
+ colorFace,
+ colorEdge,
+ lineWidth);
++counter;
}
}
-
- void DebugDrawerTopic::drawTriMeshAsPolygons(
- const VisuID& id,
- const VirtualRobot::TriMeshModel& trimesh,
- const std::vector<DrawColor>& faceColorsInner, float lineWidth,
- const DrawColor& colorEdge, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawTriMeshAsPolygons(const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh,
+ const std::vector<DrawColor>& faceColorsInner,
+ float lineWidth,
+ const DrawColor& colorEdge,
+ bool ignoreLengthScale)
{
if (!enabled())
{
@@ -836,21 +967,22 @@ namespace armarx
for (std::size_t i = 0; i < trimesh.faces.size(); ++i)
{
const auto& face = trimesh.faces[i];
- PolygonPointList points
- {
- scaled(scale, trimesh.vertices[face.id1]),
- scaled(scale, trimesh.vertices[face.id2]),
- scaled(scale, trimesh.vertices[face.id3])
- };
-
- topic->setPolygonVisu(layer(id), id.name + "_" + std::to_string(i), points,
- faceColorsInner.at(i), colorEdge, lineWidth);
+ PolygonPointList points{scaled(scale, trimesh.vertices[face.id1]),
+ scaled(scale, trimesh.vertices[face.id2]),
+ scaled(scale, trimesh.vertices[face.id3])};
+
+ topic->setPolygonVisu(layer(id),
+ id.name + "_" + std::to_string(i),
+ points,
+ faceColorsInner.at(i),
+ colorEdge,
+ lineWidth);
}
}
- void DebugDrawerTopic::drawPointCloud(
- const DebugDrawerTopic::VisuID& id,
- const DebugDrawerPointCloud& pointCloud)
+ void
+ DebugDrawerTopic::drawPointCloud(const DebugDrawerTopic::VisuID& id,
+ const DebugDrawerPointCloud& pointCloud)
{
if (enabled())
{
@@ -858,9 +990,9 @@ namespace armarx
}
}
- void DebugDrawerTopic::drawPointCloud(
- const DebugDrawerTopic::VisuID& id,
- const DebugDrawerColoredPointCloud& pointCloud)
+ void
+ DebugDrawerTopic::drawPointCloud(const DebugDrawerTopic::VisuID& id,
+ const DebugDrawerColoredPointCloud& pointCloud)
{
if (enabled())
{
@@ -868,9 +1000,9 @@ namespace armarx
}
}
- void DebugDrawerTopic::drawPointCloud(
- const DebugDrawerTopic::VisuID& id,
- const DebugDrawer24BitColoredPointCloud& pointCloud)
+ void
+ DebugDrawerTopic::drawPointCloud(const DebugDrawerTopic::VisuID& id,
+ const DebugDrawer24BitColoredPointCloud& pointCloud)
{
if (enabled())
{
@@ -878,16 +1010,20 @@ namespace armarx
}
}
- void DebugDrawerTopic::clearColoredPointCloud(const DebugDrawerTopic::VisuID& id)
+ void
+ DebugDrawerTopic::clearColoredPointCloud(const DebugDrawerTopic::VisuID& id)
{
// Draw an empty point cloud.
drawPointCloud(id, DebugDrawerColoredPointCloud{});
}
- void DebugDrawerTopic::drawFloor(
- const VisuID& id,
- const Eigen::Vector3f& at, const Eigen::Vector3f& up, float size,
- const DrawColor& color, bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawFloor(const VisuID& id,
+ const Eigen::Vector3f& at,
+ const Eigen::Vector3f& up,
+ float size,
+ const DrawColor& color,
+ bool ignoreLengthScale)
{
if (!enabled())
{
@@ -921,28 +1057,32 @@ namespace armarx
drawPolygon(id, points, color, ignoreLengthScale);
}
-
- const std::string& DebugDrawerTopic::layer(const std::string& passedLayer) const
+ const std::string&
+ DebugDrawerTopic::layer(const std::string& passedLayer) const
{
return passedLayer.empty() ? _layer : passedLayer;
}
- const std::string& DebugDrawerTopic::layer(const VisuID& id) const
+ const std::string&
+ DebugDrawerTopic::layer(const VisuID& id) const
{
return layer(id.layer);
}
- float DebugDrawerTopic::lengthScale(bool ignore) const
+ float
+ DebugDrawerTopic::lengthScale(bool ignore) const
{
return ignore ? 1 : _lengthScale;
}
- float DebugDrawerTopic::scaled(float scale, float value)
+ float
+ DebugDrawerTopic::scaled(float scale, float value)
{
return scale * value;
}
- Vector3BasePtr DebugDrawerTopic::scaled(float scale, const Eigen::Vector3f& vector)
+ Vector3BasePtr
+ DebugDrawerTopic::scaled(float scale, const Eigen::Vector3f& vector)
{
if (scale >= 1 && scale <= 1)
{
@@ -954,7 +1094,8 @@ namespace armarx
}
}
- PoseBasePtr DebugDrawerTopic::scaled(float scale, const Eigen::Matrix4f& pose)
+ PoseBasePtr
+ DebugDrawerTopic::scaled(float scale, const Eigen::Matrix4f& pose)
{
if (scale >= 1 && scale <= 1)
{
@@ -968,57 +1109,62 @@ namespace armarx
}
}
-
DebugDrawerTopic::operator bool() const
{
return enabled();
}
- armarx::DebugDrawerTopic::operator DebugDrawerInterfacePrx& ()
+ armarx::DebugDrawerTopic::operator DebugDrawerInterfacePrx&()
{
return topic;
}
- armarx::DebugDrawerTopic::operator const DebugDrawerInterfacePrx& () const
+ armarx::DebugDrawerTopic::operator const DebugDrawerInterfacePrx&() const
{
return topic;
}
- DebugDrawerInterfacePrx& DebugDrawerTopic::operator->()
+ DebugDrawerInterfacePrx&
+ DebugDrawerTopic::operator->()
{
return topic;
}
- const DebugDrawerInterfacePrx& DebugDrawerTopic::operator->() const
+ const DebugDrawerInterfacePrx&
+ DebugDrawerTopic::operator->() const
{
return topic;
}
- Eigen::Vector3f DebugDrawerTopic::rgb2hsv(const Eigen::Vector3f& rgb)
+ Eigen::Vector3f
+ DebugDrawerTopic::rgb2hsv(const Eigen::Vector3f& rgb)
{
return simox::color::rgb_to_hsv(rgb);
}
- Eigen::Vector3f DebugDrawerTopic::hsv2rgb(const Eigen::Vector3f& hsv)
+ Eigen::Vector3f
+ DebugDrawerTopic::hsv2rgb(const Eigen::Vector3f& hsv)
{
return simox::color::hsv_to_rgb(hsv);
}
-
- DrawColor DebugDrawerTopic::getGlasbeyLUTColor(int id, float alpha)
+ DrawColor
+ DebugDrawerTopic::getGlasbeyLUTColor(int id, float alpha)
{
return GlasbeyLUT::at(id, alpha);
}
- DrawColor DebugDrawerTopic::getGlasbeyLUTColor(uint32_t id, float alpha)
+ DrawColor
+ DebugDrawerTopic::getGlasbeyLUTColor(uint32_t id, float alpha)
{
return GlasbeyLUT::at(id, alpha);
}
- DrawColor DebugDrawerTopic::getGlasbeyLUTColor(std::size_t id, float alpha)
+ DrawColor
+ DebugDrawerTopic::getGlasbeyLUTColor(std::size_t id, float alpha)
{
return GlasbeyLUT::at(id, alpha);
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
index c707e5cb242660136f2b0e4f0fd42ad68f5d617d..d29e578e10633f77bd33ec660bc3fb6bb144cb8e 100644
--- a/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
+++ b/source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h
@@ -19,20 +19,19 @@ namespace Eigen
* Accordingly, the rows each contain the limits in x, y, z direction.
*/
using Matrix32f = Matrix<float, 3, 2>;
-}
+} // namespace Eigen
namespace VirtualRobot
{
class TriMeshModel;
class BoundingBox;
-}
+} // namespace VirtualRobot
namespace armarx
{
// forward declaration
class ManagedIceObject;
-
/**
* @brief The `DebugDrawerTopic` wraps a `DebugDrawerInterfacePrx` and
* provides a more useful interface than the Ice interface.
@@ -152,8 +151,6 @@ namespace armarx
class DebugDrawerTopic
{
public:
-
-
/**
* @brief A visualisation ID.
*
@@ -175,7 +172,6 @@ namespace armarx
struct VisuID
{
public:
-
/// Empty constructor.
VisuID();
@@ -187,8 +183,8 @@ namespace armarx
/// Construct a VisuID from a non-std::string source (e.g. char[]).
template <typename Source>
VisuID(const Source& name) : VisuID(std::string(name))
- {}
-
+ {
+ }
/// Get a `VisuID` with the given name and same layer as `*this.
VisuID withName(const std::string& name) const;
@@ -197,49 +193,48 @@ namespace armarx
friend std::ostream& operator<<(std::ostream& os, const VisuID& rhs);
public:
-
- std::string layer = ""; ///< The layer name (empty by default).
- std::string name = ""; ///< The visu name (empty by default).
+ std::string layer = ""; ///< The layer name (empty by default).
+ std::string name = ""; ///< The visu name (empty by default).
};
-
/// Default values for drawing functions.
struct Defaults
{
- DrawColor colorText { 0, 0, 0, 1 };
+ DrawColor colorText{0, 0, 0, 1};
- DrawColor colorArrow { 1, .5, 0, 1 };
- DrawColor colorBox { 1, 0, 0, 1 };
- DrawColor colorCylinder { 0, 1, 0, 1 };
- DrawColor colorLine { .5, 0, 0, 1 };
- DrawColor colorSphere { 0, 0, 1, 1 };
+ DrawColor colorArrow{1, .5, 0, 1};
+ DrawColor colorBox{1, 0, 0, 1};
+ DrawColor colorCylinder{0, 1, 0, 1};
+ DrawColor colorLine{.5, 0, 0, 1};
+ DrawColor colorSphere{0, 0, 1, 1};
- DrawColor colorPolygonFace { 0, 1, 1, 1 };
- DrawColor colorPolygonEdge { .75, .75, .75, 1 };
+ DrawColor colorPolygonFace{0, 1, 1, 1};
+ DrawColor colorPolygonEdge{.75, .75, .75, 1};
- DrawColor colorFloor { .1f, .1f, .1f, 1 };
+ DrawColor colorFloor{.1f, .1f, .1f, 1};
- DrawColor colorPointCloud { .5, .5, .5, 1. };
+ DrawColor colorPointCloud{.5, .5, .5, 1.};
// Default value of DebugDrawerColoredPointCloud etc.
float pointCloudPointSize = 3.0f;
float lineWidth = 2;
- DrawColor boxEdgesColor { 0, 1, 1, 1 };
+ DrawColor boxEdgesColor{0, 1, 1, 1};
float boxEdgesWidth = 2;
};
+
static const Defaults DEFAULTS;
public:
-
// CONSTRUCTION & SETUP
/// Construct without topic, and optional layer.
DebugDrawerTopic(const std::string& layer = DEFAULT_LAYER);
/// Construct with given topic and optional layer.
- DebugDrawerTopic(const DebugDrawerInterfacePrx& topic, const std::string& layer = DEFAULT_LAYER);
+ DebugDrawerTopic(const DebugDrawerInterfacePrx& topic,
+ const std::string& layer = DEFAULT_LAYER);
/// Set the topic.
@@ -261,7 +256,8 @@ namespace armarx
* @param topicNameOverride Optional override for the topic name. If left empty (default),
* uses the standard topic name (see `TOPIC_NAME`).
*/
- void offeringTopic(ManagedIceObject& component, const std::string& topicNameOverride = "") const;
+ void offeringTopic(ManagedIceObject& component,
+ const std::string& topicNameOverride = "") const;
/**
* @brief Get the topic by calling getTopic([topicName]) on the given component.
* @param component The component (`*this` when called from a component).
@@ -295,7 +291,6 @@ namespace armarx
void setPoseScaleMillimeters();
-
// SLEEP
/// Sleep for the `shortSleepDuration`. Useful after clearing.
@@ -331,17 +326,25 @@ namespace armarx
* @brief Draw text at the specified position.
* @param size the text size (in pixels, not affected by length scaling)
*/
- void drawText(const VisuID& id, const Eigen::Vector3f& position, const std::string& text,
- int size = 10, const DrawColor color = DEFAULTS.colorText,
+ void drawText(const VisuID& id,
+ const Eigen::Vector3f& position,
+ const std::string& text,
+ int size = 10,
+ const DrawColor color = DEFAULTS.colorText,
bool ignoreLengthScale = false);
/// Draw a box.
- void drawBox(const VisuID& id, const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
- const Eigen::Vector3f& extents, const DrawColor& color = DEFAULTS.colorBox,
+ void drawBox(const VisuID& id,
+ const Eigen::Vector3f& position,
+ const Eigen::Quaternionf& orientation,
+ const Eigen::Vector3f& extents,
+ const DrawColor& color = DEFAULTS.colorBox,
bool ignoreLengthScale = false);
/// Draw a box.
- void drawBox(const VisuID& id, const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
+ void drawBox(const VisuID& id,
+ const Eigen::Matrix4f& pose,
+ const Eigen::Vector3f& extents,
const DrawColor& color = DEFAULTS.colorBox,
bool ignoreLengthScale = false);
@@ -353,7 +356,8 @@ namespace armarx
/// Draw a locally axis aligned bounding box, transformed by the given pose.
void drawBox(const VisuID& id,
- const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
+ const VirtualRobot::BoundingBox& boundingBox,
+ const Eigen::Matrix4f& pose,
const DrawColor& color = DEFAULTS.colorBox,
bool ignoreLengthScale = false);
@@ -364,39 +368,49 @@ namespace armarx
/// Draw box edges (as a line set).
void drawBoxEdges(const VisuID& id,
- const Eigen::Vector3f& position, const Eigen::Quaternionf& orientation,
+ const Eigen::Vector3f& position,
+ const Eigen::Quaternionf& orientation,
const Eigen::Vector3f& extents,
- float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ float width = DEFAULTS.boxEdgesWidth,
+ const DrawColor& color = DEFAULTS.boxEdgesColor,
bool ignoreLengthScale = false);
/// Draw box edges (as a line set).
void drawBoxEdges(const VisuID& id,
- const Eigen::Matrix4f& pose, const Eigen::Vector3f& extents,
- float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ const Eigen::Matrix4f& pose,
+ const Eigen::Vector3f& extents,
+ float width = DEFAULTS.boxEdgesWidth,
+ const DrawColor& color = DEFAULTS.boxEdgesColor,
bool ignoreLengthScale = false);
/// Draw edges of an axis aligned bounding box (as a line set).
void drawBoxEdges(const VisuID& id,
const VirtualRobot::BoundingBox& boundingBox,
- float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ float width = DEFAULTS.boxEdgesWidth,
+ const DrawColor& color = DEFAULTS.boxEdgesColor,
bool ignoreLengthScale = false);
/// Draw edges of an axis aligned bounding box (as a line set).
void drawBoxEdges(const VisuID& id,
const Eigen::Matrix32f& aabb,
- float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ float width = DEFAULTS.boxEdgesWidth,
+ const DrawColor& color = DEFAULTS.boxEdgesColor,
bool ignoreLengthScale = false);
/// Draw edges of a locally axis-aligned bounding box, transformed by the given pose (as a line set).
void drawBoxEdges(const VisuID& id,
- const VirtualRobot::BoundingBox& boundingBox, const Eigen::Matrix4f& pose,
- float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ const VirtualRobot::BoundingBox& boundingBox,
+ const Eigen::Matrix4f& pose,
+ float width = DEFAULTS.boxEdgesWidth,
+ const DrawColor& color = DEFAULTS.boxEdgesColor,
bool ignoreLengthScale = false);
/// Draw edges of a locally axis-aligned bounding box, transformed by the given pose (as a line set).
void drawBoxEdges(const VisuID& id,
- const Eigen::Matrix32f& aabb, const Eigen::Matrix4f& pose,
- float width = DEFAULTS.boxEdgesWidth, const DrawColor& color = DEFAULTS.boxEdgesColor,
+ const Eigen::Matrix32f& aabb,
+ const Eigen::Matrix4f& pose,
+ float width = DEFAULTS.boxEdgesWidth,
+ const DrawColor& color = DEFAULTS.boxEdgesColor,
bool ignoreLengthScale = false);
/// Remove box edges (as a line set).
@@ -407,113 +421,122 @@ namespace armarx
* @brief Draw a cylinder with center and direction.
* @param length The full length (not half-length).
*/
- void drawCylinder(
- const VisuID& id,
- const Eigen::Vector3f& center, const Eigen::Vector3f& direction, float length, float radius,
- const DrawColor& color = DEFAULTS.colorCylinder,
- bool ignoreLengthScale = false);
+ void drawCylinder(const VisuID& id,
+ const Eigen::Vector3f& center,
+ const Eigen::Vector3f& direction,
+ float length,
+ float radius,
+ const DrawColor& color = DEFAULTS.colorCylinder,
+ bool ignoreLengthScale = false);
/**
* @brief Draw a cylinder with center and orientation.
* An identity orientation represents a cylinder with an axis aligned to the Y-axis.
* @param length The full length (not half-length).
*/
- void drawCylinder(
- const VisuID& id,
- const Eigen::Vector3f& center, const Eigen::Quaternionf& orientation, float length, float radius,
- const DrawColor& color = DEFAULTS.colorCylinder,
- bool ignoreLengthScale = false);
+ void drawCylinder(const VisuID& id,
+ const Eigen::Vector3f& center,
+ const Eigen::Quaternionf& orientation,
+ float length,
+ float radius,
+ const DrawColor& color = DEFAULTS.colorCylinder,
+ bool ignoreLengthScale = false);
/// Draw a cylinder from start to end.
- void drawCylinderFromTo(
- const VisuID& id,
- const Eigen::Vector3f& from, const Eigen::Vector3f& to, float radius,
- const DrawColor& color = DEFAULTS.colorCylinder,
- bool ignoreLengthScale = false);
+ void drawCylinderFromTo(const VisuID& id,
+ const Eigen::Vector3f& from,
+ const Eigen::Vector3f& to,
+ float radius,
+ const DrawColor& color = DEFAULTS.colorCylinder,
+ bool ignoreLengthScale = false);
/// Remove a cylinder.
void removeCylinder(const VisuID& id);
/// Draw a sphere.
- void drawSphere(
- const VisuID& id,
- const Eigen::Vector3f& center, float radius,
- const DrawColor& color = DEFAULTS.colorSphere,
- bool ignoreLengthScale = false);
+ void drawSphere(const VisuID& id,
+ const Eigen::Vector3f& center,
+ float radius,
+ const DrawColor& color = DEFAULTS.colorSphere,
+ bool ignoreLengthScale = false);
/// Remove a sphere.
void removeSphere(const VisuID& id);
/// Draw an arrow with position (start) and direction.
- void drawArrow(
- const VisuID& id,
- const Eigen::Vector3f& position, const Eigen::Vector3f& direction, float length,
- float width, const DrawColor& color = DEFAULTS.colorArrow,
- bool ignoreLengthScale = false);
+ void drawArrow(const VisuID& id,
+ const Eigen::Vector3f& position,
+ const Eigen::Vector3f& direction,
+ float length,
+ float width,
+ const DrawColor& color = DEFAULTS.colorArrow,
+ bool ignoreLengthScale = false);
/// Draw an arrow with start and end.
- void drawArrowFromTo(
- const VisuID& id,
- const Eigen::Vector3f& from, const Eigen::Vector3f& to,
- float width, const DrawColor& color = DEFAULTS.colorArrow,
- bool ignoreLengthScale = false);
+ void drawArrowFromTo(const VisuID& id,
+ const Eigen::Vector3f& from,
+ const Eigen::Vector3f& to,
+ float width,
+ const DrawColor& color = DEFAULTS.colorArrow,
+ bool ignoreLengthScale = false);
/// Remove an arrow.
void removeArrow(const VisuID& id);
/// Draw a polygon.
- void drawPolygon(
- const VisuID& id,
- const std::vector<Eigen::Vector3f>& points,
- const DrawColor& colorFace,
- float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
- bool ignoreLengthScale = false);
+ void drawPolygon(const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ const DrawColor& colorFace,
+ float lineWidth = 0,
+ const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
+ bool ignoreLengthScale = false);
/// Remove a polygon.
void removePolygon(const VisuID& id);
/// Draw a line from start to end.
- void drawLine(
- const VisuID& id,
- const Eigen::Vector3f& from, const Eigen::Vector3f& to,
- float width, const DrawColor& color = DEFAULTS.colorLine,
- bool ignoreLengthScale = false);
+ void drawLine(const VisuID& id,
+ const Eigen::Vector3f& from,
+ const Eigen::Vector3f& to,
+ float width,
+ const DrawColor& color = DEFAULTS.colorLine,
+ bool ignoreLengthScale = false);
/// Remove a line.
void removeLine(const VisuID& id);
/// Draw a line set.
- void drawLineSet(
- const VisuID& id,
- const DebugDrawerLineSet& lineSet,
- bool ignoreLengthScale = false);
+ void drawLineSet(const VisuID& id,
+ const DebugDrawerLineSet& lineSet,
+ bool ignoreLengthScale = false);
/**
* @brief Draw a set of lines with constant color.
* @param points List of start and end points [ s1, e1, s2, e2, ..., sn, en ].
*/
- void drawLineSet(
- const VisuID& id,
- const std::vector<Eigen::Vector3f>& points,
- float width, const DrawColor& color = DEFAULTS.colorLine,
- bool ignoreLengthScale = false);
+ void drawLineSet(const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ float width,
+ const DrawColor& color = DEFAULTS.colorLine,
+ bool ignoreLengthScale = false);
/**
* @brief Draw a set of lines with constant color.
* @param points List of start and end points [ s1, e1, s2, e2, ..., sn, en ].
* @param colors List of line colors [ c1, c2, ..., cn ].
*/
- void drawLineSet(
- const VisuID& id,
- const std::vector<Eigen::Vector3f>& points,
- float width, const DrawColor& colorA, const DrawColor& colorB,
- const std::vector<float>& intensitiesB,
- bool ignoreLengthScale = false);
+ void drawLineSet(const VisuID& id,
+ const std::vector<Eigen::Vector3f>& points,
+ float width,
+ const DrawColor& colorA,
+ const DrawColor& colorB,
+ const std::vector<float>& intensitiesB,
+ bool ignoreLengthScale = false);
/// Remove a line set.
void removeLineSet(const VisuID& id);
@@ -522,17 +545,23 @@ namespace armarx
// POSE
/// Draw a pose (with the preset scale).
- void drawPose(const VisuID& id, const Eigen::Matrix4f& pose,
- bool ignoreLengthScale = false);
+ void
+ drawPose(const VisuID& id, const Eigen::Matrix4f& pose, bool ignoreLengthScale = false);
/// Draw a pose (with the preset scale).
- void drawPose(const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
+ void drawPose(const VisuID& id,
+ const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
bool ignoreLengthScale = false);
/// Draw a pose with the given scale.
- void drawPose(const VisuID& id, const Eigen::Matrix4f& pose, float scale,
+ void drawPose(const VisuID& id,
+ const Eigen::Matrix4f& pose,
+ float scale,
bool ignoreLengthScale = false);
/// Draw a pose with the given scale.
- void drawPose(const VisuID& id, const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
+ void drawPose(const VisuID& id,
+ const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
float scale,
bool ignoreLengthScale = false);
@@ -551,33 +580,29 @@ namespace armarx
* @param armarxProject The name of the ArmarX project keeping the robot model.
* @param drawStyle The draw style (full or collision model).
*/
- void drawRobot(
- const VisuID& id,
- const std::string& robotFile, const std::string& armarxProject,
- armarx::DrawStyle drawStyle = armarx::DrawStyle::FullModel);
+ void drawRobot(const VisuID& id,
+ const std::string& robotFile,
+ const std::string& armarxProject,
+ armarx::DrawStyle drawStyle = armarx::DrawStyle::FullModel);
/// Update / set the robot pose.
- void updateRobotPose(
- const VisuID& id,
- const Eigen::Matrix4f& pose,
- bool ignoreScale = false);
+ void
+ updateRobotPose(const VisuID& id, const Eigen::Matrix4f& pose, bool ignoreScale = false);
/// Update / set the robot pose.
- void updateRobotPose(
- const VisuID& id,
- const Eigen::Vector3f& pos, const Eigen::Quaternionf& ori,
- bool ignoreScale = false);
+ void updateRobotPose(const VisuID& id,
+ const Eigen::Vector3f& pos,
+ const Eigen::Quaternionf& ori,
+ bool ignoreScale = false);
/// Update / set the robot configuration (joint angles).
- void updateRobotConfig(
- const VisuID& id,
- const std::map<std::string, float>& config);
+ void updateRobotConfig(const VisuID& id, const std::map<std::string, float>& config);
/// Update / set the robot color.
void updateRobotColor(const VisuID& id, const DrawColor& color);
/// Update / set the color of a robot node.
- void updateRobotNodeColor(
- const VisuID& id, const std::string& nodeName, const DrawColor& color);
+ void
+ updateRobotNodeColor(const VisuID& id, const std::string& nodeName, const DrawColor& color);
/// Remove a robot visualization.
void removeRobot(const VisuID& id);
@@ -586,35 +611,35 @@ namespace armarx
// TRI MESH
/// Draw a TriMeshModel as DebugDrawerTriMesh.
- void drawTriMesh(
- const VisuID& id,
- const VirtualRobot::TriMeshModel& triMesh,
- const DrawColor& color = {.5, .5, .5, 1},
- bool ignoreLengthScale = false);
+ void drawTriMesh(const VisuID& id,
+ const VirtualRobot::TriMeshModel& triMesh,
+ const DrawColor& color = {.5, .5, .5, 1},
+ bool ignoreLengthScale = false);
/// Draw a TriMeshModel as individual polygons.
- void drawTriMeshAsPolygons(
- const VisuID& id,
- const VirtualRobot::TriMeshModel& trimesh,
- const DrawColor& colorFace = DEFAULTS.colorPolygonFace,
- float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
- bool ignoreLengthScale = false);
+ void drawTriMeshAsPolygons(const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh,
+ const DrawColor& colorFace = DEFAULTS.colorPolygonFace,
+ float lineWidth = 0,
+ const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
+ bool ignoreLengthScale = false);
/// Draw a TriMeshModel as individual polygons, transformed by the given pose.
- void drawTriMeshAsPolygons(
- const VisuID& id,
- const VirtualRobot::TriMeshModel& trimesh, const Eigen::Matrix4f& pose,
- const DrawColor& colorFace = DEFAULTS.colorPolygonFace,
- float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
- bool ignoreLengthScale = false);
+ void drawTriMeshAsPolygons(const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh,
+ const Eigen::Matrix4f& pose,
+ const DrawColor& colorFace = DEFAULTS.colorPolygonFace,
+ float lineWidth = 0,
+ const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
+ bool ignoreLengthScale = false);
/// Draw a TriMeshModel as individual polygons with individual face colors.
- void drawTriMeshAsPolygons(
- const VisuID& id,
- const VirtualRobot::TriMeshModel& trimesh,
- const std::vector<DrawColor>& faceColorsInner,
- float lineWidth = 0, const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
- bool ignoreLengthScale = false);
+ void drawTriMeshAsPolygons(const VisuID& id,
+ const VirtualRobot::TriMeshModel& trimesh,
+ const std::vector<DrawColor>& faceColorsInner,
+ float lineWidth = 0,
+ const DrawColor& colorEdge = DEFAULTS.colorPolygonEdge,
+ bool ignoreLengthScale = false);
// POINT CLOUD
@@ -628,12 +653,11 @@ namespace armarx
* `pointCloud` must be iterable and its elements must provide members `x, y, z`.
*/
template <class PointCloudT>
- void drawPointCloud(
- const VisuID& id,
- const PointCloudT& pointCloud,
- const DrawColor& color = DEFAULTS.colorPointCloud,
- float pointSize = DEFAULTS.pointCloudPointSize,
- bool ignoreLengthScale = false);
+ void drawPointCloud(const VisuID& id,
+ const PointCloudT& pointCloud,
+ const DrawColor& color = DEFAULTS.colorPointCloud,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
/**
* @brief Draw a colored point cloud with RGBA information.
@@ -642,11 +666,10 @@ namespace armarx
* members `x, y, z, r, g, b, a`.
*/
template <class PointCloudT>
- void drawPointCloudRGBA(
- const VisuID& id,
- const PointCloudT& pointCloud,
- float pointSize = DEFAULTS.pointCloudPointSize,
- bool ignoreLengthScale = false);
+ void drawPointCloudRGBA(const VisuID& id,
+ const PointCloudT& pointCloud,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
/**
* @brief Draw a colored point cloud with colors according to labels.
@@ -655,11 +678,10 @@ namespace armarx
* members `x, y, z, label`.
*/
template <class PointCloudT>
- void drawPointCloudLabeled(
- const VisuID& id,
- const PointCloudT& pointCloud,
- float pointSize = DEFAULTS.pointCloudPointSize,
- bool ignoreLengthScale = false);
+ void drawPointCloudLabeled(const VisuID& id,
+ const PointCloudT& pointCloud,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
/**
* @brief Draw a colored point cloud with custom colors.
@@ -671,12 +693,11 @@ namespace armarx
* color as `armarx::DrawColor`.
*/
template <class PointCloudT, class ColorFuncT>
- void drawPointCloud(
- const VisuID& id,
- const PointCloudT& pointCloud,
- const ColorFuncT& colorFunc,
- float pointSize = DEFAULTS.pointCloudPointSize,
- bool ignoreLengthScale = false);
+ void drawPointCloud(const VisuID& id,
+ const PointCloudT& pointCloud,
+ const ColorFuncT& colorFunc,
+ float pointSize = DEFAULTS.pointCloudPointSize,
+ bool ignoreLengthScale = false);
// Debug Drawer Point Cloud Types
@@ -703,12 +724,12 @@ namespace armarx
* @param up the up direction (normal of the floor plane)
* @param size the quad's edge length
*/
- void drawFloor(
- const VisuID& id = { "floor" },
- const Eigen::Vector3f& at = Eigen::Vector3f::Zero(),
- const Eigen::Vector3f& up = Eigen::Vector3f::UnitZ(),
- float size = 5, const DrawColor& color = DEFAULTS.colorFloor,
- bool ignoreLengthScale = false);
+ void drawFloor(const VisuID& id = {"floor"},
+ const Eigen::Vector3f& at = Eigen::Vector3f::Zero(),
+ const Eigen::Vector3f& up = Eigen::Vector3f::UnitZ(),
+ float size = 5,
+ const DrawColor& color = DEFAULTS.colorFloor,
+ bool ignoreLengthScale = false);
// OPERATORS
@@ -718,16 +739,15 @@ namespace armarx
operator bool() const;
/// Conversion operator to DebugDrawerInterfacePrx.
- operator DebugDrawerInterfacePrx& ();
- operator const DebugDrawerInterfacePrx& () const;
+ operator DebugDrawerInterfacePrx&();
+ operator const DebugDrawerInterfacePrx&() const;
/// Pointer member access operator to access the raw debug drawer proxy.
DebugDrawerInterfacePrx& operator->();
const DebugDrawerInterfacePrx& operator->() const;
- public: // STATIC
-
+ public: // STATIC
/**
* @brief Convert a RGB color to HSV.
* @param rgb RGB color as [r, g, b] with r, g, b in [0, 1].
@@ -755,7 +775,8 @@ namespace armarx
* @param byteToFloat If true, scale from range [0, 255] to [0, 1]
*/
template <class ColorT>
- static DrawColor toDrawColor(const ColorT& color, float alpha = 1, bool byteToFloat = false);
+ static DrawColor
+ toDrawColor(const ColorT& color, float alpha = 1, bool byteToFloat = false);
/// Get a color from the Glasbey look-up-table.
@@ -765,7 +786,6 @@ namespace armarx
private:
-
/// Get the layer to draw on. (passedLayer if not empty, _layer otherwise).
const std::string& layer(const std::string& passedLayer) const;
@@ -794,7 +814,6 @@ namespace armarx
private:
-
/// The name of the debug drawer topic.
static const std::string TOPIC_NAME;
/// The default layer ("debug").
@@ -817,13 +836,12 @@ namespace armarx
float _poseScale = 1;
/// The duration for shortSleep().
- std::chrono::milliseconds _shortSleepDuration { 100 };
-
+ std::chrono::milliseconds _shortSleepDuration{100};
};
-
template <typename DurationT>
- void DebugDrawerTopic::sleepFor(const DurationT& duration)
+ void
+ DebugDrawerTopic::sleepFor(const DurationT& duration)
{
if (enabled())
{
@@ -832,69 +850,69 @@ namespace armarx
}
template <typename DurationT>
- void DebugDrawerTopic::setShortSleepDuration(const DurationT& duration)
+ void
+ DebugDrawerTopic::setShortSleepDuration(const DurationT& duration)
{
this->_shortSleepDuration = std::chrono::duration_cast<std::chrono::milliseconds>(duration);
}
template <class ColorT>
- DrawColor DebugDrawerTopic::toDrawColor(const ColorT& color, float alpha, bool byteToFloat)
+ DrawColor
+ DebugDrawerTopic::toDrawColor(const ColorT& color, float alpha, bool byteToFloat)
{
const float scale = byteToFloat ? (1 / 255.f) : 1;
- return { color.r * scale, color.g * scale, color.b * scale, alpha };
+ return {color.r * scale, color.g * scale, color.b * scale, alpha};
}
-
template <class PointCloudT>
- void DebugDrawerTopic::drawPointCloud(
- const VisuID& id,
- const PointCloudT& pointCloud,
- const DrawColor& color,
- float pointSize,
- bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawPointCloud(const VisuID& id,
+ const PointCloudT& pointCloud,
+ const DrawColor& color,
+ float pointSize,
+ bool ignoreLengthScale)
{
- drawPointCloud(id, pointCloud, [&color](const auto&)
- {
- return color;
- },
- pointSize, ignoreLengthScale);
+ drawPointCloud(
+ id, pointCloud, [&color](const auto&) { return color; }, pointSize, ignoreLengthScale);
}
- template<class PointCloudT>
- void DebugDrawerTopic::drawPointCloudRGBA(
- const VisuID& id,
- const PointCloudT& pointCloud,
- float pointSize,
- bool ignoreLengthScale)
+ template <class PointCloudT>
+ void
+ DebugDrawerTopic::drawPointCloudRGBA(const VisuID& id,
+ const PointCloudT& pointCloud,
+ float pointSize,
+ bool ignoreLengthScale)
{
- drawPointCloud(id, pointCloud, [](const auto & p)
- {
- return toDrawColor(p, p.a);
- },
- pointSize, ignoreLengthScale);
+ drawPointCloud(
+ id,
+ pointCloud,
+ [](const auto& p) { return toDrawColor(p, p.a); },
+ pointSize,
+ ignoreLengthScale);
}
template <class PointCloudT>
- void DebugDrawerTopic::drawPointCloudLabeled(
- const VisuID& id,
- const PointCloudT& pointCloud,
- float pointSize,
- bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawPointCloudLabeled(const VisuID& id,
+ const PointCloudT& pointCloud,
+ float pointSize,
+ bool ignoreLengthScale)
{
- drawPointCloud(id, pointCloud, [](const auto & p)
- {
- return getGlasbeyLUTColor(p.label);
- },
- pointSize, ignoreLengthScale);
+ drawPointCloud(
+ id,
+ pointCloud,
+ [](const auto& p) { return getGlasbeyLUTColor(p.label); },
+ pointSize,
+ ignoreLengthScale);
}
template <class PointCloudT, class ColorFuncT>
- void DebugDrawerTopic::drawPointCloud(
- const VisuID& id,
- const PointCloudT& pointCloud,
- const ColorFuncT& colorFn,
- float pointSize,
- bool ignoreLengthScale)
+ void
+ DebugDrawerTopic::drawPointCloud(const VisuID& id,
+ const PointCloudT& pointCloud,
+ const ColorFuncT& colorFn,
+ float pointSize,
+ bool ignoreLengthScale)
{
if (!enabled())
{
@@ -910,29 +928,28 @@ namespace armarx
for (const auto& p : pointCloud)
{
- dd.points.push_back(DebugDrawerColoredPointCloudElement
- {
- lf * p.x, lf * p.y, lf * p.z, colorFn(p)
- });
+ dd.points.push_back(
+ DebugDrawerColoredPointCloudElement{lf * p.x, lf * p.y, lf * p.z, colorFn(p)});
}
drawPointCloud(id, dd);
}
-
template <class ScaledT>
- ScaledT DebugDrawerTopic::scaledT(float scale, const Eigen::Vector3f& vector)
+ ScaledT
+ DebugDrawerTopic::scaledT(float scale, const Eigen::Vector3f& vector)
{
auto scaled = vector * scale;
- return { scaled.x(), scaled.y(), scaled.z() };
+ return {scaled.x(), scaled.y(), scaled.z()};
}
template <class XYZT>
- void DebugDrawerTopic::scaleXYZ(float scale, XYZT& xyz)
+ void
+ DebugDrawerTopic::scaleXYZ(float scale, XYZT& xyz)
{
xyz.x *= scale;
xyz.y *= scale;
xyz.z *= scale;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp
index 6043474b069fb0940118ea771b30943dedc3b6ed..85650a0fc015be557978e95890f67864bdbc6320 100644
--- a/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp
+++ b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.cpp
@@ -2,45 +2,49 @@
#include <SimoxUtility/color/GlasbeyLUT.h>
-
-armarx::DrawColor armarx::toDrawColor(Eigen::Vector4f c)
+armarx::DrawColor
+armarx::toDrawColor(Eigen::Vector4f c)
{
- return { c(0), c(1), c(2), c(3) };
+ return {c(0), c(1), c(2), c(3)};
}
-armarx::DrawColor armarx::toDrawColor(simox::Color c)
+armarx::DrawColor
+armarx::toDrawColor(simox::Color c)
{
return toDrawColor(c.to_vector4f());
}
-armarx::DrawColor24Bit armarx::toDrawColor24Bit(simox::Color c)
+armarx::DrawColor24Bit
+armarx::toDrawColor24Bit(simox::Color c)
{
- return { c.r, c.g, c.b };
+ return {c.r, c.g, c.b};
}
-
namespace armarx
{
- DrawColor GlasbeyLUT::at(std::size_t id, float alpha)
+ DrawColor
+ GlasbeyLUT::at(std::size_t id, float alpha)
{
return toDrawColor(simox::color::GlasbeyLUT::atf(id, alpha));
}
- DrawColor24Bit GlasbeyLUT::atByte(std::size_t id)
+ DrawColor24Bit
+ GlasbeyLUT::atByte(std::size_t id)
{
return toDrawColor24Bit(simox::color::GlasbeyLUT::at(id));
}
-
- std::size_t GlasbeyLUT::size()
+ std::size_t
+ GlasbeyLUT::size()
{
return simox::color::GlasbeyLUT::size();
}
- const std::vector<unsigned char>& GlasbeyLUT::data()
+ const std::vector<unsigned char>&
+ GlasbeyLUT::data()
{
return simox::color::GlasbeyLUT::data();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h
index bca0feb86864c09d40188a80189634db9a8ad7a9..e517c6fd137fbd5b392e4acb773b97793cb1033b 100644
--- a/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h
+++ b/source/RobotAPI/libraries/core/visualization/GlasbeyLUT.h
@@ -4,10 +4,9 @@
#include <type_traits>
#include <vector>
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-
#include <SimoxUtility/color/Color.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
namespace armarx
{
@@ -20,7 +19,6 @@ namespace armarx
DrawColor toDrawColor(simox::Color c);
DrawColor24Bit toDrawColor24Bit(simox::Color c);
-
/**
* "Color lookup table consisting of 256 colors structured in a maximally
* discontinuous manner. Generated using the method of Glasbey et al.
@@ -32,7 +30,6 @@ namespace armarx
class GlasbeyLUT
{
public:
-
static DrawColor at(std::size_t id, float alpha = 1.f);
static DrawColor24Bit atByte(std::size_t id);
@@ -42,37 +39,39 @@ namespace armarx
* If `id` is negative, its absolute value is used.
*/
template <typename UIntT, std::enable_if_t<std::is_unsigned<UIntT>::value, int> = 0>
- static DrawColor at(UIntT id, float alpha = 1.f)
+ static DrawColor
+ at(UIntT id, float alpha = 1.f)
{
return at(static_cast<std::size_t>(id), alpha);
}
// If `id` is negative, its absolute value is used.
template <typename IntT, std::enable_if_t<std::is_signed<IntT>::value, int> = 0>
- static DrawColor at(IntT id, float alpha = 1.f)
+ static DrawColor
+ at(IntT id, float alpha = 1.f)
{
return at(static_cast<std::size_t>(id >= 0 ? id : std::abs(id)), alpha);
}
-
/**
* @brief Get a color from the lookup table with given ID (with integer values).
* The ID is automaticall wrapped if greater than `size()`.
* If `id` is negative, its absolute value is used.
*/
template <typename UIntT, std::enable_if_t<std::is_unsigned<UIntT>::value, int> = 0>
- static DrawColor24Bit atByte(UIntT id)
+ static DrawColor24Bit
+ atByte(UIntT id)
{
return atByte(static_cast<std::size_t>(id));
}
template <typename IntT, std::enable_if_t<std::is_signed<IntT>::value, int> = 0>
- static DrawColor24Bit atByte(IntT id)
+ static DrawColor24Bit
+ atByte(IntT id)
{
return atByte(static_cast<std::size_t>(id >= 0 ? id : std::abs(id)));
}
-
/// Get the number of colors in the lookup table.;
static std::size_t size();
@@ -81,10 +80,8 @@ namespace armarx
private:
-
/// Private constructor.
GlasbeyLUT() = default;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
index 833e6a399e5e3ca64ad8e7441eb6aa78f5c0a99e..7c9b0f10f0090b6fee2a0d4acbd07a555b7c6ff7 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.cpp
@@ -22,53 +22,62 @@
*/
#include "CompositeDiffIK.h"
-#include <VirtualRobot/Robot.h>
-#include <ArmarXCore/core/exceptions/Exception.h>
-#include <VirtualRobot/math/Helpers.h>
-#include <VirtualRobot/RobotNodeSet.h>
+#include <cfloat>
+
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
+#include <VirtualRobot/math/Helpers.h>
-#include <cfloat>
+#include <ArmarXCore/core/exceptions/Exception.h>
using namespace armarx;
-CompositeDiffIK::CompositeDiffIK(const VirtualRobot::RobotNodeSetPtr& rns)
- : rns(rns)
+CompositeDiffIK::CompositeDiffIK(const VirtualRobot::RobotNodeSetPtr& rns) : rns(rns)
{
- ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
+ ik.reset(new VirtualRobot::DifferentialIK(
+ rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
}
-void CompositeDiffIK::addTarget(const TargetPtr& target)
+void
+CompositeDiffIK::addTarget(const TargetPtr& target)
{
targets.emplace_back(target);
}
-CompositeDiffIK::TargetPtr CompositeDiffIK::addTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode)
+CompositeDiffIK::TargetPtr
+CompositeDiffIK::addTarget(const VirtualRobot::RobotNodePtr& tcp,
+ const Eigen::Matrix4f& target,
+ VirtualRobot::IKSolver::CartesianSelection mode)
{
TargetPtr t = std::make_shared<Target>(rns, tcp, target, mode);
addTarget(t);
return t;
}
-void CompositeDiffIK::addNullspaceGradient(const CompositeDiffIK::NullspaceGradientPtr& gradient)
+void
+CompositeDiffIK::addNullspaceGradient(const CompositeDiffIK::NullspaceGradientPtr& gradient)
{
nullspaceGradients.emplace_back(gradient);
}
-CompositeDiffIK::NullspaceTargetPtr CompositeDiffIK::addNullspacePositionTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Vector3f& target)
+CompositeDiffIK::NullspaceTargetPtr
+CompositeDiffIK::addNullspacePositionTarget(const VirtualRobot::RobotNodePtr& tcp,
+ const Eigen::Vector3f& target)
{
- CompositeDiffIK::NullspaceTargetPtr nst(new CompositeDiffIK::NullspaceTarget(rns, tcp,
- math::Helpers::CreatePose(target, Eigen::Matrix3f::Identity()),
- VirtualRobot::IKSolver::CartesianSelection::Position));
+ CompositeDiffIK::NullspaceTargetPtr nst(new CompositeDiffIK::NullspaceTarget(
+ rns,
+ tcp,
+ math::Helpers::CreatePose(target, Eigen::Matrix3f::Identity()),
+ VirtualRobot::IKSolver::CartesianSelection::Position));
addNullspaceGradient(nst);
return nst;
}
-
-CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
+CompositeDiffIK::Result
+CompositeDiffIK::solve(Parameters params)
{
//ARMARX_IMPORTANT << "###";
//ARMARX_IMPORTANT << VAROUT(rns->getJointValuesEigen().transpose());
@@ -101,7 +110,9 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
s.jointRegularization = Eigen::VectorXf::Zero(s.cols);
for (size_t i = 0; i < rns->getSize(); i++)
{
- s.jointRegularization(i) = rns->getNode(i)->isTranslationalJoint() ? params.jointRegularizationTranslation : params.jointRegularizationRotation;
+ s.jointRegularization(i) = rns->getNode(i)->isTranslationalJoint()
+ ? params.jointRegularizationTranslation
+ : params.jointRegularizationRotation;
}
s.cartesianRegularization = Eigen::VectorXf::Zero(s.rows);
@@ -112,7 +123,8 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
{
int h = CartesianSelectionToSize(target->mode);
target->jacobi = Eigen::MatrixXf::Zero(h, s.cols);
- s.cartesianRegularization.block(row, 0, h, 1) = tmpVC.calculateRegularization(target->mode);
+ s.cartesianRegularization.block(row, 0, h, 1) =
+ tmpVC.calculateRegularization(target->mode);
row += h;
}
}
@@ -129,7 +141,9 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
bool allReached = true;
for (const TargetPtr& target : targets)
{
- allReached = allReached && target->pCtrl.reached(target->target, target->mode, target->maxPosError, target->maxOriError);
+ allReached = allReached &&
+ target->pCtrl.reached(
+ target->target, target->mode, target->maxPosError, target->maxOriError);
}
//ARMARX_IMPORTANT << "END";
@@ -151,22 +165,26 @@ CompositeDiffIK::Result CompositeDiffIK::solve(Parameters params)
}
else
{
- result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(), rn->getJointLimitHi() - rn->getJointValue());
- result.minimumJointLimitMargin = std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
+ result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(),
+ rn->getJointLimitHi() - rn->getJointValue());
+ result.minimumJointLimitMargin =
+ std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
}
}
return result;
}
-Eigen::MatrixXf CompositeDiffIK::CalculateNullspaceSVD(const Eigen::Matrix4f& jacobi)
+Eigen::MatrixXf
+CompositeDiffIK::CalculateNullspaceSVD(const Eigen::Matrix4f& jacobi)
{
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
Eigen::MatrixXf nullspaceLU = lu_decomp.kernel();
return nullspaceLU;
}
-Eigen::MatrixXf CompositeDiffIK::CalculateNullspaceLU(const Eigen::Matrix4f& jacobi)
+Eigen::MatrixXf
+CompositeDiffIK::CalculateNullspaceLU(const Eigen::Matrix4f& jacobi)
{
// cols >= rows
int rows = jacobi.rows();
@@ -183,7 +201,8 @@ Eigen::MatrixXf CompositeDiffIK::CalculateNullspaceLU(const Eigen::Matrix4f& jac
return nullspaceSVD;
}
-void CompositeDiffIK::step(CompositeDiffIK::Parameters& params, SolveState& s, int stepNr)
+void
+CompositeDiffIK::step(CompositeDiffIK::Parameters& params, SolveState& s, int stepNr)
{
//ARMARX_IMPORTANT << VAROUT(i);
s.jacobi = Eigen::MatrixXf::Zero(s.rows, s.cols);
@@ -220,7 +239,8 @@ void CompositeDiffIK::step(CompositeDiffIK::Parameters& params, SolveState& s, i
for (int row = 0; row < s.rows; row++)
{
- s.jacobi.block(row, 0, 1, s.cols) = s.jacobi.block(row, 0, 1, s.cols).cwiseProduct(s.jointRegularization.transpose());
+ s.jacobi.block(row, 0, 1, s.cols) =
+ s.jacobi.block(row, 0, 1, s.cols).cwiseProduct(s.jointRegularization.transpose());
}
if (stepNr == 0)
{
@@ -244,7 +264,6 @@ void CompositeDiffIK::step(CompositeDiffIK::Parameters& params, SolveState& s, i
//LimitInfNormTo(nullspaceVel, params.maxJointAngleStep);
-
Eigen::JacobiSVD<Eigen::MatrixXf> svd(s.jacobi, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXf V = svd.matrixV();
Eigen::MatrixXf nullspaceSVD = V.block(0, s.rows, s.cols, s.cols - s.rows);
@@ -258,7 +277,8 @@ void CompositeDiffIK::step(CompositeDiffIK::Parameters& params, SolveState& s, i
// Prevent division by zero
if (squaredNorm > 1.0e-32f)
{
- nsv += s.nullspace.col(i) * s.nullspace.col(i).dot(nullspaceVel) / s.nullspace.col(i).squaredNorm();
+ nsv += s.nullspace.col(i) * s.nullspace.col(i).dot(nullspaceVel) /
+ s.nullspace.col(i).squaredNorm();
}
}
@@ -285,7 +305,8 @@ void CompositeDiffIK::step(CompositeDiffIK::Parameters& params, SolveState& s, i
s.jointValues = newJointValues;
}
-int CompositeDiffIK::CartesianSelectionToSize(VirtualRobot::IKSolver::CartesianSelection mode)
+int
+CompositeDiffIK::CartesianSelectionToSize(VirtualRobot::IKSolver::CartesianSelection mode)
{
switch (mode)
{
@@ -300,25 +321,31 @@ int CompositeDiffIK::CartesianSelectionToSize(VirtualRobot::IKSolver::CartesianS
}
}
-
-CompositeDiffIK::Target::Target(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode)
- : tcp(tcp), target(target), mode(mode), pCtrl(tcp)
+CompositeDiffIK::Target::Target(const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& tcp,
+ const Eigen::Matrix4f& target,
+ VirtualRobot::IKSolver::CartesianSelection mode) :
+ tcp(tcp), target(target), mode(mode), pCtrl(tcp)
{
jacobi = Eigen::MatrixXf::Zero(0, 0);
}
-CompositeDiffIK::NullspaceTarget::NullspaceTarget(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode)
- : tcp(tcp), target(target), mode(mode), pCtrl(tcp), vCtrl(rns, tcp)
+CompositeDiffIK::NullspaceTarget::NullspaceTarget(const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& tcp,
+ const Eigen::Matrix4f& target,
+ VirtualRobot::IKSolver::CartesianSelection mode) :
+ tcp(tcp), target(target), mode(mode), pCtrl(tcp), vCtrl(rns, tcp)
{
-
}
-void CompositeDiffIK::NullspaceTarget::init(Parameters&)
+void
+CompositeDiffIK::NullspaceTarget::init(Parameters&)
{
ikSteps.clear();
}
-Eigen::VectorXf CompositeDiffIK::NullspaceTarget::getGradient(Parameters& params)
+Eigen::VectorXf
+CompositeDiffIK::NullspaceTarget::getGradient(Parameters& params)
{
Eigen::VectorXf cv = pCtrl.calculate(target, mode);
Eigen::VectorXf jv = vCtrl.calculate(cv, mode);
@@ -335,7 +362,8 @@ Eigen::VectorXf CompositeDiffIK::NullspaceTarget::getGradient(Parameters& params
return jv;
}
-Eigen::VectorXf CompositeDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
+Eigen::VectorXf
+CompositeDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
{
float infNorm = vec.lpNorm<Eigen::Infinity>();
if (infNorm > maxValue)
@@ -345,25 +373,29 @@ Eigen::VectorXf CompositeDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxVa
return vec;
}
-CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(const VirtualRobot::RobotNodeSetPtr& rns)
- : rns(rns), target(rns->getJointValuesEigen()), weight(Eigen::VectorXf::Zero(rns->getSize()))
+CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(
+ const VirtualRobot::RobotNodeSetPtr& rns) :
+ rns(rns), target(rns->getJointValuesEigen()), weight(Eigen::VectorXf::Zero(rns->getSize()))
{
-
}
-CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& target, const Eigen::VectorXf& weight)
- : rns(rns), target(target), weight(weight)
+CompositeDiffIK::NullspaceJointTarget::NullspaceJointTarget(
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::VectorXf& target,
+ const Eigen::VectorXf& weight) :
+ rns(rns), target(target), weight(weight)
{
-
}
-void CompositeDiffIK::NullspaceJointTarget::set(int index, float target, float weight)
+void
+CompositeDiffIK::NullspaceJointTarget::set(int index, float target, float weight)
{
this->target(index) = target;
this->weight(index) = weight;
}
-void CompositeDiffIK::NullspaceJointTarget::set(const std::string& jointName, float target, float weight)
+void
+CompositeDiffIK::NullspaceJointTarget::set(const std::string& jointName, float target, float weight)
{
int index = rns->getRobotNodeIndex(jointName);
if (index < 0)
@@ -373,7 +405,10 @@ void CompositeDiffIK::NullspaceJointTarget::set(const std::string& jointName, fl
set(index, target, weight);
}
-void CompositeDiffIK::NullspaceJointTarget::set(const VirtualRobot::RobotNodePtr& rn, float target, float weight)
+void
+CompositeDiffIK::NullspaceJointTarget::set(const VirtualRobot::RobotNodePtr& rn,
+ float target,
+ float weight)
{
int index = rns->getRobotNodeIndex(rn);
if (index < 0)
@@ -383,34 +418,38 @@ void CompositeDiffIK::NullspaceJointTarget::set(const VirtualRobot::RobotNodePtr
set(index, target, weight);
}
-void CompositeDiffIK::NullspaceJointTarget::init(CompositeDiffIK::Parameters&)
+void
+CompositeDiffIK::NullspaceJointTarget::init(CompositeDiffIK::Parameters&)
{
-
}
-Eigen::VectorXf CompositeDiffIK::NullspaceJointTarget::getGradient(Parameters& params)
+Eigen::VectorXf
+CompositeDiffIK::NullspaceJointTarget::getGradient(Parameters& params)
{
return (target - rns->getJointValuesEigen()).cwiseProduct(weight);
}
-CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns)
- : rns(rns), weight(Eigen::VectorXf::Constant(rns->getSize(), 1))
+CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(
+ const VirtualRobot::RobotNodeSetPtr& rns) :
+ rns(rns), weight(Eigen::VectorXf::Constant(rns->getSize(), 1))
{
-
}
-CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& weight)
- : rns(rns), weight(weight)
+CompositeDiffIK::NullspaceJointLimitAvoidance::NullspaceJointLimitAvoidance(
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::VectorXf& weight) :
+ rns(rns), weight(weight)
{
-
}
-void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(int index, float weight)
+void
+CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(int index, float weight)
{
this->weight(index) = weight;
}
-void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const std::string& jointName, float weight)
+void
+CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const std::string& jointName, float weight)
{
int index = rns->getRobotNodeIndex(jointName);
if (index < 0)
@@ -420,7 +459,9 @@ void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const std::string&
setWeight(index, weight);
}
-void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const VirtualRobot::RobotNodePtr& rn, float weight)
+void
+CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const VirtualRobot::RobotNodePtr& rn,
+ float weight)
{
int index = rns->getRobotNodeIndex(rn);
if (index < 0)
@@ -430,7 +471,9 @@ void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeight(const VirtualRobot
setWeight(index, weight);
}
-void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeights(const VirtualRobot::RobotNodeSetPtr& rns, float weight)
+void
+CompositeDiffIK::NullspaceJointLimitAvoidance::setWeights(const VirtualRobot::RobotNodeSetPtr& rns,
+ float weight)
{
for (const VirtualRobot::RobotNodePtr& rn : rns->getAllRobotNodes())
{
@@ -438,12 +481,13 @@ void CompositeDiffIK::NullspaceJointLimitAvoidance::setWeights(const VirtualRobo
}
}
-void CompositeDiffIK::NullspaceJointLimitAvoidance::init(CompositeDiffIK::Parameters&)
+void
+CompositeDiffIK::NullspaceJointLimitAvoidance::init(CompositeDiffIK::Parameters&)
{
-
}
-Eigen::VectorXf CompositeDiffIK::NullspaceJointLimitAvoidance::getGradient(Parameters& params)
+Eigen::VectorXf
+CompositeDiffIK::NullspaceJointLimitAvoidance::getGradient(Parameters& params)
{
Eigen::VectorXf r(rns->getSize());
for (size_t i = 0; i < rns->getSize(); i++)
@@ -455,7 +499,8 @@ Eigen::VectorXf CompositeDiffIK::NullspaceJointLimitAvoidance::getGradient(Param
}
else
{
- float f = math::Helpers::ILerp(rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
+ float f = math::Helpers::ILerp(
+ rn->getJointLimitLo(), rn->getJointLimitHi(), rn->getJointValue());
r(i) = cos(f * M_PI);
//r(i) = math::MathUtils::Lerp(1.f, -1.f, f);
}
diff --git a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
index 2963293f63fa644a2769d57d6b3262b77320dc99..b5e2f1691a2cdf461fd099009db725edb5b9e337 100644
--- a/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/CompositeDiffIK.h
@@ -23,27 +23,29 @@
#pragma once
-#include <SimoxUtility/math/distance/delta_angle.h>
+#include <memory>
+#include <SimoxUtility/math/distance/delta_angle.h>
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <memory>
-
-
namespace armarx
{
typedef std::shared_ptr<class CompositeDiffIK> CompositeDiffIKPtr;
+
class CompositeDiffIK
{
public:
struct Parameters
{
- Parameters() {}
+ Parameters()
+ {
+ }
+
// IK params
size_t steps = 40;
@@ -73,10 +75,14 @@ namespace armarx
Eigen::VectorXf cartesianVel;
Eigen::VectorXf jointVel;
};
+
class NullspaceTarget : public NullspaceGradient
{
public:
- NullspaceTarget(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode);
+ NullspaceTarget(const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& tcp,
+ const Eigen::Matrix4f& target,
+ VirtualRobot::IKSolver::CartesianSelection mode);
VirtualRobot::RobotNodePtr tcp;
Eigen::Matrix4f target;
@@ -95,7 +101,9 @@ namespace armarx
{
public:
NullspaceJointTarget(const VirtualRobot::RobotNodeSetPtr& rns);
- NullspaceJointTarget(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& target, const Eigen::VectorXf& weight);
+ NullspaceJointTarget(const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::VectorXf& target,
+ const Eigen::VectorXf& weight);
void set(int index, float target, float weight);
void set(const std::string& jointName, float target, float weight);
void set(const VirtualRobot::RobotNodePtr& rn, float target, float weight);
@@ -113,7 +121,8 @@ namespace armarx
{
public:
NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns);
- NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& weight);
+ NullspaceJointLimitAvoidance(const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::VectorXf& weight);
void setWeight(int index, float weight);
void setWeight(const std::string& jointName, float weight);
void setWeight(const VirtualRobot::RobotNodePtr& rn, float weight);
@@ -139,7 +148,10 @@ namespace armarx
class Target
{
public:
- Target(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode);
+ Target(const VirtualRobot::RobotNodeSetPtr& rns,
+ const VirtualRobot::RobotNodePtr& tcp,
+ const Eigen::Matrix4f& target,
+ VirtualRobot::IKSolver::CartesianSelection mode);
VirtualRobot::RobotNodePtr tcp;
Eigen::Matrix4f target;
VirtualRobot::IKSolver::CartesianSelection mode;
@@ -154,10 +166,8 @@ namespace armarx
Eigen::Vector3f getPosError() const;
auto getOriError() const;
};
- typedef std::shared_ptr<Target> TargetPtr;
-
-
+ typedef std::shared_ptr<Target> TargetPtr;
struct Result
{
@@ -184,7 +194,6 @@ namespace armarx
Eigen::MatrixXf jacobi;
Eigen::MatrixXf invJac;
Eigen::MatrixXf nullspace;
-
};
static Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue);
@@ -192,10 +201,13 @@ namespace armarx
CompositeDiffIK(const VirtualRobot::RobotNodeSetPtr& rns);
void addTarget(const TargetPtr& target);
- TargetPtr addTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Matrix4f& target, VirtualRobot::IKSolver::CartesianSelection mode);
+ TargetPtr addTarget(const VirtualRobot::RobotNodePtr& tcp,
+ const Eigen::Matrix4f& target,
+ VirtualRobot::IKSolver::CartesianSelection mode);
void addNullspaceGradient(const NullspaceGradientPtr& gradient);
- NullspaceTargetPtr addNullspacePositionTarget(const VirtualRobot::RobotNodePtr& tcp, const Eigen::Vector3f& target);
+ NullspaceTargetPtr addNullspacePositionTarget(const VirtualRobot::RobotNodePtr& tcp,
+ const Eigen::Vector3f& target);
Result solve(Parameters params);
static Eigen::MatrixXf CalculateNullspaceSVD(const Eigen::Matrix4f& jacobi);
@@ -205,12 +217,10 @@ namespace armarx
int CartesianSelectionToSize(VirtualRobot::IKSolver::CartesianSelection mode);
-
private:
VirtualRobot::RobotNodeSetPtr rns;
std::vector<TargetPtr> targets;
std::vector<NullspaceGradientPtr> nullspaceGradients;
VirtualRobot::DifferentialIKPtr ik;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/diffik/DiffIKProvider.h b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
index b65a63b9a8c2e80cc4312afbe859c6d64e8f5f73..6dec1f75ce7e83cc5f6afdb160b5c238a8cdb34d 100644
--- a/source/RobotAPI/libraries/diffik/DiffIKProvider.h
+++ b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
@@ -23,10 +23,10 @@
#pragma once
-#include <Eigen/Core>
-
#include <memory>
+#include <Eigen/Core>
+
namespace armarx
{
typedef std::shared_ptr<class DiffIKProvider> DiffIKProviderPtr;
@@ -37,15 +37,15 @@ namespace armarx
float posError;
float oriError;
Eigen::VectorXf jointValues;
-
};
class DiffIKProvider
{
public:
virtual DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose) = 0;
- virtual DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues) = 0;
+ virtual DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose,
+ const Eigen::VectorXf& startJointValues) = 0;
virtual ~DiffIKProvider() = default;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
index ae7aace621b852c4d077aebd52afb0289da09d6e..778aad90af54340fa99e40d980240e188c057c5a 100644
--- a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
@@ -23,44 +23,59 @@
#include "GraspTrajectory.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include <VirtualRobot/math/AbstractFunctionR1R6.h>
#include <VirtualRobot/math/Helpers.h>
-using namespace armarx;
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+using namespace armarx;
-GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget)
- : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(0),
- feedForwardPosVelocity(0, 0, 0), feedForwardOriVelocity(0, 0, 0),
- feedForwardHandJointsVelocity(Eigen::VectorXf::Zero(handJointsTarget.rows()))
-{ }
+GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::VectorXf& handJointsTarget) :
+ tcpTarget(tcpTarget),
+ handJointsTarget(handJointsTarget),
+ dt(0),
+ feedForwardPosVelocity(0, 0, 0),
+ feedForwardOriVelocity(0, 0, 0),
+ feedForwardHandJointsVelocity(Eigen::VectorXf::Zero(handJointsTarget.rows()))
+{
+}
-GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget,
- float dt, const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity,
- const Eigen::VectorXf& feedForwardHandJointsVelocity)
- : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(dt),
- feedForwardPosVelocity(feedForwardPosVelocity), feedForwardOriVelocity(feedForwardOriVelocity),
- feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
-{ }
+GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::VectorXf& handJointsTarget,
+ float dt,
+ const Eigen::Vector3f& feedForwardPosVelocity,
+ const Eigen::Vector3f& feedForwardOriVelocity,
+ const Eigen::VectorXf& feedForwardHandJointsVelocity) :
+ tcpTarget(tcpTarget),
+ handJointsTarget(handJointsTarget),
+ dt(dt),
+ feedForwardPosVelocity(feedForwardPosVelocity),
+ feedForwardOriVelocity(feedForwardOriVelocity),
+ feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
+{
+}
-Eigen::Vector3f GraspTrajectory::Keypoint::getTargetPosition() const
+Eigen::Vector3f
+GraspTrajectory::Keypoint::getTargetPosition() const
{
return ::math::Helpers::GetPosition(tcpTarget);
}
-Eigen::Matrix3f GraspTrajectory::Keypoint::getTargetOrientation() const
+Eigen::Matrix3f
+GraspTrajectory::Keypoint::getTargetOrientation() const
{
return ::math::Helpers::GetOrientation(tcpTarget);
}
-Eigen::Matrix4f GraspTrajectory::Keypoint::getTargetPose() const
+Eigen::Matrix4f
+GraspTrajectory::Keypoint::getTargetPose() const
{
return tcpTarget;
}
-void GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr& prev, float dt)
+void
+GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr& prev, float dt)
{
Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(prev->tcpTarget);
Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(prev->tcpTarget);
@@ -80,14 +95,18 @@ void GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::Keypoint
feedForwardHandJointsVelocity = dhnd / dt;
}
-GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::VectorXf& handJointsStart)
+GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f& tcpStart,
+ const Eigen::VectorXf& handJointsStart)
{
KeypointPtr keypoint(new Keypoint(tcpStart, handJointsStart));
keypointMap[0] = keypoints.size();
keypoints.push_back(keypoint);
}
-void GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt)
+void
+GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::VectorXf& handJointsTarget,
+ float dt)
{
ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
KeypointPtr prev = lastKeypoint();
@@ -98,12 +117,17 @@ void GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen:
keypoints.push_back(keypoint);
}
-size_t GraspTrajectory::getKeypointCount() const
+size_t
+GraspTrajectory::getKeypointCount() const
{
return keypoints.size();
}
-void GraspTrajectory::insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt)
+void
+GraspTrajectory::insertKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const Eigen::VectorXf& handJointsTarget,
+ float dt)
{
ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
if (index <= 0 || index > keypoints.size())
@@ -122,7 +146,8 @@ void GraspTrajectory::insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTar
updateKeypointMap();
}
-void GraspTrajectory::removeKeypoint(size_t index)
+void
+GraspTrajectory::removeKeypoint(size_t index)
{
if (index <= 0 || index >= keypoints.size())
{
@@ -138,7 +163,11 @@ void GraspTrajectory::removeKeypoint(size_t index)
updateKeypointMap();
}
-void GraspTrajectory::replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt)
+void
+GraspTrajectory::replaceKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const Eigen::VectorXf& handJointsTarget,
+ float dt)
{
ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
if (index <= 0 || index >= keypoints.size())
@@ -152,7 +181,8 @@ void GraspTrajectory::replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTa
updateKeypointMap();
}
-void GraspTrajectory::setKeypointDt(size_t index, float dt)
+void
+GraspTrajectory::setKeypointDt(size_t index, float dt)
{
if (index <= 0 || index >= keypoints.size())
{
@@ -164,22 +194,26 @@ void GraspTrajectory::setKeypointDt(size_t index, float dt)
updateKeypointMap();
}
-GraspTrajectory::KeypointPtr& GraspTrajectory::lastKeypoint()
+GraspTrajectory::KeypointPtr&
+GraspTrajectory::lastKeypoint()
{
return keypoints.at(keypoints.size() - 1);
}
-GraspTrajectory::KeypointPtr& GraspTrajectory::getKeypoint(int i)
+GraspTrajectory::KeypointPtr&
+GraspTrajectory::getKeypoint(int i)
{
return keypoints.at(i);
}
-Eigen::Matrix4f GraspTrajectory::getStartPose()
+Eigen::Matrix4f
+GraspTrajectory::getStartPose()
{
return getKeypoint(0)->getTargetPose();
}
-void GraspTrajectory::getIndex(float t, int& i1, int& i2, float& f)
+void
+GraspTrajectory::getIndex(float t, int& i1, int& i2, float& f)
{
if (t <= 0)
{
@@ -203,15 +237,18 @@ void GraspTrajectory::getIndex(float t, int& i1, int& i2, float& f)
f = ::math::Helpers::ILerp(prev->first, it->first, t);
}
-Eigen::Vector3f GraspTrajectory::GetPosition(float t)
+Eigen::Vector3f
+GraspTrajectory::GetPosition(float t)
{
int i1, i2;
float f;
getIndex(t, i1, i2, f);
- return ::math::Helpers::Lerp(getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
+ return ::math::Helpers::Lerp(
+ getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
}
-Eigen::Matrix3f GraspTrajectory::GetOrientation(float t)
+Eigen::Matrix3f
+GraspTrajectory::GetOrientation(float t)
{
int i1, i2;
float f;
@@ -226,12 +263,14 @@ Eigen::Matrix3f GraspTrajectory::GetOrientation(float t)
return aa.toRotationMatrix() * getKeypoint(i1)->getTargetOrientation();
}
-Eigen::Matrix4f GraspTrajectory::GetPose(float t)
+Eigen::Matrix4f
+GraspTrajectory::GetPose(float t)
{
return ::math::Helpers::CreatePose(GetPosition(t), GetOrientation(t));
}
-std::vector<Eigen::Matrix4f> GraspTrajectory::getAllKeypointPoses()
+std::vector<Eigen::Matrix4f>
+GraspTrajectory::getAllKeypointPoses()
{
std::vector<Eigen::Matrix4f> res;
for (const KeypointPtr& keypoint : keypoints)
@@ -241,7 +280,8 @@ std::vector<Eigen::Matrix4f> GraspTrajectory::getAllKeypointPoses()
return res;
}
-std::vector<Eigen::Vector3f> GraspTrajectory::getAllKeypointPositions()
+std::vector<Eigen::Vector3f>
+GraspTrajectory::getAllKeypointPositions()
{
std::vector<Eigen::Vector3f> res;
for (const KeypointPtr& keypoint : keypoints)
@@ -251,7 +291,8 @@ std::vector<Eigen::Vector3f> GraspTrajectory::getAllKeypointPositions()
return res;
}
-std::vector<Eigen::Matrix3f> GraspTrajectory::getAllKeypointOrientations()
+std::vector<Eigen::Matrix3f>
+GraspTrajectory::getAllKeypointOrientations()
{
std::vector<Eigen::Matrix3f> res;
for (const KeypointPtr& keypoint : keypoints)
@@ -261,7 +302,8 @@ std::vector<Eigen::Matrix3f> GraspTrajectory::getAllKeypointOrientations()
return res;
}
-Eigen::VectorXf GraspTrajectory::GetHandValues(float t)
+Eigen::VectorXf
+GraspTrajectory::GetHandValues(float t)
{
int i1, i2;
float f;
@@ -270,7 +312,8 @@ Eigen::VectorXf GraspTrajectory::GetHandValues(float t)
return getKeypoint(i1)->handJointsTarget * (1 - f) + getKeypoint(i2)->handJointsTarget * f;
}
-Eigen::Vector3f GraspTrajectory::GetPositionDerivative(float t)
+Eigen::Vector3f
+GraspTrajectory::GetPositionDerivative(float t)
{
int i1, i2;
float f;
@@ -278,7 +321,8 @@ Eigen::Vector3f GraspTrajectory::GetPositionDerivative(float t)
return getKeypoint(i2)->feedForwardPosVelocity;
}
-Eigen::Vector3f GraspTrajectory::GetOrientationDerivative(float t)
+Eigen::Vector3f
+GraspTrajectory::GetOrientationDerivative(float t)
{
int i1, i2;
float f;
@@ -286,7 +330,8 @@ Eigen::Vector3f GraspTrajectory::GetOrientationDerivative(float t)
return getKeypoint(i2)->feedForwardOriVelocity;
}
-Eigen::Vector6f GraspTrajectory::GetTcpDerivative(float t)
+Eigen::Vector6f
+GraspTrajectory::GetTcpDerivative(float t)
{
Eigen::Vector6f ffVel;
ffVel.block<3, 1>(0, 0) = GetPositionDerivative(t);
@@ -294,7 +339,8 @@ Eigen::Vector6f GraspTrajectory::GetTcpDerivative(float t)
return ffVel;
}
-Eigen::VectorXf GraspTrajectory::GetHandJointsDerivative(float t)
+Eigen::VectorXf
+GraspTrajectory::GetHandJointsDerivative(float t)
{
int i1, i2;
float f;
@@ -302,12 +348,14 @@ Eigen::VectorXf GraspTrajectory::GetHandJointsDerivative(float t)
return getKeypoint(i2)->feedForwardHandJointsVelocity;
}
-float GraspTrajectory::getDuration() const
+float
+GraspTrajectory::getDuration() const
{
return keypointMap.rbegin()->first;
}
-GraspTrajectory::Length GraspTrajectory::calculateLength() const
+GraspTrajectory::Length
+GraspTrajectory::calculateLength() const
{
Length l;
for (size_t i = 1; i < keypoints.size(); i++)
@@ -327,25 +375,36 @@ GraspTrajectory::Length GraspTrajectory::calculateLength() const
return l;
}
-int GraspTrajectory::GetHandJointCount() const
+int
+GraspTrajectory::GetHandJointCount() const
{
return keypoints.at(0)->handJointsTarget.rows();
}
-GraspTrajectoryPtr GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation)
+GraspTrajectoryPtr
+GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f& translation,
+ const Eigen::Matrix3f& rotation)
{
- GraspTrajectoryPtr traj(new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(getKeypoint(0)->getTargetPose(), translation, rotation), getKeypoint(0)->handJointsTarget));
+ GraspTrajectoryPtr traj(
+ new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(
+ getKeypoint(0)->getTargetPose(), translation, rotation),
+ getKeypoint(0)->handJointsTarget));
for (size_t i = 1; i < keypoints.size(); i++)
{
KeypointPtr& kp = keypoints.at(i);
- traj->addKeypoint(::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation), kp->handJointsTarget, kp->dt);
+ traj->addKeypoint(
+ ::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation),
+ kp->handJointsTarget,
+ kp->dt);
}
return traj;
}
-GraspTrajectoryPtr GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform)
+GraspTrajectoryPtr
+GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform)
{
- GraspTrajectoryPtr traj(new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
+ GraspTrajectoryPtr traj(
+ new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
for (size_t i = 1; i < keypoints.size(); i++)
{
KeypointPtr& kp = keypoints.at(i);
@@ -354,12 +413,15 @@ GraspTrajectoryPtr GraspTrajectory::getTransformed(const Eigen::Matrix4f& transf
return traj;
}
-GraspTrajectoryPtr GraspTrajectory::getClone()
+GraspTrajectoryPtr
+GraspTrajectory::getClone()
{
return getTransformed(Eigen::Matrix4f::Identity());
}
-GraspTrajectoryPtr GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward)
+GraspTrajectoryPtr
+GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f& target,
+ const Eigen::Vector3f& handForward)
{
Eigen::Matrix4f startPose = getStartPose();
Eigen::Vector3f targetHandForward = ::math::Helpers::TransformDirection(target, handForward);
@@ -369,16 +431,24 @@ GraspTrajectoryPtr GraspTrajectory::getTransformedToGraspPose(const Eigen::Matri
float angle = ::math::Helpers::Angle(targetHandForward, trajHandForward, up);
Eigen::AngleAxisf aa(angle, up);
- Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(-::math::Helpers::GetPosition(startPose), aa.toRotationMatrix(), ::math::Helpers::GetPosition(target));
+ Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(
+ -::math::Helpers::GetPosition(startPose),
+ aa.toRotationMatrix(),
+ ::math::Helpers::GetPosition(target));
return getTransformed(transform);
}
-SimpleDiffIK::Reachability GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
+SimpleDiffIK::Reachability
+GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp,
+ SimpleDiffIK::Parameters params)
{
- return SimpleDiffIK::CalculateReachability(getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
+ return SimpleDiffIK::CalculateReachability(
+ getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
}
-void GraspTrajectory::writeToFile(const std::string& filename)
+void
+GraspTrajectory::writeToFile(const std::string& filename)
{
RapidXmlWriter writer;
RapidXmlWriterNode root = writer.createRootNode("GraspTrajectory");
@@ -393,15 +463,18 @@ void GraspTrajectory::writeToFile(const std::string& filename)
writer.saveToFile(filename, true);
}
-GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const grasping::GraspCandidatePtr& cnd)
+GraspTrajectoryPtr
+GraspTrajectory::ReadFromFile(const grasping::GraspCandidatePtr& cnd)
{
- std::string packageName = "armar6_skills";// cnd->executionHints->graspTrajectoryPackage;
+ std::string packageName = "armar6_skills"; // cnd->executionHints->graspTrajectoryPackage;
armarx::CMakePackageFinder finder(packageName);
std::string dataDir = finder.getDataDir() + "/" + packageName;
- return GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/" + cnd->executionHints->graspTrajectoryName + ".xml");
+ return GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/" +
+ cnd->executionHints->graspTrajectoryName + ".xml");
}
-GraspTrajectoryPtr GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
+GraspTrajectoryPtr
+GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
{
RapidXmlReaderNode root = reader->getRoot("GraspTrajectory");
GraspTrajectoryPtr traj;
@@ -442,17 +515,20 @@ GraspTrajectoryPtr GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& read
return traj;
}
-GraspTrajectoryPtr GraspTrajectory::ReadFromFile(const std::string& filename)
+GraspTrajectoryPtr
+GraspTrajectory::ReadFromFile(const std::string& filename)
{
return ReadFromReader(RapidXmlReader::FromFile(filename));
}
-GraspTrajectoryPtr GraspTrajectory::ReadFromString(const std::string& xml)
+GraspTrajectoryPtr
+GraspTrajectory::ReadFromString(const std::string& xml)
{
return ReadFromReader(RapidXmlReader::FromXmlString(xml));
}
-void GraspTrajectory::updateKeypointMap()
+void
+GraspTrajectory::updateKeypointMap()
{
keypointMap.clear();
float t = 0;
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.h b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
index 777ed4066e29dd153284021272039e8cb37d61cc..a8b98b2a81cd2850d08c2be7cb8e7d973efd38aa 100644
--- a/source/RobotAPI/libraries/diffik/GraspTrajectory.h
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
@@ -24,13 +24,10 @@
#pragma once
+#include <map>
+#include <vector>
-#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
-#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
-#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
-
-#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
-#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+#include <Eigen/Core>
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
@@ -38,10 +35,12 @@
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/interface/serialization/Eigen.h>
-#include <Eigen/Core>
+#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
+#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
-#include <vector>
-#include <map>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
namespace armarx
{
@@ -64,8 +63,11 @@ namespace armarx
Eigen::VectorXf feedForwardHandJointsVelocity;
Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget);
- Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt,
- const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity,
+ Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::VectorXf& handJointsTarget,
+ float dt,
+ const Eigen::Vector3f& feedForwardPosVelocity,
+ const Eigen::Vector3f& feedForwardOriVelocity,
const Eigen::VectorXf& feedForwardHandJointsVelocity);
Eigen::Vector3f getTargetPosition() const;
Eigen::Matrix3f getTargetOrientation() const;
@@ -83,15 +85,23 @@ namespace armarx
public:
GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::VectorXf& handJointsStart);
- void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+ void addKeypoint(const Eigen::Matrix4f& tcpTarget,
+ const Eigen::VectorXf& handJointsTarget,
+ float dt);
size_t getKeypointCount() const;
- void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+ void insertKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const Eigen::VectorXf& handJointsTarget,
+ float dt);
void removeKeypoint(size_t index);
- void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+ void replaceKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const Eigen::VectorXf& handJointsTarget,
+ float dt);
void setKeypointDt(size_t index, float dt);
@@ -127,14 +137,20 @@ namespace armarx
int GetHandJointCount() const;
- GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation);
+ GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation,
+ const Eigen::Matrix3f& rotation);
GraspTrajectoryPtr getTransformed(const Eigen::Matrix4f& transform);
GraspTrajectoryPtr getClone();
- GraspTrajectoryPtr getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
+ GraspTrajectoryPtr
+ getTransformedToGraspPose(const Eigen::Matrix4f& target,
+ const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
- SimpleDiffIK::Reachability calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
+ SimpleDiffIK::Reachability
+ calculateReachability(VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(),
+ SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
void writeToFile(const std::string& filename);
@@ -145,11 +161,10 @@ namespace armarx
static GraspTrajectoryPtr ReadFromString(const std::string& xml);
private:
-
void updateKeypointMap();
private:
std::vector<KeypointPtr> keypoints;
std::map<float, size_t> keypointMap;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
index 802f1e01c139423de6a09eec82eb986e77951247..44dc01cafaa45dbc4edbff6546b5d2c438944c8c 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.cpp
@@ -24,19 +24,19 @@
#include "NaturalDiffIK.h"
+#include <cfloat>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <cfloat>
-
namespace armarx
{
- Eigen::VectorXf NaturalDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
+ Eigen::VectorXf
+ NaturalDiffIK::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
{
float infNorm = vec.lpNorm<Eigen::Infinity>();
if (infNorm > maxValue)
@@ -46,7 +46,14 @@ namespace armarx
return vec;
}
- NaturalDiffIK::Result NaturalDiffIK::CalculateDiffIK(const Eigen::Matrix4f& targetPose, const Eigen::Vector3f& elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, Mode setOri, Parameters params)
+ NaturalDiffIK::Result
+ NaturalDiffIK::CalculateDiffIK(const Eigen::Matrix4f& targetPose,
+ const Eigen::Vector3f& elbowTarget,
+ VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp,
+ VirtualRobot::RobotNodePtr elbow,
+ Mode setOri,
+ Parameters params)
{
VirtualRobot::IKSolver::CartesianSelection mode = ModeToCartesianSelection(setOri);
@@ -86,8 +93,10 @@ namespace armarx
int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
- Eigen::Vector3f pdTcp = posLen ? pcTcp.getPositionDiff(targetPose) : Eigen::Vector3f::Zero();
- Eigen::Vector3f odTcp = oriLen ? pcTcp.getOrientationDiff(targetPose) : Eigen::Vector3f::Zero();
+ Eigen::Vector3f pdTcp =
+ posLen ? pcTcp.getPositionDiff(targetPose) : Eigen::Vector3f::Zero();
+ Eigen::Vector3f odTcp =
+ oriLen ? pcTcp.getOrientationDiff(targetPose) : Eigen::Vector3f::Zero();
Eigen::VectorXf cartesianVel(posLen + oriLen);
if (posLen)
{
@@ -101,12 +110,15 @@ namespace armarx
Eigen::Vector3f pdElb = pcElb.getPositionDiffVec3(elbowTarget);
Eigen::VectorXf cartesianVelElb(3);
cartesianVelElb.block<3, 1>(0, 0) = pdElb;
- Eigen::VectorXf jvElb = params.elbowKp * vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
- Eigen::VectorXf jvLA = params.jointLimitAvoidanceKp * vcTcp.calculateJointLimitAvoidance();
+ Eigen::VectorXf jvElb =
+ params.elbowKp * vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
+ Eigen::VectorXf jvLA =
+ params.jointLimitAvoidanceKp * vcTcp.calculateJointLimitAvoidance();
Eigen::VectorXf jv = vcTcp.calculate(cartesianVel, jvElb + jvLA, mode);
- float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ float stepLength =
+ i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
Eigen::VectorXf jvClamped = jv * stepLength;
jvClamped = LimitInfNormTo(jvClamped, params.maxJointAngleStep);
@@ -165,7 +177,8 @@ namespace armarx
result.oriDiff = pcTcp.getOrientationDiff(targetPose);
result.posError = pcTcp.getPositionError(targetPose);
result.oriError = pcTcp.getOrientationError(targetPose);
- result.reached = result.posError < params.maxPosError && (setOri == Mode::Position || result.oriError < params.maxOriError);
+ result.reached = result.posError < params.maxPosError &&
+ (setOri == Mode::Position || result.oriError < params.maxOriError);
result.posDiffElbow = pcElb.getPositionDiffVec3(elbowTarget);
result.posErrorElbow = result.posDiffElbow.norm();
@@ -180,19 +193,21 @@ namespace armarx
}
else
{
- result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(), rn->getJointLimitHi() - rn->getJointValue());
- result.minimumJointLimitMargin = std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
+ result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(),
+ rn->getJointLimitHi() - rn->getJointValue());
+ result.minimumJointLimitMargin =
+ std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
}
}
return result;
}
- VirtualRobot::IKSolver::CartesianSelection NaturalDiffIK::ModeToCartesianSelection(NaturalDiffIK::Mode mode)
+ VirtualRobot::IKSolver::CartesianSelection
+ NaturalDiffIK::ModeToCartesianSelection(NaturalDiffIK::Mode mode)
{
return mode == Mode::All ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
}
-
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
index dbbd1e1172e271887392f8cfd6433b0578b2d0d6..3c235697848901110637ca5f36ce49fc268b4d81 100644
--- a/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/NaturalDiffIK.h
@@ -23,18 +23,26 @@
#pragma once
-#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/IK/IKSolver.h>
+#include <VirtualRobot/VirtualRobot.h>
namespace armarx
{
class NaturalDiffIK
{
public:
- enum class Mode {Position, All};
+ enum class Mode
+ {
+ Position,
+ All
+ };
+
struct Parameters
{
- Parameters() {}
+ Parameters()
+ {
+ }
+
// IK params
float ikStepLengthInitial = 0.2f;
float ikStepLengthFineTune = 0.5f;
@@ -48,6 +56,7 @@ namespace armarx
bool resetRnsValues = true;
bool returnIKSteps = false;
};
+
struct IKStep
{
Eigen::VectorXf jointValues;
@@ -81,7 +90,13 @@ namespace armarx
};
static Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue);
- static Result CalculateDiffIK(const Eigen::Matrix4f& targetPose, const Eigen::Vector3f& elbowTarget, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, VirtualRobot::RobotNodePtr elbow, Mode setOri, Parameters params = Parameters());
+ static Result CalculateDiffIK(const Eigen::Matrix4f& targetPose,
+ const Eigen::Vector3f& elbowTarget,
+ VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp,
+ VirtualRobot::RobotNodePtr elbow,
+ Mode setOri,
+ Parameters params = Parameters());
static VirtualRobot::IKSolver::CartesianSelection ModeToCartesianSelection(Mode mode);
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/diffik/RobotPlacement.cpp b/source/RobotAPI/libraries/diffik/RobotPlacement.cpp
index 4075c9aae309b66ce51d7125ede732b4869ba88d..cf44e19f1f501cd1b42ffd2ed4b24b4908061192 100644
--- a/source/RobotAPI/libraries/diffik/RobotPlacement.cpp
+++ b/source/RobotAPI/libraries/diffik/RobotPlacement.cpp
@@ -31,19 +31,32 @@ RobotPlacement::RobotPlacement(const DiffIKProviderPtr& ikProvider)
{
}
-std::vector<Eigen::Matrix4f> RobotPlacement::CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa)
+std::vector<Eigen::Matrix4f>
+RobotPlacement::CreateGrid(float dx,
+ int minx,
+ int maxx,
+ float dy,
+ int miny,
+ int maxy,
+ float da,
+ int mina,
+ int maxa)
{
std::vector<Eigen::Matrix4f> r;
for (int x = minx; x <= maxx; x++)
for (int y = miny; y <= maxy; y++)
for (int a = mina; a <= maxa; a++)
{
- r.emplace_back(math::Helpers::CreatePose(Eigen::Vector3f(x * dx, y * dy, 0), Eigen::AngleAxisf(a * da, Eigen::Vector3f::UnitZ()).toRotationMatrix()));
+ r.emplace_back(math::Helpers::CreatePose(
+ Eigen::Vector3f(x * dx, y * dy, 0),
+ Eigen::AngleAxisf(a * da, Eigen::Vector3f::UnitZ()).toRotationMatrix()));
}
return r;
}
-std::vector<RobotPlacement::Result> RobotPlacement::evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const RobotPlacement::PlacementParams& params)
+std::vector<RobotPlacement::Result>
+RobotPlacement::evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements,
+ const RobotPlacement::PlacementParams& params)
{
std::vector<RobotPlacement::Result> r;
std::vector<Eigen::Matrix4f> tcpTargets;
diff --git a/source/RobotAPI/libraries/diffik/RobotPlacement.h b/source/RobotAPI/libraries/diffik/RobotPlacement.h
index 0eb34d3f3c4cbf43c8b50c78e7e8c9a41a179ec6..557aab04bf3befdea043fac55c1805aa81d8082a 100644
--- a/source/RobotAPI/libraries/diffik/RobotPlacement.h
+++ b/source/RobotAPI/libraries/diffik/RobotPlacement.h
@@ -23,11 +23,11 @@
#pragma once
+#include <memory>
+
#include "DiffIKProvider.h"
#include "GraspTrajectory.h"
-#include <memory>
-
namespace armarx
{
typedef std::shared_ptr<class RobotPlacement> RobotPlacementPtr;
@@ -39,21 +39,32 @@ namespace armarx
{
DiffIKResult ikResult;
};
+
struct PlacementParams
{
std::vector<Eigen::Matrix4f> prePoses;
GraspTrajectoryPtr graspTrajectory;
};
+
public:
RobotPlacement(const DiffIKProviderPtr& ikProvider);
- static std::vector<Eigen::Matrix4f> CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa);
+ static std::vector<Eigen::Matrix4f> CreateGrid(float dx,
+ int minx,
+ int maxx,
+ float dy,
+ int miny,
+ int maxy,
+ float da,
+ int mina,
+ int maxa);
- std::vector<Result> evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const PlacementParams& params);
+ std::vector<Result> evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements,
+ const PlacementParams& params);
private:
DiffIKProviderPtr ikProvider;
bool returnOnlyReachable = true;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
index ef6a4d91b9d916ea46725f5db6e98b58bd0ef5b9..21dde21fb7fe7fe1e6212a3935c4a56594a8cc90 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
@@ -23,17 +23,21 @@
#include "SimpleDiffIK.h"
+#include <cfloat>
+
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/libraries/core/CartesianPositionController.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <cfloat>
-
namespace armarx
{
- SimpleDiffIK::Result SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, Parameters params)
+ SimpleDiffIK::Result
+ SimpleDiffIK::CalculateDiffIK(const Eigen::Matrix4f targetPose,
+ VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp,
+ Parameters params)
{
tcp = tcp ? tcp : rns->getTCP();
@@ -66,12 +70,14 @@ namespace armarx
Eigen::VectorXf cartesianVel(6);
cartesianVel << posDiff(0), posDiff(1), posDiff(2), oriDiff(0), oriDiff(1), oriDiff(2);
- const Eigen::VectorXf jnv = params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
- const Eigen::VectorXf jv = velocityController.calculate(cartesianVel, jnv, VirtualRobot::IKSolver::All);
-
+ const Eigen::VectorXf jnv =
+ params.jointLimitAvoidanceKp * velocityController.calculateJointLimitAvoidance();
+ const Eigen::VectorXf jv =
+ velocityController.calculate(cartesianVel, jnv, VirtualRobot::IKSolver::All);
- float stepLength = i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
+ float stepLength =
+ i < params.stepsInitial ? params.ikStepLengthInitial : params.ikStepLengthFineTune;
Eigen::VectorXf jvClamped = jv * stepLength;
float infNorm = jvClamped.lpNorm<Eigen::Infinity>();
@@ -106,7 +112,8 @@ namespace armarx
result.oriDiff = positionController.getOrientationDiff(targetPose);
result.posError = positionController.getPositionError(targetPose);
result.oriError = positionController.getOrientationError(targetPose);
- result.reached = result.posError < params.maxPosError && result.oriError < params.maxOriError;
+ result.reached =
+ result.posError < params.maxPosError && result.oriError < params.maxOriError;
result.jointLimitMargins = Eigen::VectorXf::Zero(rns->getSize());
result.minimumJointLimitMargin = FLT_MAX;
@@ -119,15 +126,22 @@ namespace armarx
}
else
{
- result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(), rn->getJointLimitHi() - rn->getJointValue());
- result.minimumJointLimitMargin = std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
+ result.jointLimitMargins(i) = std::min(rn->getJointValue() - rn->getJointLimitLo(),
+ rn->getJointLimitHi() - rn->getJointValue());
+ result.minimumJointLimitMargin =
+ std::min(result.minimumJointLimitMargin, result.jointLimitMargins(i));
}
}
return result;
}
- SimpleDiffIK::Reachability SimpleDiffIK::CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
+ SimpleDiffIK::Reachability
+ SimpleDiffIK::CalculateReachability(const std::vector<Eigen::Matrix4f> targets,
+ const Eigen::VectorXf& initialJV,
+ VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp,
+ SimpleDiffIK::Parameters params)
{
Reachability r;
rns->setJointValues(initialJV);
@@ -143,12 +157,15 @@ namespace armarx
return r;
}
- SimpleDiffIKProvider::SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
- : rns(rns), tcp(tcp), params(params)
+ SimpleDiffIKProvider::SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp,
+ SimpleDiffIK::Parameters params) :
+ rns(rns), tcp(tcp), params(params)
{
}
- DiffIKResult SimpleDiffIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
+ DiffIKResult
+ SimpleDiffIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
{
params.resetRnsValues = true;
SimpleDiffIK::Result result = SimpleDiffIK::CalculateDiffIK(targetPose, rns, tcp, params);
@@ -158,10 +175,11 @@ namespace armarx
r.posError = result.posError;
r.reachable = result.reached;
return r;
-
}
- DiffIKResult SimpleDiffIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues)
+ DiffIKResult
+ SimpleDiffIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose,
+ const Eigen::VectorXf& startJointValues)
{
params.resetRnsValues = false;
rns->setJointValues(startJointValues);
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
index 583fef2a8fec427dbde267d28fb5863ba9da7942..213a49be4443fd1e24615c6b60f7f00da9b923f9 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
@@ -23,11 +23,11 @@
#pragma once
-#include "DiffIKProvider.h"
+#include <memory>
#include <VirtualRobot/VirtualRobot.h>
-#include <memory>
+#include "DiffIKProvider.h"
namespace armarx
{
@@ -38,7 +38,10 @@ namespace armarx
public:
struct Parameters
{
- Parameters() {}
+ Parameters()
+ {
+ }
+
// IK params
float ikStepLengthInitial = 0.2f;
float ikStepLengthFineTune = 0.5f;
@@ -51,6 +54,7 @@ namespace armarx
bool returnIKSteps = false;
bool resetRnsValues = true;
};
+
struct IKStep
{
Eigen::VectorXf jointValues;
@@ -76,7 +80,6 @@ namespace armarx
std::vector<IKStep> ikSteps;
};
-
struct Reachability
{
@@ -90,23 +93,33 @@ namespace armarx
void aggregate(const Result& result);
};
- static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
+ static Result CalculateDiffIK(const Eigen::Matrix4f targetPose,
+ VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(),
+ Parameters params = Parameters());
///@brief Use this to check a trajectory of waypoints
- static Reachability CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
+ static Reachability
+ CalculateReachability(const std::vector<Eigen::Matrix4f> targets,
+ const Eigen::VectorXf& initialJV,
+ VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(),
+ Parameters params = Parameters());
};
- class SimpleDiffIKProvider :
- public DiffIKProvider
+ class SimpleDiffIKProvider : public DiffIKProvider
{
public:
- SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
+ SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns,
+ VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(),
+ SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose);
- DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues);
+ DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose,
+ const Eigen::VectorXf& startJointValues);
private:
VirtualRobot::RobotNodeSetPtr rns;
VirtualRobot::RobotNodePtr tcp;
SimpleDiffIK::Parameters params;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/diffik/diffik.h b/source/RobotAPI/libraries/diffik/diffik.h
index 0cca7135d50c63101d0229593e0b2245786da79d..820789cef36db76f71843304a87e7abb431be84a 100644
--- a/source/RobotAPI/libraries/diffik/diffik.h
+++ b/source/RobotAPI/libraries/diffik/diffik.h
@@ -22,7 +22,6 @@
#pragma once
-
namespace armarx
{
/**
@@ -39,7 +38,6 @@ namespace armarx
class diffik
{
public:
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/diffik/test/diffikTest.cpp b/source/RobotAPI/libraries/diffik/test/diffikTest.cpp
index 8ebfea4268229e26053befcac6ca0ad005a6f135..1a01d9b579801115d8d14226bc37cbb535d6f54d 100644
--- a/source/RobotAPI/libraries/diffik/test/diffikTest.cpp
+++ b/source/RobotAPI/libraries/diffik/test/diffikTest.cpp
@@ -24,9 +24,10 @@
#define ARMARX_BOOST_TEST
-#include <RobotAPI/Test.h>
#include "../diffik.h"
+#include <RobotAPI/Test.h>
+
#include <iostream>
BOOST_AUTO_TEST_CASE(testExample)
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
index bb532f39331aeb2ec1c599a918f88dad373b5314..e63f53b82f2caba1847d6330faa6ecb224d548e6 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
@@ -22,27 +22,33 @@
*/
#include "CartesianNaturalPositionControllerProxy.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+
#include <RobotAPI/libraries/core/CartesianPositionController.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
//#include <RobotAPI/libraries/aron/core/navigator/Navigator.h>
+#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
-#include <VirtualRobot/MathTools.h>
using namespace armarx;
-
-
-CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config)
- : _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config), _arm(arm)
+CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(
+ const NaturalIK& ik,
+ const NaturalIK::ArmJoints& arm,
+ const RobotUnitInterfacePrx& robotUnit,
+ const std::string& controllerName,
+ NJointCartesianNaturalPositionControllerConfigPtr config) :
+ _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config), _arm(arm)
{
_runCfg = _config->runCfg;
_velocityBaseSettings.basePosVel = _runCfg.maxTcpPosVel;
_velocityBaseSettings.baseOriVel = _runCfg.maxTcpOriVel;
- _velocityBaseSettings.baseJointVelocity = CalculateMaxJointVelocity(arm.rns, _runCfg.maxTcpPosVel);
+ _velocityBaseSettings.baseJointVelocity =
+ CalculateMaxJointVelocity(arm.rns, _runCfg.maxTcpPosVel);
_userNullspaceTargets = std::vector<float>(arm.rns->getSize(), std::nanf(""));
_naturalNullspaceTargets = std::vector<float>(arm.rns->getSize(), std::nanf(""));
updateBaseKpValues(_runCfg);
@@ -67,15 +73,19 @@ CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy
_ftOffset.torque = Eigen::Vector3f::Zero();
}
-NJointCartesianNaturalPositionControllerConfigPtr CartesianNaturalPositionControllerProxy::MakeConfig(const std::string& rns, const std::string& elbowNode)
+NJointCartesianNaturalPositionControllerConfigPtr
+CartesianNaturalPositionControllerProxy::MakeConfig(const std::string& rns,
+ const std::string& elbowNode)
{
- NJointCartesianNaturalPositionControllerConfigPtr cfg = new NJointCartesianNaturalPositionControllerConfig();
+ NJointCartesianNaturalPositionControllerConfigPtr cfg =
+ new NJointCartesianNaturalPositionControllerConfig();
cfg->rns = rns;
cfg->elbowNode = elbowNode;
return cfg;
}
-void CartesianNaturalPositionControllerProxy::init()
+void
+CartesianNaturalPositionControllerProxy::init()
{
NJointControllerInterfacePrx ctrl = _robotUnit->getNJointController(_controllerName);
if (ctrl)
@@ -86,7 +96,8 @@ void CartesianNaturalPositionControllerProxy::init()
}
else
{
- ctrl = _robotUnit->createNJointController("NJointCartesianNaturalPositionController", _controllerName, _config);
+ ctrl = _robotUnit->createNJointController(
+ "NJointCartesianNaturalPositionController", _controllerName, _config);
_controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
_controllerCreated = true;
}
@@ -96,21 +107,26 @@ void CartesianNaturalPositionControllerProxy::init()
}
}
-bool CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget, NaturalDiffIK::Mode setOri, std::optional<float> minElbowHeight)
+bool
+CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget,
+ NaturalDiffIK::Mode setOri,
+ std::optional<float> minElbowHeight)
{
ScopedJointValueRestore jvr(_arm.rns);
_tcpTarget = tcpTarget;
_setOri = setOri;
_fwd = _ik.solveSoechtingIK(math::Helpers::Position(tcpTarget), minElbowHeight);
//VirtualRobot::IKSolver::CartesianSelection mode = setOri ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
- NaturalDiffIK::Result ikResult = NaturalDiffIK::CalculateDiffIK(tcpTarget, _fwd.elbow, _arm.rns, _arm.tcp, _arm.elbow, setOri, _natikParams.diffIKparams);
+ NaturalDiffIK::Result ikResult = NaturalDiffIK::CalculateDiffIK(
+ tcpTarget, _fwd.elbow, _arm.rns, _arm.tcp, _arm.elbow, setOri, _natikParams.diffIKparams);
if (!ikResult.reached)
{
ARMARX_ERROR << "could not solve natural IK for target: " << tcpTarget;
return false;
}
_elbTarget = _arm.elbow->getPoseInRootFrame();
- _controller->setTarget(_tcpTarget, _arm.elbow->getPositionInRootFrame(), setOri == NaturalDiffIK::Mode::All);
+ _controller->setTarget(
+ _tcpTarget, _arm.elbow->getPositionInRootFrame(), setOri == NaturalDiffIK::Mode::All);
if (_setJointNullspaceFromNaturalIK)
{
for (size_t i = 0; i < _rnsToElb.size(); i++)
@@ -123,29 +139,35 @@ bool CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& t
return true;
}
-void CartesianNaturalPositionControllerProxy::setNullspaceTarget(const std::vector<float>& nullspaceTargets)
+void
+CartesianNaturalPositionControllerProxy::setNullspaceTarget(
+ const std::vector<float>& nullspaceTargets)
{
ARMARX_CHECK(_arm.rns->getSize() == nullspaceTargets.size());
_userNullspaceTargets = nullspaceTargets;
updateNullspaceTargets();
}
-Eigen::Vector3f CartesianNaturalPositionControllerProxy::getCurrentTargetPosition()
+Eigen::Vector3f
+CartesianNaturalPositionControllerProxy::getCurrentTargetPosition()
{
return math::Helpers::Position(_tcpTarget);
}
-Eigen::Vector3f CartesianNaturalPositionControllerProxy::getCurrentElbowTargetPosition()
+Eigen::Vector3f
+CartesianNaturalPositionControllerProxy::getCurrentElbowTargetPosition()
{
return math::Helpers::Position(_elbTarget);
}
-float CartesianNaturalPositionControllerProxy::getTcpPositionError()
+float
+CartesianNaturalPositionControllerProxy::getTcpPositionError()
{
return CartesianPositionController::GetPositionError(_tcpTarget, _arm.tcp);
}
-float CartesianNaturalPositionControllerProxy::getTcpOrientationError()
+float
+CartesianNaturalPositionControllerProxy::getTcpOrientationError()
{
if (_setOri == NaturalDiffIK::Mode::Position)
{
@@ -154,23 +176,29 @@ float CartesianNaturalPositionControllerProxy::getTcpOrientationError()
return CartesianPositionController::GetOrientationError(_tcpTarget, _arm.tcp);
}
-float CartesianNaturalPositionControllerProxy::getElbPositionError()
+float
+CartesianNaturalPositionControllerProxy::getElbPositionError()
{
return CartesianPositionController::GetPositionError(_elbTarget, _arm.elbow);
}
-bool CartesianNaturalPositionControllerProxy::isFinalWaypointReached()
+bool
+CartesianNaturalPositionControllerProxy::isFinalWaypointReached()
{
return isLastWaypoint() && currentWaypoint().reached(_arm.tcp);
}
-void CartesianNaturalPositionControllerProxy::useCurrentFTasOffset()
+void
+CartesianNaturalPositionControllerProxy::useCurrentFTasOffset()
{
_ftOffset = _controller->getAverageFTValue();
_controller->setFTOffset(_ftOffset);
}
-void CartesianNaturalPositionControllerProxy::enableFTLimit(float force, float torque, bool useCurrentFTasOffset)
+void
+CartesianNaturalPositionControllerProxy::enableFTLimit(float force,
+ float torque,
+ bool useCurrentFTasOffset)
{
if (useCurrentFTasOffset)
{
@@ -179,13 +207,15 @@ void CartesianNaturalPositionControllerProxy::enableFTLimit(float force, float t
_controller->setFTLimit(force, torque);
}
-void CartesianNaturalPositionControllerProxy::disableFTLimit()
+void
+CartesianNaturalPositionControllerProxy::disableFTLimit()
{
_controller->clearFTLimit();
//_controller->resetFTOffset();
}
-FTSensorValue CartesianNaturalPositionControllerProxy::getCurrentFTValue(bool substactOffset)
+FTSensorValue
+CartesianNaturalPositionControllerProxy::getCurrentFTValue(bool substactOffset)
{
FTSensorValue ft = _controller->getCurrentFTValue();
if (substactOffset)
@@ -195,7 +225,9 @@ FTSensorValue CartesianNaturalPositionControllerProxy::getCurrentFTValue(bool su
}
return ft;
}
-armarx::FTSensorValue armarx::CartesianNaturalPositionControllerProxy::getAverageFTValue(bool substactOffset)
+
+armarx::FTSensorValue
+armarx::CartesianNaturalPositionControllerProxy::getAverageFTValue(bool substactOffset)
{
FTSensorValue ft = _controller->getAverageFTValue();
if (substactOffset)
@@ -206,7 +238,8 @@ armarx::FTSensorValue armarx::CartesianNaturalPositionControllerProxy::getAverag
return ft;
}
-void CartesianNaturalPositionControllerProxy::stopClear()
+void
+CartesianNaturalPositionControllerProxy::stopClear()
{
_controller->stopMovement();
_controller->clearFTLimit();
@@ -214,9 +247,8 @@ void CartesianNaturalPositionControllerProxy::stopClear()
clearWaypoints();
}
-
-
-void CartesianNaturalPositionControllerProxy::updateDynamicKp()
+void
+CartesianNaturalPositionControllerProxy::updateDynamicKp()
{
if (_dynamicKp.enabled)
{
@@ -229,11 +261,11 @@ void CartesianNaturalPositionControllerProxy::updateDynamicKp()
_runCfg.jointLimitAvoidanceKp = KpJla;
_controller->setConfig(_runCfg);
ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
-
}
}
-void CartesianNaturalPositionControllerProxy::updateNullspaceTargets()
+void
+CartesianNaturalPositionControllerProxy::updateNullspaceTargets()
{
std::vector<float> nsTargets = _userNullspaceTargets;
for (size_t i = 0; i < _naturalNullspaceTargets.size(); i++)
@@ -246,25 +278,33 @@ void CartesianNaturalPositionControllerProxy::updateNullspaceTargets()
_controller->setNullspaceTarget(nsTargets);
}
-void CartesianNaturalPositionControllerProxy::DynamicKp::calculate(float error, float& KpElb, float& KpJla)
+void
+CartesianNaturalPositionControllerProxy::DynamicKp::calculate(float error,
+ float& KpElb,
+ float& KpJla)
{
float f = std::exp(-0.5f * (error * error) / (sigmaMM * sigmaMM));
KpElb = f * maxKp;
KpJla = (1 - f) * maxKp;
}
-void CartesianNaturalPositionControllerProxy::setRuntimeConfig(CartesianNaturalPositionControllerConfig runCfg)
+void
+CartesianNaturalPositionControllerProxy::setRuntimeConfig(
+ CartesianNaturalPositionControllerConfig runCfg)
{
_controller->setConfig(runCfg);
this->_runCfg = runCfg;
}
-CartesianNaturalPositionControllerConfig CartesianNaturalPositionControllerProxy::getRuntimeConfig()
+CartesianNaturalPositionControllerConfig
+CartesianNaturalPositionControllerProxy::getRuntimeConfig()
{
return _runCfg;
}
-void CartesianNaturalPositionControllerProxy::addWaypoint(const CartesianNaturalPositionControllerProxy::Waypoint& waypoint)
+void
+CartesianNaturalPositionControllerProxy::addWaypoint(
+ const CartesianNaturalPositionControllerProxy::Waypoint& waypoint)
{
if (_waypoints.size() == 0)
{
@@ -273,12 +313,18 @@ void CartesianNaturalPositionControllerProxy::addWaypoint(const CartesianNatural
_waypoints.emplace_back(waypoint);
}
-CartesianNaturalPositionControllerProxy::Waypoint CartesianNaturalPositionControllerProxy::createWaypoint(const Eigen::Vector3f& tcpTarget, const std::vector<float>& userNullspaceTargets, std::optional<float> minElbowHeight)
+CartesianNaturalPositionControllerProxy::Waypoint
+CartesianNaturalPositionControllerProxy::createWaypoint(
+ const Eigen::Vector3f& tcpTarget,
+ const std::vector<float>& userNullspaceTargets,
+ std::optional<float> minElbowHeight)
{
return createWaypoint(tcpTarget, minElbowHeight).setNullspaceTargets(userNullspaceTargets);
}
-CartesianNaturalPositionControllerProxy::Waypoint CartesianNaturalPositionControllerProxy::createWaypoint(const Eigen::Vector3f& tcpTarget, std::optional<float> minElbowHeight)
+CartesianNaturalPositionControllerProxy::Waypoint
+CartesianNaturalPositionControllerProxy::createWaypoint(const Eigen::Vector3f& tcpTarget,
+ std::optional<float> minElbowHeight)
{
Waypoint w;
w.config = _defaultWaypointConfigs["default"];
@@ -289,7 +335,9 @@ CartesianNaturalPositionControllerProxy::Waypoint CartesianNaturalPositionContro
return w;
}
-CartesianNaturalPositionControllerProxy::Waypoint CartesianNaturalPositionControllerProxy::createWaypoint(const Eigen::Matrix4f& tcpTarget, std::optional<float> minElbowHeight)
+CartesianNaturalPositionControllerProxy::Waypoint
+CartesianNaturalPositionControllerProxy::createWaypoint(const Eigen::Matrix4f& tcpTarget,
+ std::optional<float> minElbowHeight)
{
Waypoint w;
w.config = _defaultWaypointConfigs["default"];
@@ -300,26 +348,35 @@ CartesianNaturalPositionControllerProxy::Waypoint CartesianNaturalPositionContro
return w;
}
-void CartesianNaturalPositionControllerProxy::clearWaypoints()
+void
+CartesianNaturalPositionControllerProxy::clearWaypoints()
{
_waypoints.clear();
_currentWaypointIndex = 0;
}
-void CartesianNaturalPositionControllerProxy::setDefaultWaypointConfig(const CartesianNaturalPositionControllerProxy::WaypointConfig& config)
+void
+CartesianNaturalPositionControllerProxy::setDefaultWaypointConfig(
+ const CartesianNaturalPositionControllerProxy::WaypointConfig& config)
{
_defaultWaypointConfigs["default"] = config;
}
-std::string CartesianNaturalPositionControllerProxy::getStatusText()
+std::string
+CartesianNaturalPositionControllerProxy::getStatusText()
{
std::stringstream ss;
ss.precision(2);
- ss << std::fixed << "Waypoint: " << (_currentWaypointIndex + 1) << "/" << _waypoints.size() << " distance: " << getTcpPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getTcpOrientationError()) << " deg";
+ ss << std::fixed << "Waypoint: " << (_currentWaypointIndex + 1) << "/" << _waypoints.size()
+ << " distance: " << getTcpPositionError() << " mm "
+ << VirtualRobot::MathTools::rad2deg(getTcpOrientationError()) << " deg";
return ss.str();
}
-std::vector<float> CartesianNaturalPositionControllerProxy::CalculateMaxJointVelocity(const VirtualRobot::RobotNodeSetPtr& rns, float maxPosVel)
+std::vector<float>
+CartesianNaturalPositionControllerProxy::CalculateMaxJointVelocity(
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ float maxPosVel)
{
size_t len = rns->getSize();
std::vector<Eigen::Vector3f> positions;
@@ -345,7 +402,8 @@ std::vector<float> CartesianNaturalPositionControllerProxy::CalculateMaxJointVel
return result;
}
-std::vector<float> CartesianNaturalPositionControllerProxy::ScaleVec(const std::vector<float>& vec, float scale)
+std::vector<float>
+CartesianNaturalPositionControllerProxy::ScaleVec(const std::vector<float>& vec, float scale)
{
std::vector<float> result(vec.size(), 0);
for (size_t i = 0; i < vec.size(); i++)
@@ -355,12 +413,14 @@ std::vector<float> CartesianNaturalPositionControllerProxy::ScaleVec(const std::
return result;
}
-void CartesianNaturalPositionControllerProxy::setActivateControllerOnInit(bool value)
+void
+CartesianNaturalPositionControllerProxy::setActivateControllerOnInit(bool value)
{
_activateControllerOnInit = value;
}
-void CartesianNaturalPositionControllerProxy::setMaxVelocities(float referencePosVel)
+void
+CartesianNaturalPositionControllerProxy::setMaxVelocities(float referencePosVel)
{
VelocityBaseSettings& v = _velocityBaseSettings;
KpBaseSettings& k = _kpBaseSettings;
@@ -369,20 +429,23 @@ void CartesianNaturalPositionControllerProxy::setMaxVelocities(float referencePo
_runCfg.maxTcpOriVel = v.baseOriVel * v.scaleTcpOriVel * scale;
_runCfg.maxElbPosVel = v.basePosVel * v.scaleElbPosVel * scale;
_runCfg.maxJointVelocity = ScaleVec(v.baseJointVelocity, v.scaleJointVelocities * scale);
- _runCfg.maxNullspaceVelocity = ScaleVec(v.baseJointVelocity, v.scaleNullspaceVelocities * scale);
- _runCfg.KpPos = k.baseKpTcpPos; // * scale;
- _runCfg.KpOri = k.baseKpTcpOri; // * scale;
- _runCfg.elbowKp = k.baseKpElbPos; // * scale;
+ _runCfg.maxNullspaceVelocity =
+ ScaleVec(v.baseJointVelocity, v.scaleNullspaceVelocities * scale);
+ _runCfg.KpPos = k.baseKpTcpPos; // * scale;
+ _runCfg.KpOri = k.baseKpTcpOri; // * scale;
+ _runCfg.elbowKp = k.baseKpElbPos; // * scale;
_runCfg.jointLimitAvoidanceKp = k.baseKpJla; // * scale;
- _runCfg.nullspaceTargetKp = k.baseKpNs; // * scale;
- _runCfg.maxNullspaceAcceleration = k.maxNullspaceAcceleration; // * scale;
- _runCfg.maxPositionAcceleration = k.maxPositionAcceleration; // * scale;
+ _runCfg.nullspaceTargetKp = k.baseKpNs; // * scale;
+ _runCfg.maxNullspaceAcceleration = k.maxNullspaceAcceleration; // * scale;
+ _runCfg.maxPositionAcceleration = k.maxPositionAcceleration; // * scale;
_runCfg.maxOrientationAcceleration = k.maxOrientationAcceleration; // * scale;
_controller->setConfig(_runCfg);
}
-void CartesianNaturalPositionControllerProxy::updateBaseKpValues(const CartesianNaturalPositionControllerConfig& runCfg)
+void
+CartesianNaturalPositionControllerProxy::updateBaseKpValues(
+ const CartesianNaturalPositionControllerConfig& runCfg)
{
_kpBaseSettings.baseKpTcpPos = _runCfg.KpPos;
_kpBaseSettings.baseKpTcpOri = _runCfg.KpOri;
@@ -394,7 +457,8 @@ void CartesianNaturalPositionControllerProxy::updateBaseKpValues(const Cartesian
_kpBaseSettings.maxOrientationAcceleration = _runCfg.maxOrientationAcceleration;
}
-void CartesianNaturalPositionControllerProxy::cleanup()
+void
+CartesianNaturalPositionControllerProxy::cleanup()
{
if (_controllerCreated)
{
@@ -409,7 +473,8 @@ void CartesianNaturalPositionControllerProxy::cleanup()
_controllerCreated = false;
}
-bool CartesianNaturalPositionControllerProxy::update()
+bool
+CartesianNaturalPositionControllerProxy::update()
{
if (_waypoints.size() == 0)
{
@@ -429,57 +494,73 @@ bool CartesianNaturalPositionControllerProxy::update()
return onWaypointChanged();
}
return true;
-
}
-bool CartesianNaturalPositionControllerProxy::onWaypointChanged()
+
+bool
+CartesianNaturalPositionControllerProxy::onWaypointChanged()
{
Waypoint& w = currentWaypoint();
setMaxVelocities(w.config.referencePosVel);
_userNullspaceTargets = w.targets.userNullspaceTargets;
- ARMARX_IMPORTANT << "Waypoint target position: " << math::Helpers::GetPosition(w.targets.tcpTarget).transpose();
+ ARMARX_IMPORTANT << "Waypoint target position: "
+ << math::Helpers::GetPosition(w.targets.tcpTarget).transpose();
return setTarget(w.targets.tcpTarget, w.targets.setOri, w.targets.minElbowHeight);
}
-CartesianNaturalPositionControllerProxy::Waypoint& CartesianNaturalPositionControllerProxy::currentWaypoint()
+CartesianNaturalPositionControllerProxy::Waypoint&
+CartesianNaturalPositionControllerProxy::currentWaypoint()
{
ARMARX_CHECK(_waypoints.size() > 0);
return _waypoints.at(_currentWaypointIndex);
}
-bool CartesianNaturalPositionControllerProxy::isLastWaypoint()
+bool
+CartesianNaturalPositionControllerProxy::isLastWaypoint()
{
return _waypoints.size() == 0 || _currentWaypointIndex == _waypoints.size() - 1;
}
-NJointCartesianNaturalPositionControllerInterfacePrx CartesianNaturalPositionControllerProxy::getInternalController()
+NJointCartesianNaturalPositionControllerInterfacePrx
+CartesianNaturalPositionControllerProxy::getInternalController()
{
return _controller;
}
-void CartesianNaturalPositionControllerProxy::setDynamicKp(DynamicKp dynamicKp)
+void
+CartesianNaturalPositionControllerProxy::setDynamicKp(DynamicKp dynamicKp)
{
_dynamicKp = dynamicKp;
updateDynamicKp();
}
-CartesianNaturalPositionControllerProxy::DynamicKp CartesianNaturalPositionControllerProxy::getDynamicKp()
+CartesianNaturalPositionControllerProxy::DynamicKp
+CartesianNaturalPositionControllerProxy::getDynamicKp()
{
return _dynamicKp;
}
-
-bool CartesianNaturalPositionControllerProxy::Waypoint::reached(const VirtualRobot::RobotNodePtr& tcp)
+bool
+CartesianNaturalPositionControllerProxy::Waypoint::reached(const VirtualRobot::RobotNodePtr& tcp)
{
- return CartesianPositionController::Reached(targets.tcpTarget, tcp, targets.setOri == NaturalDiffIK::Mode::All, config.thresholdTcpPosReached, config.thresholdTcpPosReached / config.rad2mmFactor);
+ return CartesianPositionController::Reached(targets.tcpTarget,
+ tcp,
+ targets.setOri == NaturalDiffIK::Mode::All,
+ config.thresholdTcpPosReached,
+ config.thresholdTcpPosReached /
+ config.rad2mmFactor);
}
-CartesianNaturalPositionControllerProxy::Waypoint& CartesianNaturalPositionControllerProxy::Waypoint::setConfig(const CartesianNaturalPositionControllerProxy::WaypointConfig& config)
+CartesianNaturalPositionControllerProxy::Waypoint&
+CartesianNaturalPositionControllerProxy::Waypoint::setConfig(
+ const CartesianNaturalPositionControllerProxy::WaypointConfig& config)
{
this->config = config;
return *this;
}
-CartesianNaturalPositionControllerProxy::Waypoint& CartesianNaturalPositionControllerProxy::Waypoint::setNullspaceTargets(const std::vector<float>& userNullspaceTargets)
+CartesianNaturalPositionControllerProxy::Waypoint&
+CartesianNaturalPositionControllerProxy::Waypoint::setNullspaceTargets(
+ const std::vector<float>& userNullspaceTargets)
{
ARMARX_CHECK(this->targets.userNullspaceTargets.size() == userNullspaceTargets.size());
this->targets.userNullspaceTargets = userNullspaceTargets;
@@ -522,8 +603,9 @@ void CartesianNaturalPositionControllerProxy::Waypoint::fromAron(aron::AronObjec
}*/
-CartesianNaturalPositionControllerProxy::ScopedJointValueRestore::ScopedJointValueRestore(const VirtualRobot::RobotNodeSetPtr& rns)
- : jointValues(rns->getJointValues()), rns(rns)
+CartesianNaturalPositionControllerProxy::ScopedJointValueRestore::ScopedJointValueRestore(
+ const VirtualRobot::RobotNodeSetPtr& rns) :
+ jointValues(rns->getJointValues()), rns(rns)
{
}
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
index c2ccc06f5ce8b1a43106443ff3c169d0e12aa4e3..72a9f3b89f263d0914db3159b63c2465f0267046 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
@@ -23,17 +23,18 @@
#pragma once
-#include "NaturalIK.h"
+#include <memory>
+#include <RobotAPI/interface/aron.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include <RobotAPI/interface/aron.h>
-#include <memory>
+#include "NaturalIK.h"
namespace armarx
{
- typedef std::shared_ptr<class CartesianNaturalPositionControllerProxy> CartesianNaturalPositionControllerProxyPtr;
+ typedef std::shared_ptr<class CartesianNaturalPositionControllerProxy>
+ CartesianNaturalPositionControllerProxyPtr;
class CartesianNaturalPositionControllerProxy
{
@@ -78,6 +79,7 @@ namespace armarx
float rad2mmFactor = 100 / M_PI;
//float thresholdTcpOriReached;
};
+
struct WaypointTargets
{
Eigen::Matrix4f tcpTarget;
@@ -109,17 +111,26 @@ namespace armarx
public:
ScopedJointValueRestore(const VirtualRobot::RobotNodeSetPtr& rns);
~ScopedJointValueRestore();
+
private:
std::vector<float> jointValues;
VirtualRobot::RobotNodeSetPtr rns;
};
- CartesianNaturalPositionControllerProxy(const NaturalIK& _ik, const NaturalIK::ArmJoints& arm, const RobotUnitInterfacePrx& _robotUnit, const std::string& _controllerName, NJointCartesianNaturalPositionControllerConfigPtr _config);
- static NJointCartesianNaturalPositionControllerConfigPtr MakeConfig(const std::string& rns, const std::string& elbowNode);
+ CartesianNaturalPositionControllerProxy(
+ const NaturalIK& _ik,
+ const NaturalIK::ArmJoints& arm,
+ const RobotUnitInterfacePrx& _robotUnit,
+ const std::string& _controllerName,
+ NJointCartesianNaturalPositionControllerConfigPtr _config);
+ static NJointCartesianNaturalPositionControllerConfigPtr
+ MakeConfig(const std::string& rns, const std::string& elbowNode);
void init();
- bool setTarget(const Eigen::Matrix4f& tcpTarget, NaturalDiffIK::Mode setOri, std::optional<float> minElbowHeight = std::nullopt);
+ bool setTarget(const Eigen::Matrix4f& tcpTarget,
+ NaturalDiffIK::Mode setOri,
+ std::optional<float> minElbowHeight = std::nullopt);
void setNullspaceTarget(const std::vector<float>& nullspaceTargets);
Eigen::Vector3f getCurrentTargetPosition();
@@ -143,9 +154,13 @@ namespace armarx
CartesianNaturalPositionControllerConfig getRuntimeConfig();
void addWaypoint(const Waypoint& waypoint);
- Waypoint createWaypoint(const Eigen::Vector3f& tcpTarget, const std::vector<float>& userNullspaceTargets, std::optional<float> minElbowHeight = std::nullopt);
- Waypoint createWaypoint(const Eigen::Vector3f& tcpTarget, std::optional<float> minElbowHeight = std::nullopt);
- Waypoint createWaypoint(const Eigen::Matrix4f& tcpTarget, std::optional<float> minElbowHeight = std::nullopt);
+ Waypoint createWaypoint(const Eigen::Vector3f& tcpTarget,
+ const std::vector<float>& userNullspaceTargets,
+ std::optional<float> minElbowHeight = std::nullopt);
+ Waypoint createWaypoint(const Eigen::Vector3f& tcpTarget,
+ std::optional<float> minElbowHeight = std::nullopt);
+ Waypoint createWaypoint(const Eigen::Matrix4f& tcpTarget,
+ std::optional<float> minElbowHeight = std::nullopt);
void clearWaypoints();
void setDefaultWaypointConfig(const WaypointConfig& config);
@@ -156,7 +171,8 @@ namespace armarx
//void setNullSpaceControl(bool enabled);
- static std::vector<float> CalculateMaxJointVelocity(const VirtualRobot::RobotNodeSetPtr& rns, float maxPosVel);
+ static std::vector<float>
+ CalculateMaxJointVelocity(const VirtualRobot::RobotNodeSetPtr& rns, float maxPosVel);
void setMaxVelocities(float referencePosVel);
void updateBaseKpValues(const CartesianNaturalPositionControllerConfig& runCfg);
@@ -211,4 +227,4 @@ namespace armarx
FTSensorValue _ftOffset;
bool _activateControllerOnInit = false;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.cpp b/source/RobotAPI/libraries/natik/NaturalIK.cpp
index 6a561959d34228fe6b64a436e74e713891ffcb47..9f52f6cd36322ef6d0e774c1378162b855c94775 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.cpp
+++ b/source/RobotAPI/libraries/natik/NaturalIK.cpp
@@ -22,7 +22,6 @@
*/
#include "NaturalIK.h"
-#include <ArmarXCore/core/exceptions/Exception.h>
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/math/convert/rad_to_deg.h>
@@ -30,17 +29,18 @@
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
+#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/logging/Logging.h>
using namespace armarx;
-NaturalIK::NaturalIK(std::string side, Eigen::Vector3f shoulderPos, float scale)
- : side(side), shoulderPos(shoulderPos), scale(scale)
+NaturalIK::NaturalIK(std::string side, Eigen::Vector3f shoulderPos, float scale) :
+ side(side), shoulderPos(shoulderPos), scale(scale)
{
-
}
-NaturalIK::SoechtingForwardPositions NaturalIK::solveSoechtingIK(const Eigen::Vector3f& targetPos, std::optional<float> minElbowHeight)
+NaturalIK::SoechtingForwardPositions
+NaturalIK::solveSoechtingIK(const Eigen::Vector3f& targetPos, std::optional<float> minElbowHeight)
{
Eigen::Vector3f vtgt = targetPos;
@@ -77,7 +77,6 @@ NaturalIK::SoechtingForwardPositions NaturalIK::solveSoechtingIK(const Eigen::Ve
{
lo = a;
}
-
}
fwd.elbow = elb;
}
@@ -85,35 +84,62 @@ NaturalIK::SoechtingForwardPositions NaturalIK::solveSoechtingIK(const Eigen::Ve
return fwd;
}
-NaturalDiffIK::Result NaturalIK::calculateIK(const Eigen::Matrix4f& targetPose, ArmJoints arm, NaturalIK::Parameters params)
+NaturalDiffIK::Result
+NaturalIK::calculateIK(const Eigen::Matrix4f& targetPose,
+ ArmJoints arm,
+ NaturalIK::Parameters params)
{
Eigen::Vector3f targetPos = math::Helpers::Position(targetPose);
- NaturalIK::SoechtingForwardPositions fwd = solveSoechtingIK(targetPos, params.minimumElbowHeight);
- return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, NaturalDiffIK::Mode::All, params.diffIKparams);
+ NaturalIK::SoechtingForwardPositions fwd =
+ solveSoechtingIK(targetPos, params.minimumElbowHeight);
+ return NaturalDiffIK::CalculateDiffIK(targetPose,
+ fwd.elbow,
+ arm.rns,
+ arm.tcp,
+ arm.elbow,
+ NaturalDiffIK::Mode::All,
+ params.diffIKparams);
}
-NaturalDiffIK::Result NaturalIK::calculateIKpos(const Eigen::Vector3f& targetPos, NaturalIK::ArmJoints arm, NaturalIK::Parameters params)
+NaturalDiffIK::Result
+NaturalIK::calculateIKpos(const Eigen::Vector3f& targetPos,
+ NaturalIK::ArmJoints arm,
+ NaturalIK::Parameters params)
{
Eigen::Matrix4f targetPose = math::Helpers::Pose(targetPos, Eigen::Matrix3f::Identity());
- NaturalIK::SoechtingForwardPositions fwd = solveSoechtingIK(targetPos, params.minimumElbowHeight);
- return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, NaturalDiffIK::Mode::Position, params.diffIKparams);
+ NaturalIK::SoechtingForwardPositions fwd =
+ solveSoechtingIK(targetPos, params.minimumElbowHeight);
+ return NaturalDiffIK::CalculateDiffIK(targetPose,
+ fwd.elbow,
+ arm.rns,
+ arm.tcp,
+ arm.elbow,
+ NaturalDiffIK::Mode::Position,
+ params.diffIKparams);
}
-NaturalDiffIK::Result NaturalIK::calculateIK(const Eigen::Matrix4f& targetPose, NaturalIK::ArmJoints arm, NaturalDiffIK::Mode setOri, NaturalIK::Parameters params)
+NaturalDiffIK::Result
+NaturalIK::calculateIK(const Eigen::Matrix4f& targetPose,
+ NaturalIK::ArmJoints arm,
+ NaturalDiffIK::Mode setOri,
+ NaturalIK::Parameters params)
{
Eigen::Vector3f targetPos = math::Helpers::Position(targetPose);
- NaturalIK::SoechtingForwardPositions fwd = solveSoechtingIK(targetPos, params.minimumElbowHeight);
+ NaturalIK::SoechtingForwardPositions fwd =
+ solveSoechtingIK(targetPos, params.minimumElbowHeight);
//VirtualRobot::IKSolver::CartesianSelection mode = setOri ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
- return NaturalDiffIK::CalculateDiffIK(targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, setOri, params.diffIKparams);
+ return NaturalDiffIK::CalculateDiffIK(
+ targetPose, fwd.elbow, arm.rns, arm.tcp, arm.elbow, setOri, params.diffIKparams);
}
-Eigen::Vector3f NaturalIK::getShoulderPos()
+Eigen::Vector3f
+NaturalIK::getShoulderPos()
{
return shoulderPos;
}
-
-NaturalIK::SoechtingAngles NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f target)
+NaturalIK::SoechtingAngles
+NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f target)
{
target = target - shoulderPos;
if (side == "Right")
@@ -150,9 +176,9 @@ NaturalIK::SoechtingAngles NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f t
// Angles derived from pointing in the dark
SoechtingAngles sa;
- sa.SE = -4.0 + 1.10 * R + 0.90 * Psi;
- sa.EE = 39.4 + 0.54 * R - 1.06 * Psi;
- sa.SY = 13.2 + 0.86 * Chi + 0.11 * Psi;
+ sa.SE = -4.0 + 1.10 * R + 0.90 * Psi;
+ sa.EE = 39.4 + 0.54 * R - 1.06 * Psi;
+ sa.SY = 13.2 + 0.86 * Chi + 0.11 * Psi;
sa.EY = -10.0 + 1.08 * Chi - 0.35 * Psi;
sa.SE = simox::math::deg_to_rad(sa.SE);
@@ -162,14 +188,15 @@ NaturalIK::SoechtingAngles NaturalIK::CalculateSoechtingAngles(Eigen::Vector3f t
if (side == "Left")
{
- sa.SY = - sa.SY;
- sa.EY = - sa.EY;
+ sa.SY = -sa.SY;
+ sa.EY = -sa.EY;
}
return sa;
}
-NaturalIK::SoechtingForwardPositions NaturalIK::forwardKinematics(SoechtingAngles sa)
+NaturalIK::SoechtingForwardPositions
+NaturalIK::forwardKinematics(SoechtingAngles sa)
{
Eigen::AngleAxisf aaSE(sa.SE, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf aaSY(-sa.SY, Eigen::Vector3f::UnitZ());
@@ -190,42 +217,52 @@ NaturalIK::SoechtingForwardPositions NaturalIK::forwardKinematics(SoechtingAngle
return res;
}
-void NaturalIK::setScale(float scale)
+void
+NaturalIK::setScale(float scale)
{
this->scale = scale;
}
-float NaturalIK::getScale()
+float
+NaturalIK::getScale()
{
return scale;
}
-float NaturalIK::getUpperArmLength() const
+float
+NaturalIK::getUpperArmLength() const
{
return upperArmLength;
}
-void NaturalIK::setUpperArmLength(float value)
+void
+NaturalIK::setUpperArmLength(float value)
{
upperArmLength = value;
}
-float NaturalIK::getLowerArmLength() const
+float
+NaturalIK::getLowerArmLength() const
{
return lowerArmLength;
}
-void NaturalIK::setLowerArmLength(float value)
+void
+NaturalIK::setLowerArmLength(float value)
{
lowerArmLength = value;
}
-NaturalIKProvider::NaturalIKProvider(const NaturalIK& natik, const NaturalIK::ArmJoints& arm, const NaturalDiffIK::Mode& setOri, const NaturalIK::Parameters& params)
- : natik(natik), arm(arm), setOri(setOri), params(params)
+NaturalIKProvider::NaturalIKProvider(const NaturalIK& natik,
+ const NaturalIK::ArmJoints& arm,
+ const NaturalDiffIK::Mode& setOri,
+ const NaturalIK::Parameters& params) :
+ natik(natik), arm(arm), setOri(setOri), params(params)
{
}
-DiffIKResult NaturalIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
+DiffIKResult
+NaturalIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
{
params.diffIKparams.resetRnsValues = true;
NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params);
@@ -237,7 +274,9 @@ DiffIKResult NaturalIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
return r;
}
-DiffIKResult NaturalIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues)
+DiffIKResult
+NaturalIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose,
+ const Eigen::VectorXf& startJointValues)
{
params.diffIKparams.resetRnsValues = false;
arm.rns->setJointValues(startJointValues);
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.h b/source/RobotAPI/libraries/natik/NaturalIK.h
index e71511d9bd4ffef7f64b6fc4c9a86d22d49eb07d..1abc5ebf5bb55091dad8deb096d20dfa74fd6301 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.h
+++ b/source/RobotAPI/libraries/natik/NaturalIK.h
@@ -27,11 +27,12 @@
//#include <RobotAPI/libraries/core/SimpleDiffIK.h>
+#include <optional>
+
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/libraries/diffik/DiffIKProvider.h>
#include <RobotAPI/libraries/diffik/NaturalDiffIK.h>
-#include <optional>
namespace armarx
{
@@ -53,11 +54,14 @@ namespace armarx
public:
struct Parameters
{
- Parameters() {}
+ Parameters()
+ {
+ }
+
NaturalDiffIK::Parameters diffIKparams;
std::optional<float> minimumElbowHeight = std::nullopt;
-
};
+
struct ArmJoints
{
VirtualRobot::RobotNodeSetPtr rns;
@@ -66,7 +70,6 @@ namespace armarx
VirtualRobot::RobotNodePtr tcp;
};
-
struct SoechtingAngles
{
float SE;
@@ -97,13 +100,25 @@ namespace armarx
};
public:
- NaturalIK(std::string side, Eigen::Vector3f shoulderPos = Eigen::Vector3f::Zero(), float scale = 1);
-
- NaturalIK::SoechtingForwardPositions solveSoechtingIK(const Eigen::Vector3f& targetPos, std::optional<float> minElbowHeight = std::nullopt);
- NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f& targetPose, ArmJoints arm, NaturalIK::Parameters params = NaturalIK::Parameters());
- NaturalDiffIK::Result calculateIKpos(const Eigen::Vector3f& targetPos, ArmJoints arm, NaturalIK::Parameters params = NaturalIK::Parameters());
-
- NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f& targetPose, ArmJoints arm, NaturalDiffIK::Mode setOri, NaturalIK::Parameters params = NaturalIK::Parameters());
+ NaturalIK(std::string side,
+ Eigen::Vector3f shoulderPos = Eigen::Vector3f::Zero(),
+ float scale = 1);
+
+ NaturalIK::SoechtingForwardPositions
+ solveSoechtingIK(const Eigen::Vector3f& targetPos,
+ std::optional<float> minElbowHeight = std::nullopt);
+ NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f& targetPose,
+ ArmJoints arm,
+ NaturalIK::Parameters params = NaturalIK::Parameters());
+ NaturalDiffIK::Result
+ calculateIKpos(const Eigen::Vector3f& targetPos,
+ ArmJoints arm,
+ NaturalIK::Parameters params = NaturalIK::Parameters());
+
+ NaturalDiffIK::Result calculateIK(const Eigen::Matrix4f& targetPose,
+ ArmJoints arm,
+ NaturalDiffIK::Mode setOri,
+ NaturalIK::Parameters params = NaturalIK::Parameters());
//static SimpleDiffIK::Result CalculateNaturalIK(const Eigen::Matrix4f targetPose, ArmJoints armjoints, Parameters params = Parameters());
@@ -141,13 +156,16 @@ namespace armarx
float lowerArmLength = SoechtingAngles::MMM_LOWER_ARM_LENGTH;
};
- class NaturalIKProvider
- : public DiffIKProvider
+ class NaturalIKProvider : public DiffIKProvider
{
public:
- NaturalIKProvider(const NaturalIK& natik, const NaturalIK::ArmJoints& arm, const NaturalDiffIK::Mode& setOri, const NaturalIK::Parameters& params = NaturalIK::Parameters());
+ NaturalIKProvider(const NaturalIK& natik,
+ const NaturalIK::ArmJoints& arm,
+ const NaturalDiffIK::Mode& setOri,
+ const NaturalIK::Parameters& params = NaturalIK::Parameters());
DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose);
- DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues);
+ DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose,
+ const Eigen::VectorXf& startJointValues);
private:
NaturalIK natik;
@@ -155,4 +173,4 @@ namespace armarx
NaturalDiffIK::Mode setOri;
NaturalIK::Parameters params;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/natik/natik.h b/source/RobotAPI/libraries/natik/natik.h
index 1abd32f967eabd56f9b823f20483c5a954a551a5..1b0c1c0d5f2129c985b8bc88310afca078b27c5d 100644
--- a/source/RobotAPI/libraries/natik/natik.h
+++ b/source/RobotAPI/libraries/natik/natik.h
@@ -22,7 +22,6 @@
#pragma once
-
namespace armarx
{
/**
@@ -39,7 +38,6 @@ namespace armarx
class natik
{
public:
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/natik/test/natikTest.cpp b/source/RobotAPI/libraries/natik/test/natikTest.cpp
index 2e02739fc2f35313f076669152ba2a6d11119087..19891b1e8a1801cc6525137a40ae2a0b3253684d 100644
--- a/source/RobotAPI/libraries/natik/test/natikTest.cpp
+++ b/source/RobotAPI/libraries/natik/test/natikTest.cpp
@@ -24,15 +24,18 @@
#define ARMARX_BOOST_TEST
-#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/Test.h>
-#include "../NaturalIK.h"
#include <iostream>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "../NaturalIK.h"
+
using namespace armarx;
-float calcAvgError(float upperArmLength, float lowerArmLength)
+float
+calcAvgError(float upperArmLength, float lowerArmLength)
{
float errSum = 0;
int count = 0;
@@ -106,4 +109,3 @@ BOOST_AUTO_TEST_CASE(testSoechtingAngles)
BOOST_CHECK_EQUAL(true, true);*/
}
-
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
index b8165c4ba086e82aec64611efac92f7da4fea376..9d1a9fba4bbf8caa72891e3691817d2738a1a389 100644
--- a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -32,7 +32,6 @@
#include <RobotAPI/components/ArViz/Client/elements/Mesh.h>
-
namespace armarx::obstacle_avoidance
{
diff --git a/source/RobotAPI/libraries/skills/core/Skill.cpp b/source/RobotAPI/libraries/skills/core/Skill.cpp
index 3a279a8b8e28bb2d7e01eb5ac5b17958c5b5ed51..08c9517c7a987b85faa124ea1b71811e14ce7a94 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.cpp
+++ b/source/RobotAPI/libraries/skills/core/Skill.cpp
@@ -1,4 +1,5 @@
#include "Skill.h"
+
#include <ArmarXCore/core/time/Metronome.h>
#include <RobotAPI/libraries/skills/core/error/Exception.h>
@@ -41,7 +42,6 @@ namespace armarx
return ret;
}
-
skills::SkillExecutionID
Skill::callSubskillAsync(const skills::SkillProxy& prx)
{
@@ -57,7 +57,11 @@ namespace armarx
auto eid = prx.executeSkillAsync(executorHistory, params);
std::unique_lock l(subskillsMutex);
- throwIfSkillShouldTerminate([&](){prx.abortSkillAsync(eid);}); // also notify newly added skill as it was not added to subskills list yet
+ throwIfSkillShouldTerminate(
+ [&]()
+ {
+ prx.abortSkillAsync(eid);
+ }); // also notify newly added skill as it was not added to subskills list yet
this->subskills.push_back(eid);
return eid;
}
diff --git a/source/RobotAPI/libraries/skills/core/Skill.h b/source/RobotAPI/libraries/skills/core/Skill.h
index 9be47862810009ec00fb8e9b8138b9ac1e0d5df0..3eb32b8889f39f1380507dd159a78cca484b51bc 100644
--- a/source/RobotAPI/libraries/skills/core/Skill.h
+++ b/source/RobotAPI/libraries/skills/core/Skill.h
@@ -10,7 +10,6 @@
#include <RobotAPI/libraries/skills/core/SkillProxy.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
-
namespace armarx
{
namespace skills
diff --git a/source/RobotAPI/libraries/skills/core/SkillID.h b/source/RobotAPI/libraries/skills/core/SkillID.h
index 1de4e5fc764d10bb5af2324f02ce4f71b81b60ef..698d939aa052581dd29c876bf67537fa4adacc81 100644
--- a/source/RobotAPI/libraries/skills/core/SkillID.h
+++ b/source/RobotAPI/libraries/skills/core/SkillID.h
@@ -1,7 +1,7 @@
#pragma once
-#include <string>
#include <optional>
+#include <string>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
diff --git a/source/RobotAPI/libraries/skills/core/SkillProxy.h b/source/RobotAPI/libraries/skills/core/SkillProxy.h
index d91dc2ae633cfdbf23c4d7938e455d442892910a..8ea51f557480b51708397fd14ef912834b40a971 100644
--- a/source/RobotAPI/libraries/skills/core/SkillProxy.h
+++ b/source/RobotAPI/libraries/skills/core/SkillProxy.h
@@ -1,6 +1,7 @@
#pragma once
#include <ArmarXCore/core/logging/Logging.h>
+
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
diff --git a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
index 2f57db3940b0d622650565951ccc04e8950d2277..da3914ecec4fdd9b70e654f4edf1298e38f59247 100644
--- a/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
+++ b/source/RobotAPI/libraries/skills/core/SkillStatusUpdate.cpp
@@ -369,12 +369,11 @@ namespace armarx
SkillStatusUpdate::FromIce(const provider::dto::SkillStatusUpdate& update,
const std::optional<skills::ProviderID>& providerId)
{
- SkillStatusUpdate ret{{
- .executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
- .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
- .callbackInterface = update.callbackInterface,
- .result = armarx::aron::data::Dict::FromAronDictDTO(update.result)
- }};
+ SkillStatusUpdate ret{
+ {.executionId = skills::SkillExecutionID::FromIce(update.executionId, providerId),
+ .parameters = armarx::aron::data::Dict::FromAronDictDTO(update.parameters),
+ .callbackInterface = update.callbackInterface,
+ .result = armarx::aron::data::Dict::FromAronDictDTO(update.result)}};
skills::fromIce(update.status, ret.status);
setResultFromIce(ret, update);
return ret;
diff --git a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
index 261037f624c2821d15d79560d24f127125740f8b..be6e9fefa6f684209e5c43fdf3250c0e3b250925 100644
--- a/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h
@@ -8,11 +8,11 @@
#include <ArmarXCore/core/ManagedIceObject.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
-#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include <RobotAPI/libraries/skills/core/ProviderID.h>
#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
namespace armarx
{
@@ -60,11 +60,16 @@ namespace armarx::plugins
skills::ProviderID getFirstProviderNameThatHasSkill(const skills::SkillID& skillid);
private:
- [[ noreturn ]] void handleExceptionNonLockingThrow(const char* funcName, const std::exception& e, skills::ProviderID providerId,
- bool eraseSkillProvider = true);
-
- std::optional<std::map<skills::ProviderID, skills::provider::dti::SkillProviderInterfacePrx>::iterator>
- handleExceptionNonLocking(const char* funcName, const std::exception& e, skills::ProviderID providerId,
+ [[noreturn]] void handleExceptionNonLockingThrow(const char* funcName,
+ const std::exception& e,
+ skills::ProviderID providerId,
+ bool eraseSkillProvider = true);
+
+ std::optional<std::map<skills::ProviderID,
+ skills::provider::dti::SkillProviderInterfacePrx>::iterator>
+ handleExceptionNonLocking(const char* funcName,
+ const std::exception& e,
+ skills::ProviderID providerId,
bool eraseSkillProvider = true);
skills::manager::dti::SkillManagerInterfacePrx myPrx;
diff --git a/source/RobotAPI/libraries/skills/provider/LambdaSkill.h b/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
index 16d39dd39578e25ecb901815e10311e7e57bab72..2b9bdf1b818c975db7e09204cea3ff7b5563f2b2 100644
--- a/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/LambdaSkill.h
@@ -12,7 +12,8 @@ namespace armarx
using FunctionType = std::function<TerminatedSkillStatus()>;
LambdaSkill() = delete;
- LambdaSkill(const SkillDescription& desc, const FunctionType& f) : Skill(desc), fun(f){};
+ LambdaSkill(const SkillDescription& desc, const FunctionType& f) :
+ Skill(desc), fun(f){};
private:
MainResult main() override;
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp
index 4c70437ab37e8fd79fcff3cc373d5b349e1cf9f5..b4e4cd694fc73c7f2562e5a32fdf983165dbec97 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.cpp
@@ -18,11 +18,11 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-
- #include "PeriodicSpecializedSkill.h"
-
- namespace armarx::skills
- {
-
-
- } // namespace armarx::skills
+
+#include "PeriodicSpecializedSkill.h"
+
+namespace armarx::skills
+{
+
+
+} // namespace armarx::skills
diff --git a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
index b707aee0d5224ab8588c4c95461ec22f5f300fa6..d5b4f89354f3ad2830572b4a974c0a065fb0c19a 100644
--- a/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h
@@ -24,8 +24,8 @@
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
-#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include "PeriodicSkill.h"
#include "SpecializedSkill.h"
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
index 68b1fb7e137bbf33045cc320100de697e7ab52e5..c7109d937f7e174b0c73fc668c05c0a3e7a709e8 100644
--- a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSkill.cpp
@@ -1,4 +1,5 @@
#include "SimplePeriodicSkill.h"
+
#include <ArmarXCore/core/time/Metronome.h>
#include <RobotAPI/libraries/skills/core/error/Exception.h>
diff --git a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
index 37d1e6084e81f99dd59973e9d22df022df509fb0..a13e4690a18c7482e0de4d8088b2b0fe53bf7841 100644
--- a/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
+++ b/source/RobotAPI/libraries/skills/provider/SimplePeriodicSpecializedSkill.h
@@ -1,7 +1,9 @@
#pragma once
#include <ArmarXCore/core/time/Metronome.h>
+
#include <RobotAPI/libraries/skills/core/error/Exception.h>
+
#include "PeriodicSkill.h"
#include "SimpleSpecializedSkill.h"
diff --git a/source/RobotAPI/libraries/skills/provider/SkillFactory.h b/source/RobotAPI/libraries/skills/provider/SkillFactory.h
index 1e3b40e3daf092e9214fc65846ec1c58b57e2b52..31b3c6d5a89c27cafa79127183a5c2fee5817546 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillFactory.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillFactory.h
@@ -26,7 +26,7 @@ namespace armarx
virtual ~SkillBlueprint() = default;
template <class _Skill, class... Args>
- requires isSkill<_Skill>
+ requires isSkill<_Skill>
static std::unique_ptr<SkillBlueprint>
ForSkill(Args&&... args)
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
index ac7ab61a80b89081778f571478dee3ed3fe3910d..4d43d5e18321ec4f884f74a9864af20b0d4a0d37 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.cpp
@@ -86,7 +86,7 @@ namespace armarx::plugins
ARMARX_INFO << "Adding skill `" << skillId << "` to component `" << componentName << "` .";
skillFactories.emplace(skillId, std::move(fac));
-
+
// if (connected)
// {
@@ -235,9 +235,11 @@ namespace armarx::plugins
ret.result = x.result;
ret.status = armarx::skills::toSkillStatus(x.status);
}
- catch(std::exception& e)
+ catch (std::exception& e)
{
- ARMARX_WARNING << "Got an uncatched Exception when executing a skill. Exception was: " << e.what();
+ ARMARX_WARNING
+ << "Got an uncatched Exception when executing a skill. Exception was: "
+ << e.what();
}
});
} // release lock. We don't know how long the skill needs to finish and we have to release the lock for being able to abort the execution
@@ -276,12 +278,13 @@ namespace armarx::plugins
const std::unique_lock l2{skillExecutionsMutex};
executionId = skills::SkillExecutionID{executionRequest.skillId,
- executionRequest.executorName,
- armarx::core::time::DateTime::Now()};
+ executionRequest.executorName,
+ armarx::core::time::DateTime::Now()};
if (skillExecutions.count(executionId) > 0)
{
- ARMARX_ERROR << "SkillsExecutionID already exists! This is undefined behaviour and should not occur!";
+ ARMARX_ERROR << "SkillsExecutionID already exists! This is undefined behaviour "
+ "and should not occur!";
}
auto it =
@@ -303,9 +306,11 @@ namespace armarx::plugins
// execute waits until the previous execution finishes.
auto x = wrapper->executeSkill();
}
- catch(std::exception& e)
+ catch (std::exception& e)
{
- ARMARX_WARNING << "Got an uncatched Exception when executing a skill. Exception was: " << e.what();
+ ARMARX_WARNING
+ << "Got an uncatched Exception when executing a skill. Exception was: "
+ << e.what();
}
});
}
@@ -320,7 +325,6 @@ namespace armarx::plugins
{
break;
}
-
}
std::this_thread::sleep_for(std::chrono::milliseconds(20));
diff --git a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
index 21e0d6d97ecb7841237b4ed495355531c38d1972..0136f150f97ba09b5d1142603a557377faed277f 100644
--- a/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
+++ b/source/RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h
@@ -9,13 +9,11 @@
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionRequest.h>
-#include "detail/SkillImplementationWrapper.h"
-
#include "LambdaSkill.h"
+#include "detail/SkillImplementationWrapper.h"
namespace armarx
{
@@ -45,7 +43,8 @@ namespace armarx::plugins
template <class SkillT, typename... Args>
- requires skills::isSkill<SkillT> skills::SkillBlueprint*
+ requires skills::isSkill<SkillT>
+ skills::SkillBlueprint*
addSkillFactory(Args&&... args)
{
auto fac = skills::SkillBlueprint::ForSkill<SkillT>(std::forward<Args>(args)...);
@@ -56,7 +55,8 @@ namespace armarx::plugins
template <typename FactoryT, typename... Args>
- requires skills::isSkillBlueprint<FactoryT> FactoryT*
+ requires skills::isSkillBlueprint<FactoryT>
+ FactoryT*
addCustomSkillFactory(Args&&... args)
{
auto fac = std::make_unique<FactoryT>(std::forward<Args>(args)...);
@@ -167,7 +167,8 @@ namespace armarx
template <class SkillT, typename... Args>
- requires skills::isSkill<SkillT> skills::SkillBlueprint*
+ requires skills::isSkill<SkillT>
+ skills::SkillBlueprint*
addSkillFactory(Args&&... args)
{
return plugin->addSkillFactory<SkillT>(std::forward<Args>(args)...);
@@ -175,7 +176,8 @@ namespace armarx
template <typename FacT, typename... Args>
- requires skills::isSkillBlueprint<FacT> FacT*
+ requires skills::isSkillBlueprint<FacT>
+ FacT*
addCustomSkillFactory(Args&&... args)
{
return plugin->addCustomSkillFactory<FacT>(std::forward<Args>(args)...);
diff --git a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.cpp b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.cpp
index c9a42436eadec5f4c9297830ea31de73e19446d2..624f91166e33a01fd8de7bb97ac7bd2b5408a63e 100644
--- a/source/RobotAPI/libraries/skills/provider/SpecializedSkill.cpp
+++ b/source/RobotAPI/libraries/skills/provider/SpecializedSkill.cpp
@@ -6,4 +6,4 @@ namespace armarx
{
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h b/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h
index 6a6802204822692867f0a9c5545359d4a0fc77bb..0e7210c47a5d9748e693c3eaf01af5745ec88823 100644
--- a/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h
+++ b/source/RobotAPI/libraries/skills/provider/blueprints/SkillWithContextBlueprint.h
@@ -30,10 +30,10 @@ namespace armarx
{
namespace skills
{
-// template <typename T>
-// concept takesContext = requires(typename T::Context cntxt) {
-// { T::connectTo(cntxt) } -> void;
-// };
+ // template <typename T>
+ // concept takesContext = requires(typename T::Context cntxt) {
+ // { T::connectTo(cntxt) } -> void;
+ // };
template <typename SkillT>
diff --git a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
index 35fc69ddd8a073f3b78ae008a67466dddc36ce68..a07e5a0b9c7df4d7535cfe7f451d3a7fa78ba8eb 100644
--- a/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
+++ b/source/RobotAPI/libraries/skills/provider/detail/SkillImplementationWrapper.cpp
@@ -1,10 +1,10 @@
#include "SkillImplementationWrapper.h"
+
#include <ArmarXCore/core/exceptions/LocalException.h>
-#include <RobotAPI/libraries/skills/core/error/Exception.h>
-#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/skills/core/aron/SkillErrorResult.aron.generated.h>
-
+#include <RobotAPI/libraries/skills/core/error/Exception.h>
namespace armarx
{
@@ -129,7 +129,9 @@ namespace armarx
// set initial parameters that were attached to the execution request (only add as we are not sure whether some updates already arrived)
this->updateSkillParameters(initial_aron_params);
- auto makeAbortedResult = [&](const std::string& errorCode, const aron::data::DictPtr& data, const std::string& message)
+ auto makeAbortedResult = [&](const std::string& errorCode,
+ const aron::data::DictPtr& data,
+ const std::string& message)
{
armarx::skills::arondto::SkillErrorResult errorResult;
errorResult.errorCode = errorCode;
@@ -149,7 +151,9 @@ namespace armarx
};
- auto makeFailedResult = [&](const std::string& errorCode, const aron::data::DictPtr& data, const std::string& message)
+ auto makeFailedResult = [&](const std::string& errorCode,
+ const aron::data::DictPtr& data,
+ const std::string& message)
{
armarx::skills::arondto::SkillErrorResult errorResult;
errorResult.errorCode = errorCode;
@@ -183,12 +187,16 @@ namespace armarx
// }
- auto exitAndMakeFailedResult = [&](const std::string& errorCode, const aron::data::DictPtr& data, const std::string& message)
+ auto exitAndMakeFailedResult = [&](const std::string& errorCode,
+ const aron::data::DictPtr& data,
+ const std::string& message)
{
skill->exitSkill(); // try to exit skill. Ignore return value
return makeFailedResult(errorCode, data, message);
};
- auto exitAndMakeAbortedResult = [&](const std::string& errorCode, const aron::data::DictPtr& data, const std::string& message)
+ auto exitAndMakeAbortedResult = [&](const std::string& errorCode,
+ const aron::data::DictPtr& data,
+ const std::string& message)
{
skill->exitSkill(); // try to exit skill. Ignore return value
return makeAbortedResult(errorCode, data, message);
@@ -314,7 +322,7 @@ namespace armarx
"SkillError 501e: An error occured during the main method of skill '" +
skillName + "'. The error was: " + GetHandledExceptionString();
ARMARX_ERROR_S << message;
- return exitAndMakeFailedResult("501e", mainRet.data, message);
+ return exitAndMakeFailedResult("501e", mainRet.data, message);
}
// Main succeeded!
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/All.h b/source/RobotAPI/libraries/skills/provider/mixins/All.h
index 7f74ab14ff3d6f438c4343996852fb3065841c68..d81537a3732fb1a76ed48fffd621a18a3e54cc05 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/All.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/All.h
@@ -1,6 +1,6 @@
#include "ArvizSkillMixin.h"
-#include "MNSSkillMixin.h"
#include "GraspReadingSkillMixin.h"
+#include "MNSSkillMixin.h"
#include "MemoryReadingSkillMixin.h"
#include "ObjectReadingSkillMixin.h"
#include "ObjectWritingSkillMixin.h"
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/ArvizSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/ArvizSkillMixin.h
index 7b77cb543cb1c853f565cc4fc51d06181cca61ab..8f99b65a107140998d5bd58a4f2b872c6cc11980 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/ArvizSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/ArvizSkillMixin.h
@@ -12,15 +12,15 @@ namespace armarx::skills::mixin
std::string layerName;
ArvizSkillMixin(const armarx::viz::Client& a, const std::string& ln) :
- arviz(a),
- layerName(ln)
+ arviz(a), layerName(ln)
{
}
- void clearLayer()
+ void
+ clearLayer()
{
auto l = arviz.layer(layerName);
arviz.commit(l);
}
};
-}
+} // namespace armarx::skills::mixin
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h
index abcfeb2d285a93254dfe1c029d7e7f1763709070..74ee35b70067d3e563309ca84d4176191306dc7f 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/GraspReadingSkillMixin.h
@@ -12,7 +12,6 @@ namespace armarx::skills::mixin
GraspReadingSkillMixin(const armem::grasping::known_grasps::Reader& r) : graspReader(r)
{
-
}
};
@@ -21,7 +20,10 @@ namespace armarx::skills::mixin
std::string objectEntityId;
armem::grasping::known_grasps::Reader graspReader;
- SpecificGraspReadingSkillMixin(const std::string& n, const armem::grasping::known_grasps::Reader& r) : objectEntityId(n), graspReader(r)
- {}
+ SpecificGraspReadingSkillMixin(const std::string& n,
+ const armem::grasping::known_grasps::Reader& r) :
+ objectEntityId(n), graspReader(r)
+ {
+ }
};
-}
+} // namespace armarx::skills::mixin
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/MNSSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/MNSSkillMixin.h
index f0bde8bdf665d23f3bbe59d060d74f010fd29d3f..d04030f5843dd242b552732b3230788a252cb50d 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/MNSSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/MNSSkillMixin.h
@@ -11,6 +11,7 @@ namespace armarx::skills::mixin
armem::client::MemoryNameSystem mns;
MNSSkillMixin(const armem::client::MemoryNameSystem& m) : mns(m)
- {}
+ {
+ }
};
-}
+} // namespace armarx::skills::mixin
diff --git a/source/RobotAPI/libraries/skills/provider/mixins/RobotWritingSkillMixin.h b/source/RobotAPI/libraries/skills/provider/mixins/RobotWritingSkillMixin.h
index 34856452c4b8bd3b4e61dd69ee03eb122d409566..7dc7890a979cd63a6e3b9cfcfa7d0677c25ddf78 100644
--- a/source/RobotAPI/libraries/skills/provider/mixins/RobotWritingSkillMixin.h
+++ b/source/RobotAPI/libraries/skills/provider/mixins/RobotWritingSkillMixin.h
@@ -10,9 +10,9 @@ namespace armarx::skills::mixin
{
armem::robot_state::VirtualRobotWriter robotWriter;
- RobotWritingSkillMixin(armem::client::MemoryNameSystem& mns) :
- robotWriter(mns)
- {}
+ RobotWritingSkillMixin(armem::client::MemoryNameSystem& mns) : robotWriter(mns)
+ {
+ }
};
struct SpecificRobotWritingSkillMixin
@@ -20,9 +20,10 @@ namespace armarx::skills::mixin
std::string robotName;
armem::robot_state::VirtualRobotWriter robotWriter;
- SpecificRobotWritingSkillMixin(const std::string& rn, armem::client::MemoryNameSystem& mns) :
- robotName(rn),
- robotWriter(mns)
- {}
+ SpecificRobotWritingSkillMixin(const std::string& rn,
+ armem::client::MemoryNameSystem& mns) :
+ robotName(rn), robotWriter(mns)
+ {
+ }
};
-}
+} // namespace armarx::skills::mixin
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/AronTreeWidgetController.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/AronTreeWidgetController.cpp
index 8c24f906fef8edea5f33db3b4a890431f5bc7bb4..2c877a7f5c52a732d991db7d6e2866db46f9064d 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/AronTreeWidgetController.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/AronTreeWidgetController.cpp
@@ -136,7 +136,7 @@ namespace armarx::skills::gui
void
AronTreeWidgetController::onTreeWidgetItemChanged(QTreeWidgetItem* item, int column)
{
-
+
tree->blockSignals(true);
auto* aronElem = AronTreeWidgetItem::DynamicCast(item);
@@ -160,7 +160,6 @@ namespace armarx::skills::gui
// else perhaps the GUI was stopped or died.
tree->blockSignals(false);
-
}
} // namespace armarx::skills::gui
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/AronTreeWidgetItem.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/AronTreeWidgetItem.cpp
index 5f24c40c959e8008457c096b4d872e8407041ab8..acbd6b8768f0aa392b57ea77b23c0878f042966d 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/AronTreeWidgetItem.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/AronTreeWidgetItem.cpp
@@ -149,7 +149,7 @@ namespace armarx::skills::gui
void
AronTreeWidgetItem::onUserChange(int changedColumn)
{
-
+
QTreeWidgetItem* qParent = QTreeWidgetItem::parent();
ARMARX_CHECK(qParent);
AronTreeWidgetItem* aronParent = DynamicCast(qParent);
@@ -173,7 +173,6 @@ namespace armarx::skills::gui
preventIllegalKeyChange();
}
}
-
}
void
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetConverter.cpp
index 164e4498f0adcc2ca99bfdacbfcb36cf441a8879..c7bc9f77a900c73202208d0e6631c1fbeca66054 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetConverter.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetConverter.cpp
@@ -75,19 +75,18 @@ namespace armarx::skills::gui
void
AronTreeWidgetConverterVisitor::handleErrors(bool ownFault)
{
-
+
ARMARX_TRACE;
isDirectError = ownFault;
auto* aronItem = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
ARMARX_CHECK(aronItem);
aronItem->setValueErrorState(isDirectError, false);
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ObjectPtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronDict = std::make_shared<aron::data::Dict>(i->getPath());
createdAron = createdAronDict;
@@ -116,13 +115,12 @@ namespace armarx::skills::gui
createdAronDict->addElement(key, v.createdAron);
}
}
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::DictPtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronDict = std::make_shared<aron::data::Dict>(i->getPath());
createdAron = createdAronDict;
@@ -151,13 +149,12 @@ namespace armarx::skills::gui
createdAronDict->addElement(key, v.createdAron);
}
}
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::ListPtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronList = std::make_shared<aron::data::List>(i->getPath());
createdAron = createdAronList;
@@ -186,13 +183,12 @@ namespace armarx::skills::gui
createdAronList->addElement(convVisitor.createdAron);
}
}
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::PairPtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronPair = std::make_shared<aron::data::List>(i->getPath());
createdAron = createdAronPair;
@@ -217,13 +213,12 @@ namespace armarx::skills::gui
createdAronPair->addElement(convVisitor.createdAron);
}
}
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::TuplePtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronList = std::make_shared<aron::data::List>(i->getPath());
createdAron = createdAronList;
@@ -251,7 +246,6 @@ namespace armarx::skills::gui
createdAronList->addElement(v.createdAron);
}
}
-
}
void
@@ -264,7 +258,7 @@ namespace armarx::skills::gui
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::MatrixPtr& i)
{
-
+
ARMARX_TRACE;
auto createdMatrix = std::make_shared<aron::data::NDArray>(i->getPath());
int dataSize = 0;
@@ -289,7 +283,7 @@ namespace armarx::skills::gui
break;
};
-
+
// UGLY HACK: FIX ME!!!
switch (i->getElementType())
{
@@ -322,7 +316,7 @@ namespace armarx::skills::gui
break;
};
-
+
createdMatrix->setShape({i->getRows(), i->getCols(), dataSize});
int totalByteSize = i->getRows() * i->getCols() * dataSize;
createdAron = createdMatrix;
@@ -338,7 +332,7 @@ namespace armarx::skills::gui
}
}
-
+
auto* rootWidget = el->treeWidget();
ARMARX_CHECK(rootWidget);
auto* widget = rootWidget->itemWidget(el, 1);
@@ -352,7 +346,7 @@ namespace armarx::skills::gui
return;
}
-
+
// write to aron data
std::vector<unsigned char> elems;
elems.reserve(totalByteSize);
@@ -368,13 +362,12 @@ namespace armarx::skills::gui
}
}
createdMatrix->setData(totalByteSize, elems.data());
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::QuaternionPtr& i)
{
-
+
ARMARX_TRACE;
auto createdQuat = std::make_shared<aron::data::NDArray>(i->getPath());
createdAron = createdQuat;
@@ -411,7 +404,6 @@ namespace armarx::skills::gui
<< "serialized quaternions did not return byte sequence of correct length!";
}
createdQuat->setData(serialized.size(), serialized.data());
-
}
void
@@ -431,7 +423,7 @@ namespace armarx::skills::gui
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::IntEnumPtr& i)
{
-
+
ARMARX_TRACE;
QTreeWidgetItem* el = parentItem->child(index);
@@ -456,13 +448,12 @@ namespace armarx::skills::gui
std::tie(success, createdAron) = intEnumWidget->parseToAron();
handleErrors(!success);
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::IntPtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronInt = std::make_shared<aron::data::Int>(i->getPath());
createdAron = createdAronInt;
@@ -497,13 +488,12 @@ namespace armarx::skills::gui
return;
}
handleErrors(false);
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::LongPtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronLong = std::make_shared<aron::data::Long>(i->getPath());
createdAron = createdAronLong;
@@ -537,13 +527,12 @@ namespace armarx::skills::gui
return;
}
handleErrors(false);
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::FloatPtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronFloat = std::make_shared<aron::data::Float>(i->getPath());
createdAron = createdAronFloat;
@@ -576,13 +565,12 @@ namespace armarx::skills::gui
return;
}
handleErrors(false);
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::DoublePtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronDouble = std::make_shared<aron::data::Double>(i->getPath());
createdAron = createdAronDouble;
@@ -615,13 +603,12 @@ namespace armarx::skills::gui
return;
}
handleErrors(false);
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::BoolPtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronBool = std::make_shared<aron::data::Bool>(i->getPath());
createdAron = createdAronBool;
@@ -654,13 +641,12 @@ namespace armarx::skills::gui
return;
}
handleErrors(false);
-
}
void
AronTreeWidgetConverterVisitor::visitAronVariant(const aron::type::StringPtr& i)
{
-
+
ARMARX_TRACE;
auto createdAronString = std::make_shared<aron::data::String>(i->getPath());
createdAron = createdAronString;
@@ -683,7 +669,6 @@ namespace armarx::skills::gui
std::string str = el->text(1).toStdString();
createdAronString->setValue(str);
-
}
void
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetSetter.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetSetter.cpp
index a9f7e4db55e3e90abcd02bf29e4d4e8c3527b4f6..c092904db448efa530951611d13143ea85e59f5c 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetSetter.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/visitors/AronTreeWidgetSetter.cpp
@@ -35,12 +35,12 @@ template <typename T>
std::string
usString(T number, size_t precision = 3)
{
-
+
std::stringstream ss;
const char* locale = "C";
ss.imbue(std::locale(locale));
ss << std::fixed << std::setprecision(precision) << number;
-
+
return ss.str();
}
@@ -103,7 +103,7 @@ namespace armarx::skills::gui
void
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DictPtr& i)
{
-
+
// either it is the root or it has a name
if (i->getPath().size() == 0 ||
checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
@@ -128,13 +128,12 @@ namespace armarx::skills::gui
el->setCheckState(2, Qt::CheckState::Checked);
}
}
-
}
void
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::ListPtr& i)
{
-
+
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
@@ -165,7 +164,6 @@ namespace armarx::skills::gui
el->setCheckState(2, Qt::CheckState::Checked);
}
}
-
}
void
@@ -173,7 +171,7 @@ namespace armarx::skills::gui
const std::shared_ptr<armarx::aron::type::Matrix>& matrixType,
const aron::data::NDArrayPtr& arr)
{
-
+
auto elemType = matrixType->getElementType();
auto* rawData = arr->getData();
// string can convert any item
@@ -256,7 +254,7 @@ namespace armarx::skills::gui
std::shared_ptr<armarx::aron::type::Quaternion>& quatType,
const aron::data::NDArrayPtr& arr)
{
-
+
auto elemType = quatType->getElementType();
auto rawData = arr->getData();
auto shape = arr->getShape();
@@ -287,13 +285,12 @@ namespace armarx::skills::gui
{
quatWidget->setText((QuaternionWidget::QuaternionComponents)i, toString(i));
}
-
}
void
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::NDArrayPtr& arr)
{
-
+
// Matrices are handled as NDArray. Raw ndarrays cannot be created currently
auto* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
ARMARX_CHECK(el);
@@ -324,13 +321,12 @@ namespace armarx::skills::gui
{
el->setCheckState(2, Qt::CheckState::Checked);
}
-
}
void
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::IntPtr& i)
{
-
+
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
@@ -352,13 +348,12 @@ namespace armarx::skills::gui
el->setCheckState(2, Qt::CheckState::Checked);
}
}
-
}
void
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::LongPtr& i)
{
-
+
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
@@ -369,7 +364,6 @@ namespace armarx::skills::gui
el->setCheckState(2, Qt::CheckState::Checked);
}
}
-
}
void
@@ -386,13 +380,12 @@ namespace armarx::skills::gui
el->setCheckState(2, Qt::CheckState::Checked);
}
}
-
}
void
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::DoublePtr& i)
{
-
+
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
@@ -403,13 +396,12 @@ namespace armarx::skills::gui
el->setCheckState(2, Qt::CheckState::Checked);
}
}
-
}
void
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::BoolPtr& i)
{
-
+
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
@@ -420,13 +412,12 @@ namespace armarx::skills::gui
el->setCheckState(2, Qt::CheckState::Checked);
}
}
-
}
void
AronTreeWidgetSetterVisitor::visitAronVariant(const aron::data::StringPtr& i)
{
-
+
if (checkTreeWidgetItemForSimilarName(i->getPath().getLastElement()))
{
AronTreeWidgetItem* el = AronTreeWidgetItem::DynamicCast(parentItem->child(index));
@@ -437,7 +428,6 @@ namespace armarx::skills::gui
el->setCheckState(2, Qt::CheckState::Checked);
}
}
-
}
void
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.h b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.h
index e179a85936904abd6ceb96749663252ef939767e..313026bbed2b850a59f64ff293397ed89eafd287 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.h
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/EditMatrixWidget.h
@@ -6,6 +6,7 @@
#include <QObject>
#include <QTreeWidgetItem>
#include <QVBoxLayout>
+
#include <SimoxUtility/algorithm/string/string_conversion.h>
#include "RobotAPI/libraries/aron/core/type/variant/ndarray/Matrix.h"
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.cpp b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.cpp
index f6d123b7621f051eb21e39dd8d01965e84e66636..e09fbe6e02eaba85e642f7caf031217c97bfd382 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.cpp
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.cpp
@@ -4,11 +4,11 @@
#include <QLabel>
#include <QLineEdit>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include "../ColorPalettes.h"
#include "NDArrayHelper.h"
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
namespace armarx::skills::gui
{
diff --git a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.h b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.h
index c256f0a224acd64f4824f6b46cfcb77cc97d1880..09bcacad9d79b8980c5944744c63d61c957938ef 100644
--- a/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.h
+++ b/source/RobotAPI/libraries/skills_gui/aron_tree_widget/widgets/QuaternionWidget.h
@@ -6,8 +6,10 @@
#include <QLineEdit>
#include <QObject>
#include <QVBoxLayout>
+
#include <SimoxUtility/algorithm/string/string_conversion.h>
#include <SimoxUtility/error/SimoxError.h>
+
#include <RobotAPI/interface/aron/Aron.h>
#include "CustomWidget.h"
diff --git a/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp b/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp
index 87a254728e4479584bc23d6b66959162363c2c24..e0d3ee5609a76f7ffcda68b65e09750f4581e5f9 100644
--- a/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp
+++ b/source/RobotAPI/libraries/skills_gui/executions/SkillExecutionTreeWidget.cpp
@@ -4,6 +4,7 @@
#include <QMenu>
#include <QTreeWidgetItem>
+
#include <ArmarXCore/core/logging/Logging.h>
#include "SkillExecutionTreeWidgetItem.h"
diff --git a/source/RobotAPI/libraries/skills_gui/gui_utils.h b/source/RobotAPI/libraries/skills_gui/gui_utils.h
index 3901038ba99f7b59494df40210af0da867ba6ff3..15a961f42679299198caae68721099b31665f74f 100644
--- a/source/RobotAPI/libraries/skills_gui/gui_utils.h
+++ b/source/RobotAPI/libraries/skills_gui/gui_utils.h
@@ -1,6 +1,7 @@
#pragma once
#include <iostream>
+#include <mutex>
#include <QLayout>
#include <QLayoutItem>
@@ -9,8 +10,6 @@
#include <QTreeWidgetItem>
#include <QWidget>
-#include <mutex>
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx::gui
diff --git a/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp b/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
index cc72d4565872bf8367c67cdc89ddf9af8381b4df..ffdc422954d9f60e8cc089e462ed4d7a04616ee0 100644
--- a/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
+++ b/source/RobotAPI/libraries/skills_gui/memory/SkillManagerWrapper.cpp
@@ -1,7 +1,9 @@
#include "SkillManagerWrapper.h"
#include <mutex>
+
#include <SimoxUtility/algorithm/string/string_tools.h>
+
#include <ArmarXCore/core/logging/Logging.h>
#include "RobotAPI/libraries/skills/core/SkillExecutionRequest.h"
@@ -28,7 +30,8 @@ namespace armarx::skills::gui
// we return this map
StatusMap statusMap;
- auto currentManagerStatuses = memory->ice_invocationTimeout(10000)->getSkillExecutionStatuses();
+ auto currentManagerStatuses =
+ memory->ice_invocationTimeout(10000)->getSkillExecutionStatuses();
// iterate over raw data and convert to common types
for (const auto& [k, v] : currentManagerStatuses)
@@ -299,7 +302,8 @@ namespace armarx::skills::gui
{
ARMARX_INFO << "Aborting skill '" << executionId.skillId.skillName << "'...";
std::scoped_lock l(mutex_memory);
- this->memory->ice_invocationTimeout(5000)->abortSkillAsync(executionId.toManagerIce());
+ this->memory->ice_invocationTimeout(5000)->abortSkillAsync(
+ executionId.toManagerIce());
}
catch (Ice::Exception const& e)
{
diff --git a/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidgetItem.h b/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidgetItem.h
index 2a2ae418e4f546027753a4faf62daf7693ee0f7a..5d3ffb9c5c1624bc933d0902af74ee13fe9601bb 100644
--- a/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidgetItem.h
+++ b/source/RobotAPI/libraries/skills_gui/skills/SkillTreeWidgetItem.h
@@ -1,9 +1,8 @@
#pragma once
#include <QTreeWidgetItem>
-#include "RobotAPI/libraries/skills/core/SkillID.h"
-
+#include "RobotAPI/libraries/skills/core/SkillID.h"
namespace armarx::skills::gui
{
@@ -22,5 +21,4 @@ namespace armarx::skills::gui
skills::SkillID skillId;
};
-}
-
+} // namespace armarx::skills::gui
diff --git a/source/RobotAPI/libraries/ukfm/SystemModel.cpp b/source/RobotAPI/libraries/ukfm/SystemModel.cpp
index a67507af4f13491407eae3c2582f3f97730ba082..96f9a955bb4d37c152bb07139707cd4f6fa45eb0 100644
--- a/source/RobotAPI/libraries/ukfm/SystemModel.cpp
+++ b/source/RobotAPI/libraries/ukfm/SystemModel.cpp
@@ -26,40 +26,41 @@
*/
#include "SystemModel.h"
-#include <manif/SO3.h>
+#include <manif/SO3.h>
-template<typename floatT>
-typename SystemModelSE3<floatT>::ObsT SystemModelSE3<floatT>::observationFunction(const StateT &state)
+template <typename floatT>
+typename SystemModelSE3<floatT>::ObsT
+SystemModelSE3<floatT>::observationFunction(const StateT& state)
{
ObsT observation = state.pose.log().coeffs();
return observation;
}
-template<typename floatT>
+template <typename floatT>
typename SystemModelSE3<floatT>::StateT
-SystemModelSE3<floatT>::propagationFunction(const StateT &state, const ControlT &control,
- const ControlNoiseT &noise, FloatT dt)
+SystemModelSE3<floatT>::propagationFunction(const StateT& state,
+ const ControlT& control,
+ const ControlNoiseT& noise,
+ FloatT dt)
{
StateT new_state;
new_state.pose = state.pose.template rplus(control.velocity * dt);
return new_state;
}
-
-template<typename floatT>
+template <typename floatT>
typename SystemModelSE3<floatT>::SigmaPointsT
-SystemModelSE3<floatT>::inverseRetraction(const StateT &state1, const StateT &state2)
+SystemModelSE3<floatT>::inverseRetraction(const StateT& state1, const StateT& state2)
{
SigmaPointsT sigma;
sigma = state2.pose.lminus(state1.pose).coeffs();
return sigma;
}
-
-template<typename floatT>
+template <typename floatT>
typename SystemModelSE3<floatT>::StateT
-SystemModelSE3<floatT>::retraction(const StateT &state, const SigmaPointsT &sigmaPoints)
+SystemModelSE3<floatT>::retraction(const StateT& state, const SigmaPointsT& sigmaPoints)
{
StateT new_state;
manif::SE3Tangent<FloatT> tan;
@@ -72,36 +73,40 @@ SystemModelSE3<floatT>::retraction(const StateT &state, const SigmaPointsT &sigm
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename floatT>
-typename SystemModelSE3xV<floatT>::ObsT SystemModelSE3xV<floatT>::observationFunction(const SystemModelSE3xV::StateT &state)
+template <typename floatT>
+typename SystemModelSE3xV<floatT>::ObsT
+SystemModelSE3xV<floatT>::observationFunction(const SystemModelSE3xV::StateT& state)
{
ObsT observation = state.pose.log().coeffs();
return observation;
}
-template<typename floatT>
-typename SystemModelSE3xV<floatT>::StateT SystemModelSE3xV<floatT>::propagationFunction(const SystemModelSE3xV::StateT &state,
- const SystemModelSE3xV::ControlT &control,
- const SystemModelSE3xV::ControlNoiseT &noise,
- FloatT dt)
+template <typename floatT>
+typename SystemModelSE3xV<floatT>::StateT
+SystemModelSE3xV<floatT>::propagationFunction(const SystemModelSE3xV::StateT& state,
+ const SystemModelSE3xV::ControlT& control,
+ const SystemModelSE3xV::ControlNoiseT& noise,
+ FloatT dt)
{
// transform acceleration into global frame
// TODO: figure out what to do here; probably something with adjoint
-// Eigen::Matrix<FloatT, 3, 1> local_acc = control.acceleration.segment(0, 3) + noise.template block<3, 1>(3, 0);
-// Eigen::Matrix<FloatT, 3, 1> acc = state.pose.asSO3().template act(local_acc);
+ // Eigen::Matrix<FloatT, 3, 1> local_acc = control.acceleration.segment(0, 3) + noise.template block<3, 1>(3, 0);
+ // Eigen::Matrix<FloatT, 3, 1> acc = state.pose.asSO3().template act(local_acc);
StateT new_state;
-// manif::SO3<FloatT> rotation = state.pose.asSO3().lplus(
-// manif::SO3Tangent<FloatT>(control.acceleration.segment(3, 3) * dt + noise.segment(0, 3)));
-// Eigen::Matrix<FloatT, 3, 1> position = state.pose.translation() + state.velocity * dt + 0.5 * control * dt * dt;
- new_state.pose = state.pose.template lplus(state.velocity * dt + 0.5 * control.acceleration * dt * dt);
+ // manif::SO3<FloatT> rotation = state.pose.asSO3().lplus(
+ // manif::SO3Tangent<FloatT>(control.acceleration.segment(3, 3) * dt + noise.segment(0, 3)));
+ // Eigen::Matrix<FloatT, 3, 1> position = state.pose.translation() + state.velocity * dt + 0.5 * control * dt * dt;
+ new_state.pose =
+ state.pose.template lplus(state.velocity * dt + 0.5 * control.acceleration * dt * dt);
new_state.velocity = state.velocity + control.acceleration * dt;
return new_state;
}
-template<typename floatT>
-typename SystemModelSE3xV<floatT>::StateT SystemModelSE3xV<floatT>::retraction(const SystemModelSE3xV::StateT &state,
- const SystemModelSE3xV::SigmaPointsT &sigmaPoints)
+template <typename floatT>
+typename SystemModelSE3xV<floatT>::StateT
+SystemModelSE3xV<floatT>::retraction(const SystemModelSE3xV::StateT& state,
+ const SystemModelSE3xV::SigmaPointsT& sigmaPoints)
{
StateT new_state;
manif::SE3Tangent<FloatT> tan;
@@ -113,9 +118,10 @@ typename SystemModelSE3xV<floatT>::StateT SystemModelSE3xV<floatT>::retraction(c
return new_state;
}
-template<typename floatT>
-typename SystemModelSE3xV<floatT>::SigmaPointsT SystemModelSE3xV<floatT>::inverseRetraction(const SystemModelSE3xV::StateT &state1,
- const SystemModelSE3xV::StateT &state2)
+template <typename floatT>
+typename SystemModelSE3xV<floatT>::SigmaPointsT
+SystemModelSE3xV<floatT>::inverseRetraction(const SystemModelSE3xV::StateT& state1,
+ const SystemModelSE3xV::StateT& state2)
{
SigmaPointsT sigma;
sigma.segment(0, 6) = state2.pose.lminus(state1.pose).coeffs();
@@ -128,8 +134,9 @@ typename SystemModelSE3xV<floatT>::SigmaPointsT SystemModelSE3xV<floatT>::invers
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-template<typename floatT>
-typename SystemModelSO3xR3<floatT>::ObsT SystemModelSO3xR3<floatT>::observationFunction(const SystemModelSO3xR3::StateT& state)
+template <typename floatT>
+typename SystemModelSO3xR3<floatT>::ObsT
+SystemModelSO3xR3<floatT>::observationFunction(const SystemModelSO3xR3::StateT& state)
{
ObsT observation;
observation.segment(0, 3) = state.position;
@@ -137,11 +144,12 @@ typename SystemModelSO3xR3<floatT>::ObsT SystemModelSO3xR3<floatT>::observationF
return observation;
}
-template<typename floatT>
-typename SystemModelSO3xR3<floatT>::StateT SystemModelSO3xR3<floatT>::propagationFunction(const SystemModelSO3xR3::StateT& state,
- const SystemModelSO3xR3::ControlT& control,
- const SystemModelSO3xR3::ControlNoiseT& noise,
- FloatT dt)
+template <typename floatT>
+typename SystemModelSO3xR3<floatT>::StateT
+SystemModelSO3xR3<floatT>::propagationFunction(const SystemModelSO3xR3::StateT& state,
+ const SystemModelSO3xR3::ControlT& control,
+ const SystemModelSO3xR3::ControlNoiseT& noise,
+ FloatT dt)
{
StateT new_state;
new_state.orientation = state.orientation.template rplus(control.angular_velocity * dt);
@@ -149,9 +157,10 @@ typename SystemModelSO3xR3<floatT>::StateT SystemModelSO3xR3<floatT>::propagatio
return new_state;
}
-template<typename floatT>
-typename SystemModelSO3xR3<floatT>::StateT SystemModelSO3xR3<floatT>::retraction(const SystemModelSO3xR3::StateT& state,
- const SystemModelSO3xR3::SigmaPointsT& sigmaPoints)
+template <typename floatT>
+typename SystemModelSO3xR3<floatT>::StateT
+SystemModelSO3xR3<floatT>::retraction(const SystemModelSO3xR3::StateT& state,
+ const SystemModelSO3xR3::SigmaPointsT& sigmaPoints)
{
StateT new_state;
manif::SO3Tangent<FloatT> tan;
@@ -162,9 +171,10 @@ typename SystemModelSO3xR3<floatT>::StateT SystemModelSO3xR3<floatT>::retraction
return new_state;
}
-template<typename floatT>
-typename SystemModelSO3xR3<floatT>::SigmaPointsT SystemModelSO3xR3<floatT>::inverseRetraction(const SystemModelSO3xR3::StateT& state1,
- const SystemModelSO3xR3::StateT& state2)
+template <typename floatT>
+typename SystemModelSO3xR3<floatT>::SigmaPointsT
+SystemModelSO3xR3<floatT>::inverseRetraction(const SystemModelSO3xR3::StateT& state1,
+ const SystemModelSO3xR3::StateT& state2)
{
SigmaPointsT sigma;
// TODO: check if right order of substraction
@@ -174,17 +184,11 @@ typename SystemModelSO3xR3<floatT>::SigmaPointsT SystemModelSO3xR3<floatT>::inve
}
-template
-struct SystemModelSE3<float>;
-template
-struct SystemModelSE3<double>;
+template struct SystemModelSE3<float>;
+template struct SystemModelSE3<double>;
-template
-struct SystemModelSE3xV<float>;
-template
-struct SystemModelSE3xV<double>;
+template struct SystemModelSE3xV<float>;
+template struct SystemModelSE3xV<double>;
-template
-struct SystemModelSO3xR3<float>;
-template
-struct SystemModelSO3xR3<double>;
+template struct SystemModelSO3xR3<float>;
+template struct SystemModelSO3xR3<double>;
diff --git a/source/RobotAPI/libraries/ukfm/SystemModel.h b/source/RobotAPI/libraries/ukfm/SystemModel.h
index 3e4d1dd97802aaef235ead0032570b6ad28059bf..eff9e32f7e3ce0ffc51e7e4947c50d480214cbd6 100644
--- a/source/RobotAPI/libraries/ukfm/SystemModel.h
+++ b/source/RobotAPI/libraries/ukfm/SystemModel.h
@@ -26,7 +26,6 @@
*/
-
#ifndef ROBDEKON_SYSTEMMODEL_H
#define ROBDEKON_SYSTEMMODEL_H
@@ -37,44 +36,51 @@
#include <manif/SO3.h>
template <typename floatT>
-struct StateSE3 {
+struct StateSE3
+{
manif::SE3<floatT> pose;
};
template <typename floatT>
-struct ControlSE3 {
- typename manif::SE3<floatT>::Tangent velocity;
+struct ControlSE3
+{
+ typename manif::SE3<floatT>::Tangent velocity;
};
template <typename floatT>
-struct StateSE3xV {
+struct StateSE3xV
+{
manif::SE3<floatT> pose;
typename manif::SE3<floatT>::Tangent velocity;
};
template <typename floatT>
-struct StateSO3xR3 {
+struct StateSO3xR3
+{
manif::SO3<floatT> orientation;
Eigen::Matrix<floatT, 3, 1> position;
};
template <typename floatT>
-struct ControlSO3xR3 {
+struct ControlSO3xR3
+{
typename manif::SO3<floatT>::Tangent angular_velocity;
Eigen::Matrix<floatT, 3, 1> euclidean_velocity;
};
-
template <typename floatT>
-struct ControlSE3xV {
+struct ControlSE3xV
+{
Eigen::Matrix<floatT, 6, 1> acceleration;
};
-template<typename floatT>
+template <typename floatT>
struct SystemModelSE3
{
static_assert(std::is_floating_point_v<floatT>);
- struct dim {
+
+ struct dim
+ {
static constexpr long state = 6, control = 6, obs = 6;
};
@@ -82,24 +88,29 @@ struct SystemModelSE3
using StateT = StateSE3<FloatT>;
using ControlT = ControlSE3<FloatT>;
using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
- using ControlNoiseT = Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using ControlNoiseT =
+ Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
static ObsT observationFunction(const StateT& state);
- static StateT propagationFunction(const StateT& state, const ControlT& control, const ControlNoiseT& noise, FloatT dt);
+ static StateT propagationFunction(const StateT& state,
+ const ControlT& control,
+ const ControlNoiseT& noise,
+ FloatT dt);
static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
-
};
-template<typename floatT>
+template <typename floatT>
struct SystemModelSO3xR3
{
static_assert(std::is_floating_point_v<floatT>);
- struct dim {
+
+ struct dim
+ {
static constexpr long state = 6, control = 6, obs = 6;
};
@@ -107,24 +118,29 @@ struct SystemModelSO3xR3
using StateT = StateSO3xR3<FloatT>;
using ControlT = ControlSO3xR3<FloatT>;
using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
- using ControlNoiseT = Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using ControlNoiseT =
+ Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
static ObsT observationFunction(const StateT& state);
- static StateT propagationFunction(const StateT& state, const ControlT& control, const ControlNoiseT& noise, FloatT dt);
+ static StateT propagationFunction(const StateT& state,
+ const ControlT& control,
+ const ControlNoiseT& noise,
+ FloatT dt);
static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
-
};
-template<typename floatT>
+template <typename floatT>
struct SystemModelSE3xV
{
static_assert(std::is_floating_point_v<floatT>);
- struct dim {
+
+ struct dim
+ {
static constexpr long state = 12, control = 6, obs = 6;
};
@@ -132,17 +148,20 @@ struct SystemModelSE3xV
using StateT = StateSE3xV<FloatT>;
using ControlT = ControlSE3xV<FloatT>;
using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
- using ControlNoiseT = Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+ using ControlNoiseT =
+ Eigen::Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
static ObsT observationFunction(const StateT& state);
- static StateT propagationFunction(const StateT& state, const ControlT& control, const ControlNoiseT& noise, FloatT dt);
+ static StateT propagationFunction(const StateT& state,
+ const ControlT& control,
+ const ControlNoiseT& noise,
+ FloatT dt);
static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
-
};
extern template struct SystemModelSE3<float>;
diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
index 0c3c84c01670eb3604d50bdda35e22560ef417ba..a58af559c0007a70afaba718006ce5648029c57c 100644
--- a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.cpp
@@ -218,22 +218,16 @@ template class UnscentedKalmanFilter<SystemModelSO3xR3<float>>;
template class UnscentedKalmanFilter<SystemModelSO3xR3<double>>;
-
///////////////////
-
template <typename SystemModelT>
UnscentedKalmanFilterWithoutControl<SystemModelT>::UnscentedKalmanFilterWithoutControl(
- ObsCovT R,
- const AlphaT& alpha,
- StateT state0,
- StateCovT P0) :
- R_(std::move(R)),
- alpha_(alpha),
- state_(std::move(state0)),
- P_(std::move(P0)),
- weights_(alpha)
+ ObsCovT R,
+ const AlphaT& alpha,
+ StateT state0,
+ StateCovT P0) :
+ R_(std::move(R)), alpha_(alpha), state_(std::move(state0)), P_(std::move(P0)), weights_(alpha)
{
}
@@ -261,10 +255,8 @@ UnscentedKalmanFilterWithoutControl<SystemModelT>::propagation(FloatT dt)
Eigen::Matrix<FloatT, 2 * dim::state, dim::state> xi_j_after;
Eigen::Matrix<FloatT, 2 * dim::control, dim::state> w_j_after;
- auto propagate_and_retract = [&dt, &state_after](long row,
- long offset,
- const StateT& state,
- auto& xi_j) -> void
+ auto propagate_and_retract =
+ [&dt, &state_after](long row, long offset, const StateT& state, auto& xi_j) -> void
{
const StateT sig_j_after = SystemModelT::propagationFunction(state, dt);
xi_j.row(offset + row) = SystemModelT::inverseRetraction(state_after, sig_j_after);
@@ -283,8 +275,8 @@ UnscentedKalmanFilterWithoutControl<SystemModelT>::propagation(FloatT dt)
const SigmaPointsT sigma = xi_j_before.row(j);
const StateT sig_j_plus = SystemModelT::retraction(state_, sigma);
const StateT sig_j_minus = SystemModelT::retraction(state_, -sigma);
- propagate_and_retract(j, 0, sig_j_plus, xi_j_after);
- propagate_and_retract(j, dim::state, sig_j_minus, xi_j_after);
+ propagate_and_retract(j, 0, sig_j_plus, xi_j_after);
+ propagate_and_retract(j, dim::state, sig_j_minus, xi_j_after);
}
StateCovT P_after = calculate_covariance(weights_.state.wj, weights_.state.w0, xi_j_after);
diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
index 343a89292cf2c743d508e257782aa193c4b9ea32..a1214920c3f9c87ff037b2e08697c2a741454f7e 100644
--- a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h
@@ -32,7 +32,6 @@
#include "SystemModel.h"
-
template <typename SystemModelT>
class UnscentedKalmanFilter
{
@@ -96,8 +95,6 @@ extern template class UnscentedKalmanFilter<SystemModelSE3<double>>;
extern template class UnscentedKalmanFilter<SystemModelSO3xR3<float>>;
extern template class UnscentedKalmanFilter<SystemModelSO3xR3<double>>;
-
-
template <typename SystemModelT>
class UnscentedKalmanFilterWithoutControl
{
@@ -115,11 +112,10 @@ public:
using AlphaT = Eigen::Matrix<FloatT, 3, 1>;
- UnscentedKalmanFilterWithoutControl(
- ObsCovT R,
- const AlphaT& alpha,
- StateT state0,
- StateCovT P0);
+ UnscentedKalmanFilterWithoutControl(ObsCovT R,
+ const AlphaT& alpha,
+ StateT state0,
+ StateCovT P0);
UnscentedKalmanFilterWithoutControl() = delete;
private:
diff --git a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilterTest.cpp b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilterTest.cpp
index 5f39a1f77ab81c5f0722772f87cc9ffaf5e53fa3..287a14b891fd97811621983bfea0ff88c85fab7f 100644
--- a/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilterTest.cpp
+++ b/source/RobotAPI/libraries/ukfm/UnscentedKalmanFilterTest.cpp
@@ -25,11 +25,14 @@
* - https://ieeexplore.ieee.org/document/8206066/
*/
#include "UnscentedKalmanFilter.h"
-#include <cstdlib> /* srand, rand */
-#include <ctime> /* time */
-#include <ArmarXCore/core/logging/Logging.h>
+
+#include <cstdlib> /* srand, rand */
+#include <ctime> /* time */
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/time.h>
+
#include <sciplot/sciplot.hpp>
using T = float;
@@ -40,39 +43,48 @@ using SystemModelT = SystemModelSE3<T>;
constexpr long num_timesteps = 3000;
constexpr T max_time = 1;
constexpr T dt = max_time / num_timesteps;
-constexpr T c = (1 / max_time) * 2 * M_PI;
+constexpr T c = (1 / max_time) * 2 * M_PI;
constexpr T acc_noise_std = 0.01;
constexpr T om_noise_std = 0.01;
constexpr T obs_noise_std = 0.01;
-constexpr T initial_offset_angle = 0.0*10*M_PI / 180;
-const Vector initial_offet_pos = 0.5*Vector(1, 0.5, 0.7);
-
-void test_retract() {
- for (int i = 0; i < num_timesteps; i++) {
+constexpr T initial_offset_angle = 0.0 * 10 * M_PI / 180;
+const Vector initial_offet_pos = 0.5 * Vector(1, 0.5, 0.7);
+
+void
+test_retract()
+{
+ for (int i = 0; i < num_timesteps; i++)
+ {
SystemModelT::StateT state1, state2;
state1.pose = manif::SE3<T>::Random();
-// state1.velocity.setRandom();
+ // state1.velocity.setRandom();
state2.pose = manif::SE3<T>::Random();
-// state2.velocity.setRandom();
+ // state2.velocity.setRandom();
// sigma = state2 - state1
SystemModelT::SigmaPointsT sigma = SystemModelT::inverseRetraction(state1, state2);
// state3 = state1 + sigma --> state3 = state2
SystemModelT::StateT state3 = SystemModelT::retraction(state1, sigma);
-// ARMARX_CHECK((state2.velocity - state3.velocity).norm() < 10 * std::numeric_limits<T>::epsilon());
- ARMARX_CHECK((state2.pose - state3.pose).coeffs().norm() < 10000 * std::numeric_limits<T>::epsilon()) << (state2.pose - state3.pose).coeffs().norm();
+ // ARMARX_CHECK((state2.velocity - state3.velocity).norm() < 10 * std::numeric_limits<T>::epsilon());
+ ARMARX_CHECK((state2.pose - state3.pose).coeffs().norm() <
+ 10000 * std::numeric_limits<T>::epsilon())
+ << (state2.pose - state3.pose).coeffs().norm();
SystemModelT::SigmaPointsT sigma2 = SystemModelT::inverseRetraction(state1, state3);
// TODO: fix bad accuracy
- ARMARX_CHECK((sigma2 - sigma).norm() < 10000 * std::numeric_limits<T>::epsilon()) << (sigma2 - sigma).norm();
+ ARMARX_CHECK((sigma2 - sigma).norm() < 10000 * std::numeric_limits<T>::epsilon())
+ << (sigma2 - sigma).norm();
}
}
-void test_se3() {
- for (int i = 1; i < num_timesteps; i++) {
+void
+test_se3()
+{
+ for (int i = 1; i < num_timesteps; i++)
+ {
const T t = dt * i;
const T angle = t * c;
- const Eigen::Matrix<T, 3,1> pos(std::sin(angle), std::cos(angle), 0);
+ const Eigen::Matrix<T, 3, 1> pos(std::sin(angle), std::cos(angle), 0);
manif::SO3<T> rot = manif::SO3<T>(0, 0, angle);
ARMARX_CHECK(std::abs(rot.log().coeffs()(2) - angle) < 1e-6);
manif::SE3<T> pose(pos, rot);
@@ -83,12 +95,16 @@ void test_se3() {
}
}
-void simulate_trajectory(std::vector<SystemModelT::StateT>& states, std::vector<SystemModelT::ControlT>& omegas) {
+void
+simulate_trajectory(std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ControlT>& omegas)
+{
SystemModelT::StateT state;
-// state.velocity = Vector(c, 0, 0);
+ // state.velocity = Vector(c, 0, 0);
state.pose = manif::SE3<T>(0, 1, 0, M_PI, M_PI, 0);
states.push_back(state);
- for (int i = 1; i < num_timesteps; i++) {
+ for (int i = 1; i < num_timesteps; i++)
+ {
const T t = dt * i;
const T t_prev = dt * (i - 1);
const T angle = t * c;
@@ -97,36 +113,40 @@ void simulate_trajectory(std::vector<SystemModelT::StateT>& states, std::vector<
const T roll = 0.1 * M_PI * std::sin(angle) + M_PI;
const T pitch = 0.1 * M_PI * std::sin(angle) + M_PI;
const Vector pos(std::sin(angle), std::cos(angle), 0);
- manif::SE3Tangent<T> tangent(states.at(i-1).pose.log());
- ARMARX_CHECK((states.at(i-1).pose.translation() - pos).norm() < 0.1);
-// ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 0.1);
- const Vector vel(std::cos(angle) * c, - std::sin(angle) * c, 0);
- const Vector acc(- std::sin(angle) * (c * c), -std::cos(angle) * (c*c), 0);
- const Vector acc_prev(- std::sin(t_prev * c) * (c * c), -std::cos(t_prev * c) * (c*c), 0);
-// const Eigen::Vector3f vel((pos - state.position) / dt);
-// Eigen::Vector3f acc = (vel - state.velocity) / dt;
+ manif::SE3Tangent<T> tangent(states.at(i - 1).pose.log());
+ ARMARX_CHECK((states.at(i - 1).pose.translation() - pos).norm() < 0.1);
+ // ARMARX_CHECK((tangent.coeffs().segment(0, 3) - pos).norm() < 0.1);
+ const Vector vel(std::cos(angle) * c, -std::sin(angle) * c, 0);
+ const Vector acc(-std::sin(angle) * (c * c), -std::cos(angle) * (c * c), 0);
+ const Vector acc_prev(-std::sin(t_prev * c) * (c * c), -std::cos(t_prev * c) * (c * c), 0);
+ // const Eigen::Vector3f vel((pos - state.position) / dt);
+ // Eigen::Vector3f acc = (vel - state.velocity) / dt;
manif::SO3<T> rot = manif::SO3<T>(roll, pitch, yaw);
-// Eigen::Matrix<T, 3, 1> rot_tan = rot.log().coeffs();
-// ARMARX_CHECK(std::abs(rot_tan(2) - yaw) < 1e-6);
+ // Eigen::Matrix<T, 3, 1> rot_tan = rot.log().coeffs();
+ // ARMARX_CHECK(std::abs(rot_tan(2) - yaw) < 1e-6);
SystemModelT::ControlT control;
-// control.linear_accel = state.pose.rotation().inverse() * acc_prev;
- Vector angular_velocity(0.1*M_PI * std::cos(angle)*c, 0.1*M_PI * std::cos(angle)*c, M_PI * std::cos(angle)*c);
+ // control.linear_accel = state.pose.rotation().inverse() * acc_prev;
+ Vector angular_velocity(0.1 * M_PI * std::cos(angle) * c,
+ 0.1 * M_PI * std::cos(angle) * c,
+ M_PI * std::cos(angle) * c);
control.velocity.coeffs() << state.pose.asSO3().inverse().act(vel), angular_velocity;
- const SystemModelT::StateT propagated = SystemModelT::propagationFunction(state, control, SystemModelT::ControlNoiseT::Zero(), dt);
+ const SystemModelT::StateT propagated = SystemModelT::propagationFunction(
+ state, control, SystemModelT::ControlNoiseT::Zero(), dt);
manif::SE3Tangent<T> pro_tangent = propagated.pose.log();
T pos_diff1 = (propagated.pose.translation() - pos).norm();
-// ARMARX_CHECK(pos_diff1 < pos_diff2);
- ARMARX_CHECK(pos_diff1 < 2e-5) << "Position is not the same: " << pos << " vs " << propagated.pose.translation();
-// ARMARX_CHECK((propagated.velocity - vel).norm() < 1e-4) << "Velocity is not the same: " << vel << " vs " << propagated.velocity;
-// ARMARX_CHECK(propagated.pose.asSO3().lminus(rot).coeffs().norm() < 1e-2) << "Rotation is not the same: " << rot.rotation() << " vs " << propagated.pose.rotation();
-// state.velocity = vel;
+ // ARMARX_CHECK(pos_diff1 < pos_diff2);
+ ARMARX_CHECK(pos_diff1 < 2e-5)
+ << "Position is not the same: " << pos << " vs " << propagated.pose.translation();
+ // ARMARX_CHECK((propagated.velocity - vel).norm() < 1e-4) << "Velocity is not the same: " << vel << " vs " << propagated.velocity;
+ // ARMARX_CHECK(propagated.pose.asSO3().lminus(rot).coeffs().norm() < 1e-2) << "Rotation is not the same: " << rot.rotation() << " vs " << propagated.pose.rotation();
+ // state.velocity = vel;
state.pose = manif::SE3<T>(pos, rot);
states.push_back(propagated);
tangent = state.pose.log();
ARMARX_CHECK((state.pose.translation() - pos).norm() < 1e-6);
-// ARMARX_CHECK(std::abs(tangent.coeffs()(5) - yaw) < 1e-2);
+ // ARMARX_CHECK(std::abs(tangent.coeffs()(5) - yaw) < 1e-2);
// add noise
control.velocity.coeffs().segment(0, 3) += acc_noise_std * Vector::Random();
@@ -135,22 +155,29 @@ void simulate_trajectory(std::vector<SystemModelT::StateT>& states, std::vector<
}
}
-void simulate_observation(const std::vector<SystemModelT::StateT>& states, std::vector<SystemModelT::ObsT>& observations) {
- for (const auto& state : states) {
+void
+simulate_observation(const std::vector<SystemModelT::StateT>& states,
+ std::vector<SystemModelT::ObsT>& observations)
+{
+ for (const auto& state : states)
+ {
SystemModelT::ObsT obs = SystemModelT::observationFunction(state);
SystemModelT::ObsT true_obs;
true_obs = state.pose.log().coeffs();
-// true_obs.segment(0, 3) = state.pose.translation();
-// true_obs.segment(3, 3) = state.pose.asSO3().log().coeffs();
+ // true_obs.segment(0, 3) = state.pose.translation();
+ // true_obs.segment(3, 3) = state.pose.asSO3().log().coeffs();
ARMARX_CHECK((obs - true_obs).norm() < std::numeric_limits<T>::epsilon());
observations.push_back(obs + obs_noise_std * SystemModelT::ObsT::Random());
}
}
-int main( ) {
+
+int
+main()
+{
srand(time(NULL));
test_retract();
-// test_se3();
+ // test_se3();
std::vector<SystemModelT::StateT> states;
std::vector<SystemModelT::ControlT> omegas;
@@ -158,22 +185,27 @@ int main( ) {
simulate_trajectory(states, omegas);
simulate_observation(states, observations);
- ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size() << " Num Obs: " << observations.size();
- UnscentedKalmanFilter<SystemModelT>::PropCovT Q = UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ ARMARX_INFO << "Num States: " << states.size() << " Num Controls: " << omegas.size()
+ << " Num Obs: " << observations.size();
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
Q.block<3, 3>(0, 0) *= acc_noise_std * acc_noise_std;
Q.block<3, 3>(3, 3) *= om_noise_std * om_noise_std;
- UnscentedKalmanFilter<SystemModelT>::ObsCovT R = UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity() * obs_noise_std * obs_noise_std;
- UnscentedKalmanFilter<SystemModelT>::StateCovT P0 = UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity() * obs_noise_std * obs_noise_std;
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
P0.block<3, 3>(3, 3) *= initial_offset_angle * initial_offset_angle;
P0.block<3, 3>(0, 0) *= initial_offet_pos.norm() * initial_offet_pos.norm();
-// P0.block<3, 3>(6, 6) *= 0.0;
- UnscentedKalmanFilter<SystemModelT>::AlphaT alpha = UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * 1e-3;
+ // P0.block<3, 3>(6, 6) *= 0.0;
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
+ UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * 1e-3;
SystemModelT::StateT state0;
manif::SO3<T> rot(0, 0, initial_offset_angle);
state0.pose = states.at(0).pose.lplus(manif::SE3<T>(initial_offet_pos, rot).log());
-// state0.velocity = states.at(0).velocity;
+ // state0.velocity = states.at(0).velocity;
UnscentedKalmanFilter<SystemModelT> ukf(Q, R, alpha, state0, P0);
std::vector<SystemModelT::StateT> ukf_states;
@@ -184,25 +216,30 @@ int main( ) {
std::vector<T> x_true, y_true, z_true, x_obs, y_obs, z_obs, x_ukf, y_ukf, z_ukf;
std::vector<T> vx_true, vy_true, vz_true, vx_ukf, vy_ukf, vz_ukf;
std::vector<T> a_true, b_true, c_true, a_obs, b_obs, c_obs, a_ukf, b_ukf, c_ukf;
-
- for (int i = 1; i < num_timesteps; i++) {
+
+ for (int i = 1; i < num_timesteps; i++)
+ {
// propagate
TIMING_START(LOOP);
TIMING_START(PROPAGATION);
- ukf.propagation(omegas.at(i-1), dt);
+ ukf.propagation(omegas.at(i - 1), dt);
TIMING_END(PROPAGATION);
- if ((i-1) % 100 == 0) {
+ if ((i - 1) % 100 == 0)
+ {
TIMING_START(UPDATE);
ukf.update(observations.at(i));
TIMING_END(UPDATE);
TIMING_START(REST);
const SystemModelT::StateT& current_state = ukf.getCurrentState();
-// ARMARX_INFO << "Velocity Diff: " << (states.at(i).velocity - current_state.velocity).norm();
- ARMARX_INFO << "Pose Diff: " << (states.at(i).pose - current_state.pose).coeffs().norm();
-// ARMARX_INFO << "Vel: " << current_state.velocity - states.at(i).velocity;
+ // ARMARX_INFO << "Velocity Diff: " << (states.at(i).velocity - current_state.velocity).norm();
+ ARMARX_INFO << "Pose Diff: "
+ << (states.at(i).pose - current_state.pose).coeffs().norm();
+ // ARMARX_INFO << "Vel: " << current_state.velocity - states.at(i).velocity;
ARMARX_INFO << "Max Cov " << ukf.getCurrentStateCovariance().cwiseAbs().maxCoeff();
- ARMARX_INFO << "Diag: " << (ukf.getCurrentStateCovariance() - Eigen::Matrix<T, 6, 6>::Identity()).norm();
-// ARMARX_CHECK((states.at(i).position - current_state.position).norm() < 1);
+ ARMARX_INFO
+ << "Diag: "
+ << (ukf.getCurrentStateCovariance() - Eigen::Matrix<T, 6, 6>::Identity()).norm();
+ // ARMARX_CHECK((states.at(i).position - current_state.position).norm() < 1);
x_true.push_back(states.at(i).pose.log().coeffs()(0));
y_true.push_back(states.at(i).pose.log().coeffs()(1));
@@ -216,13 +253,13 @@ int main( ) {
y_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(1));
z_ukf.push_back(ukf.getCurrentState().pose.log().coeffs()(2));
-// vx_true.push_back(states.at(i).velocity(0, 0));
-// vy_true.push_back(states.at(i).velocity(1, 0));
-// vz_true.push_back(states.at(i).velocity(2, 0));
-//
-// vx_ukf.push_back(ukf.getCurrentState().velocity(0, 0));
-// vy_ukf.push_back(ukf.getCurrentState().velocity(1, 0));
-// vz_ukf.push_back(ukf.getCurrentState().velocity(2, 0));
+ // vx_true.push_back(states.at(i).velocity(0, 0));
+ // vy_true.push_back(states.at(i).velocity(1, 0));
+ // vz_true.push_back(states.at(i).velocity(2, 0));
+ //
+ // vx_ukf.push_back(ukf.getCurrentState().velocity(0, 0));
+ // vy_ukf.push_back(ukf.getCurrentState().velocity(1, 0));
+ // vz_ukf.push_back(ukf.getCurrentState().velocity(2, 0));
a_true.push_back(states.at(i).pose.log().coeffs()(3));
b_true.push_back(states.at(i).pose.log().coeffs()(4));
@@ -255,10 +292,10 @@ int main( ) {
pos_plot.drawCurve(x_obs, y_obs).label("Obs");
pos_plot.drawCurve(x_ukf, y_ukf).label("UKF");
-// sciplot::Plot3D vel_plot;
-//
-// vel_plot.drawCurve(vx_true, vy_true, vz_true).label("True").lineWidth(1);
-// vel_plot.drawCurve(vx_ukf, vy_ukf, vz_ukf).label("UKF").lineWidth(1);
+ // sciplot::Plot3D vel_plot;
+ //
+ // vel_plot.drawCurve(vx_true, vy_true, vz_true).label("True").lineWidth(1);
+ // vel_plot.drawCurve(vx_ukf, vy_ukf, vz_ukf).label("UKF").lineWidth(1);
sciplot::Plot3D orientation_plot;
@@ -269,7 +306,6 @@ int main( ) {
pos_plot.show();
position_plot.show();
-// vel_plot.show();
+ // vel_plot.show();
orientation_plot.show();
-
}
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.cpp b/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.cpp
index 8737c606ca57c6178c3d1c7df4fb3874f2d940ec..43808dc600faf76d5fd71263f2560150f0f63fa9 100644
--- a/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.cpp
+++ b/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.cpp
@@ -22,13 +22,13 @@
* GNU General Public License
*/
#include "DebugLayerControlWidget.h"
+
#include <RobotAPI/libraries/widgets/ui_DebugLayerControlWidget.h>
#define UPDATE_INTERVAL 1.0 // update every second
DebugLayerControlWidget::DebugLayerControlWidget(QWidget* parent) :
- QWidget(parent),
- ui(new Ui::DebugLayerControlWidget)
+ QWidget(parent), ui(new Ui::DebugLayerControlWidget)
{
entityDrawer = NULL;
ui->setupUi(this);
@@ -40,8 +40,12 @@ DebugLayerControlWidget::DebugLayerControlWidget(QWidget* parent) :
sensor_mgr->insertTimerSensor(timerSensor);
//connect signal mapper
- QObject::connect(&layerSignalMapperVisible, SIGNAL(mapped(QString)), this, SLOT(layerToggleVisibility(QString)));
- QObject::connect(&layerSignalMapperRemove, SIGNAL(mapped(QString)), this, SLOT(layerRemove(QString)));
+ QObject::connect(&layerSignalMapperVisible,
+ SIGNAL(mapped(QString)),
+ this,
+ SLOT(layerToggleVisibility(QString)));
+ QObject::connect(
+ &layerSignalMapperRemove, SIGNAL(mapped(QString)), this, SLOT(layerRemove(QString)));
}
DebugLayerControlWidget::~DebugLayerControlWidget()
@@ -55,12 +59,14 @@ DebugLayerControlWidget::~DebugLayerControlWidget()
delete ui;
}
-void DebugLayerControlWidget::setEntityDrawer(armarx::DebugDrawerComponentPtr entityDrawer)
+void
+DebugLayerControlWidget::setEntityDrawer(armarx::DebugDrawerComponentPtr entityDrawer)
{
this->entityDrawer = entityDrawer;
}
-void DebugLayerControlWidget::updateLayers()
+void
+DebugLayerControlWidget::updateLayers()
{
//ui.layerTable->clear();
if (entityDrawer)
@@ -78,19 +84,22 @@ void DebugLayerControlWidget::updateLayers()
//add name and number of elements
ui->layerTable->setItem(i, 0, new QTableWidgetItem{name});
- ui->layerTable->setItem(i, 1, new QTableWidgetItem{QString::number(layer.elementCount)});
+ ui->layerTable->setItem(
+ i, 1, new QTableWidgetItem{QString::number(layer.elementCount)});
//add visibility checkbox
std::unique_ptr<QCheckBox> box{new QCheckBox};
box->setChecked(layer.visible);
layerSignalMapperVisible.setMapping(box.get(), name);
- QObject::connect(box.get(), SIGNAL(stateChanged(int)), &layerSignalMapperVisible, SLOT(map()));
+ QObject::connect(
+ box.get(), SIGNAL(stateChanged(int)), &layerSignalMapperVisible, SLOT(map()));
ui->layerTable->setCellWidget(i, 2, box.release());
//add remove button
std::unique_ptr<QPushButton> removeB{new QPushButton("remove")};
layerSignalMapperRemove.setMapping(removeB.get(), name);
- QObject::connect(removeB.get(), SIGNAL(clicked()), &layerSignalMapperRemove, SLOT(map()));
+ QObject::connect(
+ removeB.get(), SIGNAL(clicked()), &layerSignalMapperRemove, SLOT(map()));
ui->layerTable->setCellWidget(i, 3, removeB.release());
}
}
@@ -100,7 +109,8 @@ void DebugLayerControlWidget::updateLayers()
}
}
-void DebugLayerControlWidget::layerToggleVisibility(QString layerName)
+void
+DebugLayerControlWidget::layerToggleVisibility(QString layerName)
{
//VR_INFO << "should toggle: " << layerName.toStdString() << std::endl;
auto name = layerName.toStdString();
@@ -117,7 +127,8 @@ void DebugLayerControlWidget::layerToggleVisibility(QString layerName)
}
}
-void DebugLayerControlWidget::layerRemove(QString layerName)
+void
+DebugLayerControlWidget::layerRemove(QString layerName)
{
auto name = layerName.toStdString();
VR_INFO << "remove layer: " << name << std::endl;
@@ -132,7 +143,8 @@ void DebugLayerControlWidget::layerRemove(QString layerName)
}
}
-void DebugLayerControlWidget::onTimer(void* data, SoSensor* sensor)
+void
+DebugLayerControlWidget::onTimer(void* data, SoSensor* sensor)
{
DebugLayerControlWidget* controller = static_cast<DebugLayerControlWidget*>(data);
if (controller)
diff --git a/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.h b/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.h
index 619e936e823aa38b7e0632c30a7b75e25e0938bc..af2ce0adfa9b68a1067d0d209199e003662e52dd 100644
--- a/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.h
+++ b/source/RobotAPI/libraries/widgets/DebugLayerControlWidget.h
@@ -26,26 +26,26 @@
/* ArmarX headers */
#include <ArmarXCore/core/Component.h>
+
#include <RobotAPI/components/DebugDrawer/DebugDrawerComponent.h>
/* QT headers */
-#include <QWidget>
#include <QCheckBox>
-#include <QSignalMapper>
#include <QPushButton>
+#include <QSignalMapper>
+#include <QWidget>
/* STD headers */
#include <unordered_map>
/* Inventor headers */
-#include <Inventor/sensors/SoTimerSensor.h>
-#include <Inventor/SoDB.h>
#include <Inventor/Qt/SoQt.h>
+#include <Inventor/SoDB.h>
+#include <Inventor/sensors/SoTimerSensor.h>
/* System headers */
#include <string>
-
namespace Ui
{
class DebugLayerControlWidget;
@@ -85,7 +85,7 @@ protected:
protected slots:
void layerRemove(QString layerName);
+
private:
Ui::DebugLayerControlWidget* ui;
};
-
diff --git a/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp b/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp
index 2fbe7b66e300edd3ce7810ac68910d812e9a27d5..50ed277195aa8cadf0ec64afa694fc52b66157fb 100644
--- a/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp
+++ b/source/RobotAPI/libraries/widgets/JSONTreeModel.cpp
@@ -5,7 +5,8 @@
namespace armarx
{
- void JSONTreeModel::setRoot(nlohmann::json const& rootParam)
+ void
+ JSONTreeModel::setRoot(nlohmann::json const& rootParam)
{
root = rootParam;
rows.clear();
@@ -16,7 +17,8 @@ namespace armarx
names[&root] = "";
}
- QModelIndex JSONTreeModel::index(int row, int column, const QModelIndex& parentIndex) const
+ QModelIndex
+ JSONTreeModel::index(int row, int column, const QModelIndex& parentIndex) const
{
nlohmann::json* parent = refFrom(parentIndex);
if (row >= int(parent->size()))
@@ -49,10 +51,10 @@ namespace armarx
{
return QModelIndex();
}
-
}
- QModelIndex JSONTreeModel::parent(const QModelIndex& index) const
+ QModelIndex
+ JSONTreeModel::parent(const QModelIndex& index) const
{
if (!index.isValid())
{
@@ -76,7 +78,8 @@ namespace armarx
return createIndex(row, 0, parent);
}
- int JSONTreeModel::rowCount(const QModelIndex& parentIndex) const
+ int
+ JSONTreeModel::rowCount(const QModelIndex& parentIndex) const
{
if (parentIndex.column() > 1)
{
@@ -94,12 +97,14 @@ namespace armarx
}
}
- int JSONTreeModel::columnCount(const QModelIndex& parent) const
+ int
+ JSONTreeModel::columnCount(const QModelIndex& parent) const
{
return 2;
}
- QVariant JSONTreeModel::data(const QModelIndex& index, int role) const
+ QVariant
+ JSONTreeModel::data(const QModelIndex& index, int role) const
{
if (!index.isValid())
{
@@ -117,8 +122,8 @@ namespace armarx
{
if (!names.count(ref))
{
- ARMARX_WARNING << "Map does not contain name for ref: " <<
- (ref ? ref->dump(2) : "(null)");
+ ARMARX_WARNING << "Map does not contain name for ref: "
+ << (ref ? ref->dump(2) : "(null)");
return QVariant();
}
std::string const& name = names.at(ref);
@@ -149,7 +154,8 @@ namespace armarx
}
}
- QVariant JSONTreeModel::headerData(int section, Qt::Orientation orientation, int role) const
+ QVariant
+ JSONTreeModel::headerData(int section, Qt::Orientation orientation, int role) const
{
if (role != Qt::DisplayRole)
{
@@ -167,7 +173,8 @@ namespace armarx
}
}
- nlohmann::json* JSONTreeModel::refFrom(const QModelIndex& index) const
+ nlohmann::json*
+ JSONTreeModel::refFrom(const QModelIndex& index) const
{
if (index.isValid())
{
@@ -179,4 +186,4 @@ namespace armarx
}
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/widgets/JSONTreeModel.h b/source/RobotAPI/libraries/widgets/JSONTreeModel.h
index 5fdb53fe42a6578fb81ece91c88333801ac2227e..577398097a35ece7a0e98dd9ad307cb426c8b6bb 100644
--- a/source/RobotAPI/libraries/widgets/JSONTreeModel.h
+++ b/source/RobotAPI/libraries/widgets/JSONTreeModel.h
@@ -1,8 +1,9 @@
#pragma once
-#include <SimoxUtility/json/json.hpp>
#include <QAbstractItemModel>
+#include <SimoxUtility/json/json.hpp>
+
namespace armarx
{
@@ -29,4 +30,4 @@ namespace armarx
mutable nlohmann::json root;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.cpp
index 74f4eb5bf9130cde377e4fa99cce17b62f944bac..d747802f5196bb63ae4520654bcba4a21f9a4172 100644
--- a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.cpp
@@ -25,27 +25,43 @@
namespace armarx::DebugDrawerToArVizGroup
{
// DO NOT EDIT NEXT LINE
- DebugDrawerToArVizGroupRemoteStateOfferer::SubClassRegistry DebugDrawerToArVizGroupRemoteStateOfferer::Registry(DebugDrawerToArVizGroupRemoteStateOfferer::GetName(), &DebugDrawerToArVizGroupRemoteStateOfferer::CreateInstance);
+ DebugDrawerToArVizGroupRemoteStateOfferer::SubClassRegistry
+ DebugDrawerToArVizGroupRemoteStateOfferer::Registry(
+ DebugDrawerToArVizGroupRemoteStateOfferer::GetName(),
+ &DebugDrawerToArVizGroupRemoteStateOfferer::CreateInstance);
- DebugDrawerToArVizGroupRemoteStateOfferer::DebugDrawerToArVizGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < DebugDrawerToArVizGroupStatechartContext > (reader)
- {}
+ DebugDrawerToArVizGroupRemoteStateOfferer::DebugDrawerToArVizGroupRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<DebugDrawerToArVizGroupStatechartContext>(reader)
+ {
+ }
- void DebugDrawerToArVizGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer() {}
+ void
+ DebugDrawerToArVizGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+ {
+ }
- void DebugDrawerToArVizGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer() {}
+ void
+ DebugDrawerToArVizGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+ {
+ }
- void DebugDrawerToArVizGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer() {}
+ void
+ DebugDrawerToArVizGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+ {
+ }
// DO NOT EDIT NEXT FUNCTION
- std::string DebugDrawerToArVizGroupRemoteStateOfferer::GetName()
+ std::string
+ DebugDrawerToArVizGroupRemoteStateOfferer::GetName()
{
return "DebugDrawerToArVizGroupRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
- XMLStateOffererFactoryBasePtr DebugDrawerToArVizGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+ XMLStateOffererFactoryBasePtr
+ DebugDrawerToArVizGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new DebugDrawerToArVizGroupRemoteStateOfferer(reader));
}
-}
+} // namespace armarx::DebugDrawerToArVizGroup
diff --git a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.h
index 245714e2eda9b91edc7b56e474739517670a7e81..6046a17bd8c1ec97e10de0c047d2085afed3757a 100644
--- a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/DebugDrawerToArVizGroupRemoteStateOfferer.h
@@ -23,12 +23,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "DebugDrawerToArVizGroupStatechartContext.generated.h"
namespace armarx::DebugDrawerToArVizGroup
{
class DebugDrawerToArVizGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < DebugDrawerToArVizGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ DebugDrawerToArVizGroupStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
DebugDrawerToArVizGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -42,9 +44,5 @@ namespace armarx::DebugDrawerToArVizGroup
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
-
-
+} // namespace armarx::DebugDrawerToArVizGroup
diff --git a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.cpp b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.cpp
index f2be2a6d19333b692ea006f7174ed61cb61450ea..c1edbc6e84e667f8116321aed741f2681e3f76c5 100644
--- a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.cpp
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.cpp
@@ -28,9 +28,12 @@
namespace armarx::DebugDrawerToArVizGroup
{
// DO NOT EDIT NEXT LINE
- UpateLayerBlackWhitelist::SubClassRegistry UpateLayerBlackWhitelist::Registry(UpateLayerBlackWhitelist::GetName(), &UpateLayerBlackWhitelist::CreateInstance);
+ UpateLayerBlackWhitelist::SubClassRegistry
+ UpateLayerBlackWhitelist::Registry(UpateLayerBlackWhitelist::GetName(),
+ &UpateLayerBlackWhitelist::CreateInstance);
- void UpateLayerBlackWhitelist::onEnter()
+ void
+ UpateLayerBlackWhitelist::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
@@ -72,16 +75,17 @@ namespace armarx::DebugDrawerToArVizGroup
// // execution time should be short (<100ms)
//}
- void UpateLayerBlackWhitelist::onExit()
+ void
+ UpateLayerBlackWhitelist::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
-
// DO NOT EDIT NEXT FUNCTION
- XMLStateFactoryBasePtr UpateLayerBlackWhitelist::CreateInstance(XMLStateConstructorParams stateData)
+ XMLStateFactoryBasePtr
+ UpateLayerBlackWhitelist::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new UpateLayerBlackWhitelist(stateData));
}
-}
+} // namespace armarx::DebugDrawerToArVizGroup
diff --git a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.h b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.h
index 35cef461547e726ea7ecd3fb77ad2ef35e824cac..8a23b54db9f845958a82736035d58c0ed6a1668c 100644
--- a/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.h
+++ b/source/RobotAPI/statecharts/DebugDrawerToArVizGroup/UpateLayerBlackWhitelist.h
@@ -26,11 +26,12 @@
namespace armarx::DebugDrawerToArVizGroup
{
class UpateLayerBlackWhitelist :
- public UpateLayerBlackWhitelistGeneratedBase < UpateLayerBlackWhitelist >
+ public UpateLayerBlackWhitelistGeneratedBase<UpateLayerBlackWhitelist>
{
public:
- UpateLayerBlackWhitelist(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < UpateLayerBlackWhitelist > (stateData), UpateLayerBlackWhitelistGeneratedBase < UpateLayerBlackWhitelist > (stateData)
+ UpateLayerBlackWhitelist(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<UpateLayerBlackWhitelist>(stateData),
+ UpateLayerBlackWhitelistGeneratedBase<UpateLayerBlackWhitelist>(stateData)
{
}
@@ -48,7 +49,4 @@ namespace armarx::DebugDrawerToArVizGroup
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
-
-
-
+} // namespace armarx::DebugDrawerToArVizGroup
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.cpp b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.cpp
index d36cedf80739a6948b7921599400bf2ab17179e5..ff75e675f3d08b0dafc8407c79416af0756ebafa 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.cpp
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.cpp
@@ -20,44 +20,42 @@
* GNU General Public License
*/
+#include "DetectForceFlank.h"
+
#include <thread>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include "DetectForceFlank.h"
-
using namespace armarx;
using namespace ForceTorqueUtility;
// DO NOT EDIT NEXT LINE
-DetectForceFlank::SubClassRegistry DetectForceFlank::Registry(DetectForceFlank::GetName(), &DetectForceFlank::CreateInstance);
+DetectForceFlank::SubClassRegistry DetectForceFlank::Registry(DetectForceFlank::GetName(),
+ &DetectForceFlank::CreateInstance);
-void DetectForceFlank::run()
+void
+DetectForceFlank::run()
{
- ARMARX_CHECK_EXPRESSION(in.getTriggerOnDecreasingForceVectorLength() || in.getTriggerOnIncreasingForceVectorLength());
+ ARMARX_CHECK_EXPRESSION(in.getTriggerOnDecreasingForceVectorLength() ||
+ in.getTriggerOnIncreasingForceVectorLength());
const float forceThreshold = in.getForceVectorLengthThreshold();
- DatafieldRefPtr forceDf = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield(in.getFTDatafieldName()));
+ DatafieldRefPtr forceDf = DatafieldRefPtr::dynamicCast(
+ getForceTorqueObserver()->getForceDatafield(in.getFTDatafieldName()));
const Eigen::Vector3f weights = in.getForceWeights()->toEigen();
- const float initialForce = forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).norm();
+ const float initialForce =
+ forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).norm();
while (!isRunningTaskStopped()) // stop run function if returning true
{
- const float force = forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).norm();
+ const float force =
+ forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).norm();
ARMARX_INFO << deactivateSpam(1) << VAROUT(force) << " " << VAROUT(initialForce);
- if (
- (
- in.getTriggerOnDecreasingForceVectorLength() &&
- force < initialForce &&
- initialForce - force >= forceThreshold
- ) ||
- (
- in.getTriggerOnIncreasingForceVectorLength() &&
- force > initialForce &&
- force - initialForce >= forceThreshold
- )
- )
+ if ((in.getTriggerOnDecreasingForceVectorLength() && force < initialForce &&
+ initialForce - force >= forceThreshold) ||
+ (in.getTriggerOnIncreasingForceVectorLength() && force > initialForce &&
+ force - initialForce >= forceThreshold))
{
emitSuccess();
return;
@@ -67,10 +65,9 @@ void DetectForceFlank::run()
emitFailure();
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr DetectForceFlank::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+DetectForceFlank::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new DetectForceFlank(stateData));
}
-
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h
index de0a5494a3e7831f10d88b38be243f8b5b31afa0..c546a3d4e665d77745153e28c416b93c30612025 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceFlank.h
@@ -25,19 +25,27 @@
namespace armarx::ForceTorqueUtility
{
- class DetectForceFlank :
- public DetectForceFlankGeneratedBase < DetectForceFlank >
+ class DetectForceFlank : public DetectForceFlankGeneratedBase<DetectForceFlank>
{
public:
- DetectForceFlank(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < DetectForceFlank > (stateData), DetectForceFlankGeneratedBase < DetectForceFlank > (stateData)
+ DetectForceFlank(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<DetectForceFlank>(stateData),
+ DetectForceFlankGeneratedBase<DetectForceFlank>(stateData)
{
}
// inherited from StateBase
- void onEnter() override {}
+ void
+ onEnter() override
+ {
+ }
+
void run() override;
- void onExit() override {}
+
+ void
+ onExit() override
+ {
+ }
// static functions for AbstractFactory Method
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
@@ -47,7 +55,4 @@ namespace armarx::ForceTorqueUtility
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
-
-
-
+} // namespace armarx::ForceTorqueUtility
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
index e21c2c1785fb690f1b26d7e8a9c52e9aafc558ac..e222b4952c95b6bde41615ffc6289905516cd095 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
@@ -20,21 +20,21 @@
* GNU General Public License
*/
-#include <thread>
-
#include "DetectForceSpike.h"
+#include <thread>
+
#include <RobotAPI/libraries/core/FramedPose.h>
using namespace armarx;
using namespace ForceTorqueUtility;
// DO NOT EDIT NEXT LINE
-DetectForceSpike::SubClassRegistry DetectForceSpike::Registry(DetectForceSpike::GetName(), &DetectForceSpike::CreateInstance);
+DetectForceSpike::SubClassRegistry DetectForceSpike::Registry(DetectForceSpike::GetName(),
+ &DetectForceSpike::CreateInstance);
-
-
-void DetectForceSpike::run()
+void
+DetectForceSpike::run()
{
ARMARX_CHECK_EXPRESSION(in.getTriggerInAxisDirection() || in.getTriggerCounterAxisDirection());
ARMARX_CHECK_GREATER_EQUAL(in.getWindowSizeMs(), 10);
@@ -43,12 +43,18 @@ void DetectForceSpike::run()
const float forceThreshold = in.getForceThreshold();
ARMARX_CHECK_GREATER(forceThreshold, 0);
- DatafieldRefPtr forceDf = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield(in.getFTDatafieldName()));
+ DatafieldRefPtr forceDf = DatafieldRefPtr::dynamicCast(
+ getForceTorqueObserver()->getForceDatafield(in.getFTDatafieldName()));
const Eigen::Vector3f weights = in.getForceWeights()->toEigen();
const Eigen::Vector3f axis = in.getAxis()->toEigen().normalized();
auto getForceAlongAxis = [&]() -> float
{
- return forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).transpose() * axis;
+ return forceDf->getDataField()
+ ->get<FramedDirection>()
+ ->toEigen()
+ .cwiseProduct(weights)
+ .transpose() *
+ axis;
};
in.getTriggerInAxisDirection();
@@ -98,7 +104,8 @@ void DetectForceSpike::run()
}
}
}
- if ((in.getTriggerInAxisDirection() && r2fDetected) || (in.getTriggerCounterAxisDirection() && f2rDetected))
+ if ((in.getTriggerInAxisDirection() && r2fDetected) ||
+ (in.getTriggerCounterAxisDirection() && f2rDetected))
{
emitSuccess();
return;
@@ -109,10 +116,9 @@ void DetectForceSpike::run()
emitFailure();
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr DetectForceSpike::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+DetectForceSpike::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new DetectForceSpike(stateData));
}
-
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h
index 9ee5ce9e2c41f4bd45b78a21c2801da9cfbf2475..c613cfb6b8256a2c1a2733d64c115a0b7ac43be6 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.h
@@ -25,19 +25,27 @@
namespace armarx::ForceTorqueUtility
{
- class DetectForceSpike :
- public DetectForceSpikeGeneratedBase < DetectForceSpike >
+ class DetectForceSpike : public DetectForceSpikeGeneratedBase<DetectForceSpike>
{
public:
- DetectForceSpike(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < DetectForceSpike > (stateData), DetectForceSpikeGeneratedBase < DetectForceSpike > (stateData)
+ DetectForceSpike(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<DetectForceSpike>(stateData),
+ DetectForceSpikeGeneratedBase<DetectForceSpike>(stateData)
{
}
// inherited from StateBase
- void onEnter() override {}
+ void
+ onEnter() override
+ {
+ }
+
void run() override;
- void onExit() override {}
+
+ void
+ onExit() override
+ {
+ }
// static functions for AbstractFactory Method
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData);
@@ -47,4 +55,4 @@ namespace armarx::ForceTorqueUtility
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
+} // namespace armarx::ForceTorqueUtility
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.cpp
index 91dc4d2a822815cbbe916147702d9aab46b16c52..6c459d77bb34a5265c1d1ca863970d7c8d72132c 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.cpp
@@ -26,41 +26,42 @@ using namespace armarx;
using namespace ForceTorqueUtility;
// DO NOT EDIT NEXT LINE
-ForceTorqueUtilityRemoteStateOfferer::SubClassRegistry ForceTorqueUtilityRemoteStateOfferer::Registry(ForceTorqueUtilityRemoteStateOfferer::GetName(), &ForceTorqueUtilityRemoteStateOfferer::CreateInstance);
-
-
-
-ForceTorqueUtilityRemoteStateOfferer::ForceTorqueUtilityRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < ForceTorqueUtilityStatechartContext > (reader)
+ForceTorqueUtilityRemoteStateOfferer::SubClassRegistry
+ ForceTorqueUtilityRemoteStateOfferer::Registry(
+ ForceTorqueUtilityRemoteStateOfferer::GetName(),
+ &ForceTorqueUtilityRemoteStateOfferer::CreateInstance);
+
+ForceTorqueUtilityRemoteStateOfferer::ForceTorqueUtilityRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<ForceTorqueUtilityStatechartContext>(reader)
{
}
-void ForceTorqueUtilityRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+void
+ForceTorqueUtilityRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
-
}
-void ForceTorqueUtilityRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+void
+ForceTorqueUtilityRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
-
}
-void ForceTorqueUtilityRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+void
+ForceTorqueUtilityRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
-
}
// DO NOT EDIT NEXT FUNCTION
-std::string ForceTorqueUtilityRemoteStateOfferer::GetName()
+std::string
+ForceTorqueUtilityRemoteStateOfferer::GetName()
{
return "ForceTorqueUtilityRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateOffererFactoryBasePtr ForceTorqueUtilityRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+XMLStateOffererFactoryBasePtr
+ForceTorqueUtilityRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new ForceTorqueUtilityRemoteStateOfferer(reader));
}
-
-
-
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h
index 8fe4acb8bee17e1fc93fb9b5ed2671fa2b18dbda..adc4bbe6eedd37048be76dc7d1ff6cd375964b89 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/ForceTorqueUtilityRemoteStateOfferer.h
@@ -23,12 +23,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "ForceTorqueUtilityStatechartContext.generated.h"
namespace armarx::ForceTorqueUtility
{
class ForceTorqueUtilityRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < ForceTorqueUtilityStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ ForceTorqueUtilityStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
ForceTorqueUtilityRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -42,7 +44,5 @@ namespace armarx::ForceTorqueUtility
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
+} // namespace armarx::ForceTorqueUtility
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp
index 1324dfff62f4a6ec3df04e61d451f062125a50c8..92b1da5836b72e191ecc91330d43549795227ae0 100644
--- a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.cpp
@@ -3,27 +3,43 @@
namespace armarx::ObjectMemoryGroup
{
// DO NOT EDIT NEXT LINE
- ObjectMemoryGroupRemoteStateOfferer::SubClassRegistry ObjectMemoryGroupRemoteStateOfferer::Registry(ObjectMemoryGroupRemoteStateOfferer::GetName(), &ObjectMemoryGroupRemoteStateOfferer::CreateInstance);
+ ObjectMemoryGroupRemoteStateOfferer::SubClassRegistry
+ ObjectMemoryGroupRemoteStateOfferer::Registry(
+ ObjectMemoryGroupRemoteStateOfferer::GetName(),
+ &ObjectMemoryGroupRemoteStateOfferer::CreateInstance);
- ObjectMemoryGroupRemoteStateOfferer::ObjectMemoryGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < ObjectMemoryGroupStatechartContext > (reader)
- {}
+ ObjectMemoryGroupRemoteStateOfferer::ObjectMemoryGroupRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<ObjectMemoryGroupStatechartContext>(reader)
+ {
+ }
- void ObjectMemoryGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer() {}
+ void
+ ObjectMemoryGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+ {
+ }
- void ObjectMemoryGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer() {}
+ void
+ ObjectMemoryGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+ {
+ }
- void ObjectMemoryGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer() {}
+ void
+ ObjectMemoryGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+ {
+ }
// DO NOT EDIT NEXT FUNCTION
- std::string ObjectMemoryGroupRemoteStateOfferer::GetName()
+ std::string
+ ObjectMemoryGroupRemoteStateOfferer::GetName()
{
return "ObjectMemoryGroupRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
- XMLStateOffererFactoryBasePtr ObjectMemoryGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+ XMLStateOffererFactoryBasePtr
+ ObjectMemoryGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new ObjectMemoryGroupRemoteStateOfferer(reader));
}
-}
+} // namespace armarx::ObjectMemoryGroup
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h
index 882627aec09b55c16b4b0f07fe313c0138392368..a66d9df4b5e6c80310a0227cea8c694b90ccd22a 100644
--- a/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/ObjectMemoryGroupRemoteStateOfferer.h
@@ -1,12 +1,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "ObjectMemoryGroupStatechartContext.generated.h"
namespace armarx::ObjectMemoryGroup
{
class ObjectMemoryGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < ObjectMemoryGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ ObjectMemoryGroupStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
ObjectMemoryGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -20,7 +22,5 @@ namespace armarx::ObjectMemoryGroup
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
+} // namespace armarx::ObjectMemoryGroup
diff --git a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h
index 4390865434749d591d6082df003c55e3b8560cd6..883a82d1e8928c8e541b979909ee7b8ec0216244 100644
--- a/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h
+++ b/source/RobotAPI/statecharts/ObjectMemoryGroup/RequestObjects.h
@@ -4,8 +4,7 @@
namespace armarx::ObjectMemoryGroup
{
- class RequestObjects :
- public RequestObjectsGeneratedBase < RequestObjects >
+ class RequestObjects : public RequestObjectsGeneratedBase<RequestObjects>
{
public:
RequestObjects(const XMLStateConstructorParams& stateData);
@@ -24,4 +23,4 @@ namespace armarx::ObjectMemoryGroup
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
+} // namespace armarx::ObjectMemoryGroup
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.cpp
index a016ee99ca74581045089b05f3152589586d442c..23dc4afd5646caaf4cda79ce727eb6a1cac88fe5 100644
--- a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.cpp
@@ -26,41 +26,42 @@ using namespace armarx;
using namespace OrientedTactileSensorGroup;
// DO NOT EDIT NEXT LINE
-OrientedTactileSensorGroupRemoteStateOfferer::SubClassRegistry OrientedTactileSensorGroupRemoteStateOfferer::Registry(OrientedTactileSensorGroupRemoteStateOfferer::GetName(), &OrientedTactileSensorGroupRemoteStateOfferer::CreateInstance);
-
-
-
-OrientedTactileSensorGroupRemoteStateOfferer::OrientedTactileSensorGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < OrientedTactileSensorGroupStatechartContext > (reader)
+OrientedTactileSensorGroupRemoteStateOfferer::SubClassRegistry
+ OrientedTactileSensorGroupRemoteStateOfferer::Registry(
+ OrientedTactileSensorGroupRemoteStateOfferer::GetName(),
+ &OrientedTactileSensorGroupRemoteStateOfferer::CreateInstance);
+
+OrientedTactileSensorGroupRemoteStateOfferer::OrientedTactileSensorGroupRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<OrientedTactileSensorGroupStatechartContext>(reader)
{
}
-void OrientedTactileSensorGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+void
+OrientedTactileSensorGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
-
}
-void OrientedTactileSensorGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+void
+OrientedTactileSensorGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
-
}
-void OrientedTactileSensorGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+void
+OrientedTactileSensorGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
-
}
// DO NOT EDIT NEXT FUNCTION
-std::string OrientedTactileSensorGroupRemoteStateOfferer::GetName()
+std::string
+OrientedTactileSensorGroupRemoteStateOfferer::GetName()
{
return "OrientedTactileSensorGroupRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateOffererFactoryBasePtr OrientedTactileSensorGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+XMLStateOffererFactoryBasePtr
+OrientedTactileSensorGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new OrientedTactileSensorGroupRemoteStateOfferer(reader));
}
-
-
-
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h
index 6d132d9bc0588d832938cb565f73c68160ad8ab6..f7dccf7420338d9dc02ecf1d70f434cf04967882 100644
--- a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorGroupRemoteStateOfferer.h
@@ -23,12 +23,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "OrientedTactileSensorGroupStatechartContext.generated.h"
namespace armarx::OrientedTactileSensorGroup
{
class OrientedTactileSensorGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < OrientedTactileSensorGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ OrientedTactileSensorGroupStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
OrientedTactileSensorGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -42,7 +44,5 @@ namespace armarx::OrientedTactileSensorGroup
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
+} // namespace armarx::OrientedTactileSensorGroup
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.cpp b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.cpp
index ff18ceb1d2d9fed9466b7a6cdf32057cc24d99d1..402fe7f45b77fea5859753063a2953884757bfdc 100644
--- a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.cpp
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.cpp
@@ -26,11 +26,12 @@ using namespace armarx;
using namespace OrientedTactileSensorGroup;
// DO NOT EDIT NEXT LINE
-OrientedTactileSensorTest::SubClassRegistry OrientedTactileSensorTest::Registry(OrientedTactileSensorTest::GetName(), &OrientedTactileSensorTest::CreateInstance);
+OrientedTactileSensorTest::SubClassRegistry
+ OrientedTactileSensorTest::Registry(OrientedTactileSensorTest::GetName(),
+ &OrientedTactileSensorTest::CreateInstance);
-
-
-void OrientedTactileSensorTest::onEnter()
+void
+OrientedTactileSensorTest::onEnter()
{
//OrientedTactileSensorGroupStatechartContext* context = getContext<OrientedTactileSensorGroupStatechartContext>();
//HapticUnitObserverInterfacePrx hapticObserver = context->getHapticObserver();
@@ -59,16 +60,16 @@ void OrientedTactileSensorTest::onEnter()
// // execution time should be short (<100ms)
//}
-void OrientedTactileSensorTest::onExit()
+void
+OrientedTactileSensorTest::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr OrientedTactileSensorTest::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+OrientedTactileSensorTest::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new OrientedTactileSensorTest(stateData));
}
-
diff --git a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.h b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.h
index b9d5a2c0221770c01d73ebf206eaa86e4acc93ab..b2cce8924c7808edcddc23f8a2c27e9f1400a9e0 100644
--- a/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.h
+++ b/source/RobotAPI/statecharts/OrientedTactileSensorGroup/OrientedTactileSensorTest.h
@@ -27,11 +27,12 @@
namespace armarx::OrientedTactileSensorGroup
{
class OrientedTactileSensorTest :
- public OrientedTactileSensorTestGeneratedBase <OrientedTactileSensorTest>
+ public OrientedTactileSensorTestGeneratedBase<OrientedTactileSensorTest>
{
public:
- OrientedTactileSensorTest(const XMLStateConstructorParams& stateData):
- XMLStateTemplate <OrientedTactileSensorTest> (stateData), OrientedTactileSensorTestGeneratedBase <OrientedTactileSensorTest> (stateData)
+ OrientedTactileSensorTest(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<OrientedTactileSensorTest>(stateData),
+ OrientedTactileSensorTestGeneratedBase<OrientedTactileSensorTest>(stateData)
{
}
@@ -49,4 +50,4 @@ namespace armarx::OrientedTactileSensorGroup
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
+} // namespace armarx::OrientedTactileSensorGroup
diff --git a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.cpp b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.cpp
index f493364297a98a1e6a158904b5646ef8f3cc19fa..9c02d5a659d62e48871752f7d138a791acd3a776 100644
--- a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.cpp
+++ b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.cpp
@@ -22,37 +22,40 @@
#include "CyberGloveProsthesisControl.h"
-#include <RobotAPI/components/KITHandUnit/KITHandUnit.h>
+#include <chrono>
+#include <filesystem>
+#include <fstream>
+#include <iostream>
+
+#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
-#include <VirtualRobot/math/Helpers.h>
-#include <RobotAPI/components/DebugDrawer/DebugDrawerHelper.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include <chrono>
-#include <iostream>
-#include <fstream>
-#include <filesystem>
+#include <RobotAPI/components/DebugDrawer/DebugDrawerHelper.h>
+#include <RobotAPI/components/KITHandUnit/KITHandUnit.h>
using namespace armarx;
using namespace ProsthesisKinestheticTeachIn;
// DO NOT EDIT NEXT LINE
-CyberGloveProsthesisControl::SubClassRegistry CyberGloveProsthesisControl::Registry(CyberGloveProsthesisControl::GetName(), &CyberGloveProsthesisControl::CreateInstance);
+CyberGloveProsthesisControl::SubClassRegistry
+ CyberGloveProsthesisControl::Registry(CyberGloveProsthesisControl::GetName(),
+ &CyberGloveProsthesisControl::CreateInstance);
-
-
-void CyberGloveProsthesisControl::onEnter()
+void
+CyberGloveProsthesisControl::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
ARMARX_IMPORTANT << "onEnter";
}
-void CyberGloveProsthesisControl::shapeHand(float fingers, float thumb)
+void
+CyberGloveProsthesisControl::shapeHand(float fingers, float thumb)
{
getHandUnit()->setJointAngles({{"Fingers", fingers}, {"Thumb", thumb}});
}
@@ -91,7 +94,8 @@ void CyberGloveProsthesisControl::shapeHand(float fingers, float thumb)
//};
-void CyberGloveProsthesisControl::run()
+void
+CyberGloveProsthesisControl::run()
{
// put your user code for the execution-phase here
// runs in seperate thread, thus can do complex operations
@@ -104,18 +108,20 @@ void CyberGloveProsthesisControl::run()
// ProsthesisPositionHelperPtr prosthesisPositionHelper;
RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
- rootLayoutBuilder
- .addChild(RemoteGui::makeLabel("message"))
- .addChild(RemoteGui::makeLabel("status").value("status"))
- .addChild(RemoteGui::makeLabel("instruction"))
- .addChild(RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeButton("calibrate_cyber_glove").label("Calibrate Cyber Glove"))
- .addChild(RemoteGui::makeButton("start_recording").label("Start Recording"))
- .addChild(RemoteGui::makeButton("stop_recording").label("Stop Recording"))
- .addChild(RemoteGui::makeButton("set_latest_timestamp").label("Set Latest Timestamp")))
- .addChild(RemoteGui::makeLineEdit("file_name").value("Set file name"))
- //.addChild(RemoteGui::makeLabel("longMessage").value("test"))
- .addChild(new RemoteGui::VSpacer());
+ rootLayoutBuilder.addChild(RemoteGui::makeLabel("message"))
+ .addChild(RemoteGui::makeLabel("status").value("status"))
+ .addChild(RemoteGui::makeLabel("instruction"))
+ .addChild(
+ RemoteGui::makeHBoxLayout()
+ .addChild(
+ RemoteGui::makeButton("calibrate_cyber_glove").label("Calibrate Cyber Glove"))
+ .addChild(RemoteGui::makeButton("start_recording").label("Start Recording"))
+ .addChild(RemoteGui::makeButton("stop_recording").label("Stop Recording"))
+ .addChild(
+ RemoteGui::makeButton("set_latest_timestamp").label("Set Latest Timestamp")))
+ .addChild(RemoteGui::makeLineEdit("file_name").value("Set file name"))
+ //.addChild(RemoteGui::makeLabel("longMessage").value("test"))
+ .addChild(new RemoteGui::VSpacer());
getRemoteGui()->createTab("CyberGloveProsthesisControl", rootLayoutBuilder);
RemoteGui::TabProxy guiTab = RemoteGui::TabProxy(getRemoteGui(), "CyberGloveProsthesisControl");
@@ -157,7 +163,8 @@ void CyberGloveProsthesisControl::run()
guiTab.receiveUpdates();
int calibrateCyberGloveClickCount = guiTab.getValue<int>("calibrate_cyber_glove").get();
- bool calibrateCyberGloveButtonWasClicked = calibrateCyberGloveClickCount > lastClickCountCalibrateCyberGlove;
+ bool calibrateCyberGloveButtonWasClicked =
+ calibrateCyberGloveClickCount > lastClickCountCalibrateCyberGlove;
lastClickCountCalibrateCyberGlove = calibrateCyberGloveClickCount;
int startRecordingClickCount = guiTab.getValue<int>("start_recording").get();
@@ -169,7 +176,8 @@ void CyberGloveProsthesisControl::run()
lastClickCountRecordOff = stopRecordingClickCount;
int setLatestTimestampClickCount = guiTab.getValue<int>("set_latest_timestamp").get();
- bool setLatestTimeStampButtonClicked = setLatestTimestampClickCount > lastClickCountLatestTimestamp;
+ bool setLatestTimeStampButtonClicked =
+ setLatestTimestampClickCount > lastClickCountLatestTimestamp;
lastClickCountLatestTimestamp = setLatestTimestampClickCount;
ARMARX_IMPORTANT << "Timestamp click count: " << setLatestTimestampClickCount;
@@ -178,14 +186,10 @@ void CyberGloveProsthesisControl::run()
float thumbValue = 0;
float indexValue = 0;
- auto calcThumbSum = [](const CyberGloveValues & v)
- {
- return v.thumbAbd + v.thumbCMC + v.thumbIP + v.thumbMCP;
- };
- auto calcIndexSum = [](const CyberGloveValues & v)
- {
- return v.indexDIP + v.indexMCP + v.indexPIP;
- };
+ auto calcThumbSum = [](const CyberGloveValues& v)
+ { return v.thumbAbd + v.thumbCMC + v.thumbIP + v.thumbMCP; };
+ auto calcIndexSum = [](const CyberGloveValues& v)
+ { return v.indexDIP + v.indexMCP + v.indexPIP; };
if (setLatestTimeStampButtonClicked)
{
@@ -193,12 +197,14 @@ void CyberGloveProsthesisControl::run()
{
timestamp = latest_timestamp;
//guiTab.getValue<std::string>("longMessage").set("Resume latest timestamp. Latest timestamp is " + latest_timestamp);
- getMessageDisplay()->setMessage("Resume latest timestamp", "Latest timestamp is " + latest_timestamp);
+ getMessageDisplay()->setMessage("Resume latest timestamp",
+ "Latest timestamp is " + latest_timestamp);
}
else
{
//guiTab.getValue<std::string>("longMessage").set("No timestamp stored.");
- getMessageDisplay()->setMessage("No timestamp stored.", "Take a point cloud to generate a timestamp.");
+ getMessageDisplay()->setMessage("No timestamp stored.",
+ "Take a point cloud to generate a timestamp.");
}
}
@@ -259,8 +265,16 @@ void CyberGloveProsthesisControl::run()
if (currentPhase == PhaseType::TeachIn)
{
guiTab.getValue<std::string>("message").set("Teach-In!");
- thumbValue = math::Helpers::Clamp(0, 1, math::Helpers::ILerp(calcThumbSum(openValues), calcThumbSum(closedValues), calcThumbSum(currentValues)));
- indexValue = math::Helpers::Clamp(0, 1, math::Helpers::ILerp(calcIndexSum(openValues), calcIndexSum(closedValues), calcIndexSum(currentValues)));
+ thumbValue = math::Helpers::Clamp(0,
+ 1,
+ math::Helpers::ILerp(calcThumbSum(openValues),
+ calcThumbSum(closedValues),
+ calcThumbSum(currentValues)));
+ indexValue = math::Helpers::Clamp(0,
+ 1,
+ math::Helpers::ILerp(calcIndexSum(openValues),
+ calcIndexSum(closedValues),
+ calcIndexSum(currentValues)));
if (!std::isfinite(thumbValue))
{
@@ -285,9 +299,11 @@ void CyberGloveProsthesisControl::run()
{
end = std::chrono::system_clock::now();
- double elapsed_milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>
- (end - start).count();
- logger << elapsed_milliseconds << "," << indexValue << "," << thumbValue << "," << handMotorValues["Fingers"] << "," << handMotorValues["Thumb"] << std::endl;
+ double elapsed_milliseconds =
+ std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
+ logger << elapsed_milliseconds << "," << indexValue << "," << thumbValue << ","
+ << handMotorValues["Fingers"] << "," << handMotorValues["Thumb"]
+ << std::endl;
// if (storeCalibrationValuesForCyberGlove)
// {
@@ -305,8 +321,10 @@ void CyberGloveProsthesisControl::run()
{
std::stringstream ss;
- ss << "index: " << currentValues.indexMCP << " " << currentValues.indexPIP << " " << currentValues.indexDIP << "\n";
- ss << "thumb: " << currentValues.thumbMCP << " " << currentValues.thumbIP << " " << currentValues.thumbCMC << " " << currentValues.thumbAbd << "\n";
+ ss << "index: " << currentValues.indexMCP << " " << currentValues.indexPIP << " "
+ << currentValues.indexDIP << "\n";
+ ss << "thumb: " << currentValues.thumbMCP << " " << currentValues.thumbIP << " "
+ << currentValues.thumbCMC << " " << currentValues.thumbAbd << "\n";
ss << std::floor(indexValue * 100) << "% " << std::floor(thumbValue * 100) << "%\n";
guiTab.getValue<std::string>("status").set(ss.str());
}
@@ -330,15 +348,16 @@ void CyberGloveProsthesisControl::run()
// // execution time should be short (<100ms)
//}
-void CyberGloveProsthesisControl::onExit()
+void
+CyberGloveProsthesisControl::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr CyberGloveProsthesisControl::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+CyberGloveProsthesisControl::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new CyberGloveProsthesisControl(stateData));
}
diff --git a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.h b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.h
index dccc99d23b201d15e45c6e9b4dac79fb55e03bdb..44e58dec287eaa1977c4be21382818b9e9bdcb12 100644
--- a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.h
+++ b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.h
@@ -22,16 +22,18 @@
#pragma once
#include <RobotAPI/statecharts/ProsthesisKinestheticTeachIn/CyberGloveProsthesisControl.generated.h>
+
//#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
namespace armarx::ProsthesisKinestheticTeachIn
{
class CyberGloveProsthesisControl :
- public CyberGloveProsthesisControlGeneratedBase < CyberGloveProsthesisControl >
+ public CyberGloveProsthesisControlGeneratedBase<CyberGloveProsthesisControl>
{
public:
- CyberGloveProsthesisControl(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < CyberGloveProsthesisControl > (stateData), CyberGloveProsthesisControlGeneratedBase < CyberGloveProsthesisControl > (stateData)
+ CyberGloveProsthesisControl(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<CyberGloveProsthesisControl>(stateData),
+ CyberGloveProsthesisControlGeneratedBase<CyberGloveProsthesisControl>(stateData)
{
}
@@ -61,10 +63,5 @@ namespace armarx::ProsthesisKinestheticTeachIn
void shapeHand(float fingers, float thumb);
//SimpleJsonLoggerEntry getRawJointValuesForCalibration(CyberGloveValues& openValues, CyberGloveValues& closedValues);
//void addCurrentRawJointValues(CyberGloveValues& currentValues, SimpleJsonLoggerEntry& e);
-
-
};
-}
-
-
-
+} // namespace armarx::ProsthesisKinestheticTeachIn
diff --git a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.cpp
index 13f28136e7aa99613fabb234c8a8fecb017e85d1..629f67fd50a9b5f2ecae5237655c833bff761f7c 100644
--- a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.cpp
@@ -26,41 +26,43 @@ using namespace armarx;
using namespace ProsthesisKinestheticTeachIn;
// DO NOT EDIT NEXT LINE
-ProsthesisKinestheticTeachInRemoteStateOfferer::SubClassRegistry ProsthesisKinestheticTeachInRemoteStateOfferer::Registry(ProsthesisKinestheticTeachInRemoteStateOfferer::GetName(), &ProsthesisKinestheticTeachInRemoteStateOfferer::CreateInstance);
-
-
-
-ProsthesisKinestheticTeachInRemoteStateOfferer::ProsthesisKinestheticTeachInRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < ProsthesisKinestheticTeachInStatechartContext > (reader)
+ProsthesisKinestheticTeachInRemoteStateOfferer::SubClassRegistry
+ ProsthesisKinestheticTeachInRemoteStateOfferer::Registry(
+ ProsthesisKinestheticTeachInRemoteStateOfferer::GetName(),
+ &ProsthesisKinestheticTeachInRemoteStateOfferer::CreateInstance);
+
+ProsthesisKinestheticTeachInRemoteStateOfferer::ProsthesisKinestheticTeachInRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<ProsthesisKinestheticTeachInStatechartContext>(reader)
{
}
-void ProsthesisKinestheticTeachInRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+void
+ProsthesisKinestheticTeachInRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
-
}
-void ProsthesisKinestheticTeachInRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+void
+ProsthesisKinestheticTeachInRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
-
}
-void ProsthesisKinestheticTeachInRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+void
+ProsthesisKinestheticTeachInRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
-
}
// DO NOT EDIT NEXT FUNCTION
-std::string ProsthesisKinestheticTeachInRemoteStateOfferer::GetName()
+std::string
+ProsthesisKinestheticTeachInRemoteStateOfferer::GetName()
{
return "ProsthesisKinestheticTeachInRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateOffererFactoryBasePtr ProsthesisKinestheticTeachInRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+XMLStateOffererFactoryBasePtr
+ProsthesisKinestheticTeachInRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
- return XMLStateOffererFactoryBasePtr(new ProsthesisKinestheticTeachInRemoteStateOfferer(reader));
+ return XMLStateOffererFactoryBasePtr(
+ new ProsthesisKinestheticTeachInRemoteStateOfferer(reader));
}
-
-
-
diff --git a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.h b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.h
index 2a6b6114cca4cc7ea3a3a9879a2d555870006d83..4c6eb6da23eef6566cc03eaddce011de32df5411 100644
--- a/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/ProsthesisKinestheticTeachIn/ProsthesisKinestheticTeachInRemoteStateOfferer.h
@@ -23,12 +23,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "ProsthesisKinestheticTeachInStatechartContext.generated.h"
namespace armarx::ProsthesisKinestheticTeachIn
{
class ProsthesisKinestheticTeachInRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < ProsthesisKinestheticTeachInStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ ProsthesisKinestheticTeachInStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
ProsthesisKinestheticTeachInRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -42,9 +44,5 @@ namespace armarx::ProsthesisKinestheticTeachIn
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
-
-
+} // namespace armarx::ProsthesisKinestheticTeachIn
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.cpp
index dc941cd5a065bb5f9aecdd38f9fc4ff8bb245a24..bd42e556bd5d2f366249f17e53586d38f6ad8dd3 100644
--- a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.cpp
@@ -26,41 +26,42 @@ using namespace armarx;
using namespace RobotNameHelperTestGroup;
// DO NOT EDIT NEXT LINE
-RobotNameHelperTestGroupRemoteStateOfferer::SubClassRegistry RobotNameHelperTestGroupRemoteStateOfferer::Registry(RobotNameHelperTestGroupRemoteStateOfferer::GetName(), &RobotNameHelperTestGroupRemoteStateOfferer::CreateInstance);
-
-
-
-RobotNameHelperTestGroupRemoteStateOfferer::RobotNameHelperTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < RobotNameHelperTestGroupStatechartContext > (reader)
+RobotNameHelperTestGroupRemoteStateOfferer::SubClassRegistry
+ RobotNameHelperTestGroupRemoteStateOfferer::Registry(
+ RobotNameHelperTestGroupRemoteStateOfferer::GetName(),
+ &RobotNameHelperTestGroupRemoteStateOfferer::CreateInstance);
+
+RobotNameHelperTestGroupRemoteStateOfferer::RobotNameHelperTestGroupRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<RobotNameHelperTestGroupStatechartContext>(reader)
{
}
-void RobotNameHelperTestGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+void
+RobotNameHelperTestGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
-
}
-void RobotNameHelperTestGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+void
+RobotNameHelperTestGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
-
}
-void RobotNameHelperTestGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+void
+RobotNameHelperTestGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
-
}
// DO NOT EDIT NEXT FUNCTION
-std::string RobotNameHelperTestGroupRemoteStateOfferer::GetName()
+std::string
+RobotNameHelperTestGroupRemoteStateOfferer::GetName()
{
return "RobotNameHelperTestGroupRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateOffererFactoryBasePtr RobotNameHelperTestGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+XMLStateOffererFactoryBasePtr
+RobotNameHelperTestGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new RobotNameHelperTestGroupRemoteStateOfferer(reader));
}
-
-
-
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h
index 3553f8e4b79e8dba84fde0e90dadaae8da12a821..1f5fe1a9f4dedfb8aaea121a15375faff5714345 100644
--- a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h
@@ -23,12 +23,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "RobotNameHelperTestGroupStatechartContext.generated.h"
namespace armarx::RobotNameHelperTestGroup
{
class RobotNameHelperTestGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < RobotNameHelperTestGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ RobotNameHelperTestGroupStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
RobotNameHelperTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -42,9 +44,5 @@ namespace armarx::RobotNameHelperTestGroup
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
-
-
+} // namespace armarx::RobotNameHelperTestGroup
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp
index 95ee1853bb68ffe504777330e996b1c5078ace25..0ab22f39ab15c58e52ba7a06e8aef0e38f088a08 100644
--- a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp
@@ -21,6 +21,7 @@
*/
#include "TestGetNames.h"
+
#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
@@ -32,17 +33,18 @@ using namespace armarx;
using namespace RobotNameHelperTestGroup;
// DO NOT EDIT NEXT LINE
-TestGetNames::SubClassRegistry TestGetNames::Registry(TestGetNames::GetName(), &TestGetNames::CreateInstance);
-
+TestGetNames::SubClassRegistry TestGetNames::Registry(TestGetNames::GetName(),
+ &TestGetNames::CreateInstance);
-
-void TestGetNames::onEnter()
+void
+TestGetNames::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
}
-void TestGetNames::run()
+void
+TestGetNames::run()
{
ARMARX_IMPORTANT << "Profiles: " << getRobotNameHelper()->getProfiles();
auto side = [&](const auto s)
@@ -172,7 +174,8 @@ void TestGetNames::run()
}
try
{
- ARMARX_IMPORTANT << "TorsoKinematicChain: " << robarm.getTorsoKinematicChain()->getName();
+ ARMARX_IMPORTANT << "TorsoKinematicChain: "
+ << robarm.getTorsoKinematicChain()->getName();
}
catch (...)
{
@@ -197,15 +200,16 @@ void TestGetNames::run()
// // execution time should be short (<100ms)
//}
-void TestGetNames::onExit()
+void
+TestGetNames::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr TestGetNames::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+TestGetNames::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new TestGetNames(stateData));
}
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h
index a273d15b5992ff69bc0fd12397607e88e9b4a730..2540c61376548ff5951ce4f65ad0736c5ad47e6a 100644
--- a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h
@@ -25,12 +25,12 @@
namespace armarx::RobotNameHelperTestGroup
{
- class TestGetNames :
- public TestGetNamesGeneratedBase < TestGetNames >
+ class TestGetNames : public TestGetNamesGeneratedBase<TestGetNames>
{
public:
- TestGetNames(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < TestGetNames > (stateData), TestGetNamesGeneratedBase < TestGetNames > (stateData)
+ TestGetNames(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<TestGetNames>(stateData),
+ TestGetNamesGeneratedBase<TestGetNames>(stateData)
{
}
@@ -48,7 +48,4 @@ namespace armarx::RobotNameHelperTestGroup
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
-
-
-
+} // namespace armarx::RobotNameHelperTestGroup
diff --git a/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.cpp
index cfb969f49829d31e16e451f510c4e6c54a94820f..7fa31c5a2901b1090e5e71e02fd64ae290803b60 100644
--- a/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.cpp
@@ -26,41 +26,42 @@ using namespace armarx;
using namespace SpeechObserverTestGroup;
// DO NOT EDIT NEXT LINE
-SpeechObserverTestGroupRemoteStateOfferer::SubClassRegistry SpeechObserverTestGroupRemoteStateOfferer::Registry(SpeechObserverTestGroupRemoteStateOfferer::GetName(), &SpeechObserverTestGroupRemoteStateOfferer::CreateInstance);
-
-
-
-SpeechObserverTestGroupRemoteStateOfferer::SpeechObserverTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < SpeechObserverTestGroupStatechartContext > (reader)
+SpeechObserverTestGroupRemoteStateOfferer::SubClassRegistry
+ SpeechObserverTestGroupRemoteStateOfferer::Registry(
+ SpeechObserverTestGroupRemoteStateOfferer::GetName(),
+ &SpeechObserverTestGroupRemoteStateOfferer::CreateInstance);
+
+SpeechObserverTestGroupRemoteStateOfferer::SpeechObserverTestGroupRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<SpeechObserverTestGroupStatechartContext>(reader)
{
}
-void SpeechObserverTestGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+void
+SpeechObserverTestGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
-
}
-void SpeechObserverTestGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+void
+SpeechObserverTestGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
-
}
-void SpeechObserverTestGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+void
+SpeechObserverTestGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
-
}
// DO NOT EDIT NEXT FUNCTION
-std::string SpeechObserverTestGroupRemoteStateOfferer::GetName()
+std::string
+SpeechObserverTestGroupRemoteStateOfferer::GetName()
{
return "SpeechObserverTestGroupRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateOffererFactoryBasePtr SpeechObserverTestGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+XMLStateOffererFactoryBasePtr
+SpeechObserverTestGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new SpeechObserverTestGroupRemoteStateOfferer(reader));
}
-
-
-
diff --git a/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.h
index cc843a7395d1d270533b4a0c2f7f9b58e5bf695d..6008828d34dba4db875fe08ad018947e49b5a3d2 100644
--- a/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/SpeechObserverTestGroup/SpeechObserverTestGroupRemoteStateOfferer.h
@@ -23,12 +23,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "SpeechObserverTestGroupStatechartContext.generated.h"
namespace armarx::SpeechObserverTestGroup
{
class SpeechObserverTestGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < SpeechObserverTestGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ SpeechObserverTestGroupStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
SpeechObserverTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -42,9 +44,5 @@ namespace armarx::SpeechObserverTestGroup
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
-
-
+} // namespace armarx::SpeechObserverTestGroup
diff --git a/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.cpp b/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.cpp
index ab3e967a68208303b964cdb583566a15aeffbe0e..e449044953306479b9fd27dba84c5c67a2e032fa 100644
--- a/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.cpp
+++ b/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.cpp
@@ -29,22 +29,24 @@ using namespace armarx;
using namespace SpeechObserverTestGroup;
// DO NOT EDIT NEXT LINE
-TestTextToSpeech::SubClassRegistry TestTextToSpeech::Registry(TestTextToSpeech::GetName(), &TestTextToSpeech::CreateInstance);
+TestTextToSpeech::SubClassRegistry TestTextToSpeech::Registry(TestTextToSpeech::GetName(),
+ &TestTextToSpeech::CreateInstance);
-
-
-void TestTextToSpeech::onEnter()
+void
+TestTextToSpeech::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
}
-void TestTextToSpeech::waitForSpeechFinished()
+void
+TestTextToSpeech::waitForSpeechFinished()
{
TimeUtil::SleepMS(20);
while (true)
{
- if (getSpeechObserver()->getDatafieldByName("TextToSpeech", "State")->getString() == "FinishedSpeaking")
+ if (getSpeechObserver()->getDatafieldByName("TextToSpeech", "State")->getString() ==
+ "FinishedSpeaking")
{
break;
}
@@ -52,7 +54,8 @@ void TestTextToSpeech::waitForSpeechFinished()
}
}
-void TestTextToSpeech::run()
+void
+TestTextToSpeech::run()
{
getTextToSpeech()->reportText("Hello!");
@@ -73,16 +76,16 @@ void TestTextToSpeech::run()
// // execution time should be short (<100ms)
//}
-void TestTextToSpeech::onExit()
+void
+TestTextToSpeech::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr TestTextToSpeech::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+TestTextToSpeech::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new TestTextToSpeech(stateData));
}
-
diff --git a/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.h b/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.h
index 8488ecc1198d49a1f1fe6c20aa00e75556f945b4..655dccf89ef92514ce93e648634b50bed0865823 100644
--- a/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.h
+++ b/source/RobotAPI/statecharts/SpeechObserverTestGroup/TestTextToSpeech.h
@@ -25,12 +25,12 @@
namespace armarx::SpeechObserverTestGroup
{
- class TestTextToSpeech :
- public TestTextToSpeechGeneratedBase < TestTextToSpeech >
+ class TestTextToSpeech : public TestTextToSpeechGeneratedBase<TestTextToSpeech>
{
public:
- TestTextToSpeech(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < TestTextToSpeech > (stateData), TestTextToSpeechGeneratedBase < TestTextToSpeech > (stateData)
+ TestTextToSpeech(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<TestTextToSpeech>(stateData),
+ TestTextToSpeechGeneratedBase<TestTextToSpeech>(stateData)
{
}
@@ -49,4 +49,4 @@ namespace armarx::SpeechObserverTestGroup
// if classmember are neccessary nonetheless, reset them in onEnter
void waitForSpeechFinished();
};
-}
+} // namespace armarx::SpeechObserverTestGroup
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.cpp
index 1c27e666982f49821e7049c2852acb33c78dc647..040260664a320386a3983cb0798328e499c40740 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.cpp
@@ -26,41 +26,42 @@ using namespace armarx;
using namespace StatechartExecutionGroup;
// DO NOT EDIT NEXT LINE
-StatechartExecutionGroupRemoteStateOfferer::SubClassRegistry StatechartExecutionGroupRemoteStateOfferer::Registry(StatechartExecutionGroupRemoteStateOfferer::GetName(), &StatechartExecutionGroupRemoteStateOfferer::CreateInstance);
-
-
-
-StatechartExecutionGroupRemoteStateOfferer::StatechartExecutionGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < StatechartExecutionGroupStatechartContext > (reader)
+StatechartExecutionGroupRemoteStateOfferer::SubClassRegistry
+ StatechartExecutionGroupRemoteStateOfferer::Registry(
+ StatechartExecutionGroupRemoteStateOfferer::GetName(),
+ &StatechartExecutionGroupRemoteStateOfferer::CreateInstance);
+
+StatechartExecutionGroupRemoteStateOfferer::StatechartExecutionGroupRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<StatechartExecutionGroupStatechartContext>(reader)
{
}
-void StatechartExecutionGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+void
+StatechartExecutionGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
-
}
-void StatechartExecutionGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+void
+StatechartExecutionGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
-
}
-void StatechartExecutionGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+void
+StatechartExecutionGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
-
}
// DO NOT EDIT NEXT FUNCTION
-std::string StatechartExecutionGroupRemoteStateOfferer::GetName()
+std::string
+StatechartExecutionGroupRemoteStateOfferer::GetName()
{
return "StatechartExecutionGroupRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateOffererFactoryBasePtr StatechartExecutionGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+XMLStateOffererFactoryBasePtr
+StatechartExecutionGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new StatechartExecutionGroupRemoteStateOfferer(reader));
}
-
-
-
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.h
index 030cac2fed6160d3e0194b19a8da5b2169964323..f644f44886d09804f966f2a8c5141d77b5af36a2 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/StatechartExecutionGroupRemoteStateOfferer.h
@@ -23,12 +23,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "StatechartExecutionGroupStatechartContext.generated.h"
namespace armarx::StatechartExecutionGroup
{
class StatechartExecutionGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < StatechartExecutionGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ StatechartExecutionGroupStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
StatechartExecutionGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -42,7 +44,5 @@ namespace armarx::StatechartExecutionGroup
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
+} // namespace armarx::StatechartExecutionGroup
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.cpp b/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.cpp
index 96b7ec7f53fc2d798a0ede27c789c48c203cd5b8..97e44ed702d6961f0e3d14daac488814f33cf640 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.cpp
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.cpp
@@ -31,11 +31,12 @@ using namespace armarx;
using namespace StatechartExecutionGroup;
// DO NOT EDIT NEXT LINE
-TestStateForStatechartExecution::SubClassRegistry TestStateForStatechartExecution::Registry(TestStateForStatechartExecution::GetName(), &TestStateForStatechartExecution::CreateInstance);
+TestStateForStatechartExecution::SubClassRegistry
+ TestStateForStatechartExecution::Registry(TestStateForStatechartExecution::GetName(),
+ &TestStateForStatechartExecution::CreateInstance);
-
-
-void TestStateForStatechartExecution::onEnter()
+void
+TestStateForStatechartExecution::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
@@ -65,14 +66,16 @@ void TestStateForStatechartExecution::onEnter()
out.setOutputPose(p);
}
-void TestStateForStatechartExecution::onBreak()
+void
+TestStateForStatechartExecution::onBreak()
{
// put your user code for the breaking point here
// execution time should be short (<100ms)
ARMARX_INFO << "MainState broke";
}
-void TestStateForStatechartExecution::onExit()
+void
+TestStateForStatechartExecution::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
@@ -80,10 +83,9 @@ void TestStateForStatechartExecution::onExit()
ARMARX_INFO << "MainState exited";
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr TestStateForStatechartExecution::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+TestStateForStatechartExecution::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new TestStateForStatechartExecution(stateData));
}
-
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.h b/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.h
index 56d318155d19579dbe6d7e9e633812697c9f1d1e..f6ecb53d90572b82e9cd3631d10d8f4260bc3eca 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.h
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestStateForStatechartExecution.h
@@ -26,11 +26,12 @@
namespace armarx::StatechartExecutionGroup
{
class TestStateForStatechartExecution :
- public TestStateForStatechartExecutionGeneratedBase < TestStateForStatechartExecution >
+ public TestStateForStatechartExecutionGeneratedBase<TestStateForStatechartExecution>
{
public:
- TestStateForStatechartExecution(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < TestStateForStatechartExecution > (stateData), TestStateForStatechartExecutionGeneratedBase < TestStateForStatechartExecution > (stateData)
+ TestStateForStatechartExecution(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<TestStateForStatechartExecution>(stateData),
+ TestStateForStatechartExecutionGeneratedBase<TestStateForStatechartExecution>(stateData)
{
}
@@ -48,4 +49,4 @@ namespace armarx::StatechartExecutionGroup
// if classmember are neccessary nonetheless, reset them in onEnter
int i = 0;
};
-}
+} // namespace armarx::StatechartExecutionGroup
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestSubstate1.cpp b/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestSubstate1.cpp
index f62a9624a7b9afc1cf24f9bb87da0af5616de61d..cfe8f7afe5cf0df9ae7bac11fe184d061a63d9c2 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestSubstate1.cpp
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestSubstate1.cpp
@@ -28,16 +28,19 @@
namespace armarx::StatechartExecutionGroup
{
// DO NOT EDIT NEXT LINE
- TestSubstate1::SubClassRegistry TestSubstate1::Registry(TestSubstate1::GetName(), &TestSubstate1::CreateInstance);
+ TestSubstate1::SubClassRegistry TestSubstate1::Registry(TestSubstate1::GetName(),
+ &TestSubstate1::CreateInstance);
- void TestSubstate1::onEnter()
+ void
+ TestSubstate1::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
ARMARX_INFO << "Substate onEnter";
}
- void TestSubstate1::run()
+ void
+ TestSubstate1::run()
{
int i = in.getInputInt();
@@ -54,24 +57,26 @@ namespace armarx::StatechartExecutionGroup
emitSuccess();
}
- void TestSubstate1::onBreak()
+ void
+ TestSubstate1::onBreak()
{
// put your user code for the breaking point here
// execution time should be short (<100ms)
ARMARX_INFO << "Substate broke";
}
- void TestSubstate1::onExit()
+ void
+ TestSubstate1::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
ARMARX_INFO << "Substate exited";
}
-
// DO NOT EDIT NEXT FUNCTION
- XMLStateFactoryBasePtr TestSubstate1::CreateInstance(XMLStateConstructorParams stateData)
+ XMLStateFactoryBasePtr
+ TestSubstate1::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new TestSubstate1(stateData));
}
-}
+} // namespace armarx::StatechartExecutionGroup
diff --git a/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestSubstate1.h b/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestSubstate1.h
index cabf9b7ebe90c2c06402eb5dbf73cc8d5b27538c..234ad904a725be4f28d5f517bd43e4c592f15612 100644
--- a/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestSubstate1.h
+++ b/source/RobotAPI/statecharts/StatechartExecutionGroup/Testing/TestSubstate1.h
@@ -25,12 +25,12 @@
namespace armarx::StatechartExecutionGroup
{
- class TestSubstate1 :
- public TestSubstate1GeneratedBase < TestSubstate1 >
+ class TestSubstate1 : public TestSubstate1GeneratedBase<TestSubstate1>
{
public:
- TestSubstate1(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < TestSubstate1 > (stateData), TestSubstate1GeneratedBase < TestSubstate1 > (stateData)
+ TestSubstate1(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<TestSubstate1>(stateData),
+ TestSubstate1GeneratedBase<TestSubstate1>(stateData)
{
}
@@ -48,7 +48,4 @@ namespace armarx::StatechartExecutionGroup
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
-
-
-
+} // namespace armarx::StatechartExecutionGroup
diff --git a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.cpp
index 5319b74f811b3c748269392ed9f15adc1ae98c8b..f5571d2490af22e197024aa9eb22e0554e1dd4c3 100644
--- a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.cpp
@@ -28,41 +28,42 @@ using namespace armarx;
using namespace StatechartProfilesTestGroup;
// DO NOT EDIT NEXT LINE
-StatechartProfilesTestGroupRemoteStateOfferer::SubClassRegistry StatechartProfilesTestGroupRemoteStateOfferer::Registry(StatechartProfilesTestGroupRemoteStateOfferer::GetName(), &StatechartProfilesTestGroupRemoteStateOfferer::CreateInstance);
-
-
-
-StatechartProfilesTestGroupRemoteStateOfferer::StatechartProfilesTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < StatechartProfilesTestGroupStatechartContext > (reader)
+StatechartProfilesTestGroupRemoteStateOfferer::SubClassRegistry
+ StatechartProfilesTestGroupRemoteStateOfferer::Registry(
+ StatechartProfilesTestGroupRemoteStateOfferer::GetName(),
+ &StatechartProfilesTestGroupRemoteStateOfferer::CreateInstance);
+
+StatechartProfilesTestGroupRemoteStateOfferer::StatechartProfilesTestGroupRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<StatechartProfilesTestGroupStatechartContext>(reader)
{
}
-void StatechartProfilesTestGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+void
+StatechartProfilesTestGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
-
}
-void StatechartProfilesTestGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+void
+StatechartProfilesTestGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
-
}
-void StatechartProfilesTestGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+void
+StatechartProfilesTestGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
-
}
// DO NOT EDIT NEXT FUNCTION
-std::string StatechartProfilesTestGroupRemoteStateOfferer::GetName()
+std::string
+StatechartProfilesTestGroupRemoteStateOfferer::GetName()
{
return "StatechartProfilesTestGroupRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateOffererFactoryBasePtr StatechartProfilesTestGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+XMLStateOffererFactoryBasePtr
+StatechartProfilesTestGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new StatechartProfilesTestGroupRemoteStateOfferer(reader));
}
-
-
-
diff --git a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.h
index 40d04d84474c78b0d33e67bc9afbaff63145c9f5..87f0d77365e91fa0fffc42ddf170a5b68bf6870e 100644
--- a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupRemoteStateOfferer.h
@@ -25,12 +25,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include <RobotAPI/statecharts/StatechartProfilesTestGroup/StatechartProfilesTestGroupStatechartContext.generated.h>
namespace armarx::StatechartProfilesTestGroup
{
class StatechartProfilesTestGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < StatechartProfilesTestGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ StatechartProfilesTestGroupStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
StatechartProfilesTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -44,7 +46,5 @@ namespace armarx::StatechartProfilesTestGroup
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
+} // namespace armarx::StatechartProfilesTestGroup
diff --git a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp
index 08a05bfdd371bad4071f92ff3530d375578a145a..b2e2c5db5bd613f12e7b85b6e7dce15ef66a624d 100644
--- a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp
+++ b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp
@@ -30,14 +30,13 @@ using namespace StatechartProfilesTestGroup;
// DO NOT EDIT NEXT LINE
TestState::SubClassRegistry TestState::Registry(TestState::GetName(), &TestState::CreateInstance);
-
-
TestState::TestState(const XMLStateConstructorParams& stateData) :
- XMLStateTemplate<TestState>(stateData), TestStateGeneratedBase<TestState>(stateData)
+ XMLStateTemplate<TestState>(stateData), TestStateGeneratedBase<TestState>(stateData)
{
}
-void TestState::onEnter()
+void
+TestState::onEnter()
{
std::string emptyString = in.getEmptyStringTest();
std::string TestParam1 = in.getTestParam1();
@@ -55,7 +54,8 @@ void TestState::onEnter()
ARMARX_IMPORTANT << "All tests passed.";
}
-void TestState::run()
+void
+TestState::run()
{
// put your user code for the execution-phase here
// runs in seperate thread, thus can do complex operations
@@ -66,26 +66,25 @@ void TestState::run()
// {
// // do your calculations
// }
-
}
-void TestState::onBreak()
+void
+TestState::onBreak()
{
// put your user code for the breaking point here
// execution time should be short (<100ms)
}
-void TestState::onExit()
+void
+TestState::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
-
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr TestState::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+TestState::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new TestState(stateData));
}
-
diff --git a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.h b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.h
index adad16ef64da7717f8fa6b5e246af30a947eeedd..898631de3f38304229913f60bc504cb73a24b9ca 100644
--- a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.h
+++ b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.h
@@ -28,8 +28,7 @@
namespace armarx::StatechartProfilesTestGroup
{
- class TestState :
- public TestStateGeneratedBase<TestState>
+ class TestState : public TestStateGeneratedBase<TestState>
{
public:
TestState(const XMLStateConstructorParams& stateData);
@@ -48,4 +47,4 @@ namespace armarx::StatechartProfilesTestGroup
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
+} // namespace armarx::StatechartProfilesTestGroup
diff --git a/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.cpp b/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.cpp
index 03f5198958ca91396b9d6ff6dba6a0f9611b9e11..d420a6c687db373a506c471f2b6691149e2e483a 100644
--- a/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.cpp
+++ b/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.cpp
@@ -23,7 +23,6 @@
#include "PlayTrajectory.h"
#include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
-
#include <RobotAPI/libraries/core/Trajectory.h>
//#include <ArmarXCore/core/time/TimeUtil.h>
@@ -33,11 +32,11 @@ using namespace armarx;
using namespace TrajectoryExecutionCode;
// DO NOT EDIT NEXT LINE
-PlayTrajectory::SubClassRegistry PlayTrajectory::Registry(PlayTrajectory::GetName(), &PlayTrajectory::CreateInstance);
-
-
+PlayTrajectory::SubClassRegistry PlayTrajectory::Registry(PlayTrajectory::GetName(),
+ &PlayTrajectory::CreateInstance);
-void PlayTrajectory::onEnter()
+void
+PlayTrajectory::onEnter()
{
// put your user code for the enter-point here
// execution time should be short (<100ms)
@@ -45,11 +44,14 @@ void PlayTrajectory::onEnter()
cfg->jointNames.push_back("Hip Yaw");
cfg->maxAcceleration = 0.05;
cfg->maxVelocity = 0.3;
- NJointTrajectoryControllerInterfacePrx prx = NJointTrajectoryControllerInterfacePrx::checkedCast(getRobotUnit()->getNJointController("ActorNJointTrajectoryController"));
+ NJointTrajectoryControllerInterfacePrx prx =
+ NJointTrajectoryControllerInterfacePrx::checkedCast(
+ getRobotUnit()->getNJointController("ActorNJointTrajectoryController"));
if (!prx)
{
- prx = NJointTrajectoryControllerInterfacePrx::checkedCast(getRobotUnit()->createNJointController("NJointTrajectoryController", "ActorNJointTrajectoryController", cfg));
-
+ prx = NJointTrajectoryControllerInterfacePrx::checkedCast(
+ getRobotUnit()->createNJointController(
+ "NJointTrajectoryController", "ActorNJointTrajectoryController", cfg));
}
else
{
@@ -84,16 +86,16 @@ void PlayTrajectory::onEnter()
// // execution time should be short (<100ms)
//}
-void PlayTrajectory::onExit()
+void
+PlayTrajectory::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
}
-
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr PlayTrajectory::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+PlayTrajectory::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new PlayTrajectory(stateData));
}
-
diff --git a/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.h b/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.h
index d28777d5f0ec456357a7a7781586207c742c5b6e..02058eeaa6645b20d37b5631b99198f7961cc002 100644
--- a/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.h
+++ b/source/RobotAPI/statecharts/TrajectoryExecutionCode/PlayTrajectory.h
@@ -26,12 +26,12 @@
namespace armarx::TrajectoryExecutionCode
{
- class PlayTrajectory :
- public PlayTrajectoryGeneratedBase < PlayTrajectory >
+ class PlayTrajectory : public PlayTrajectoryGeneratedBase<PlayTrajectory>
{
public:
- PlayTrajectory(const XMLStateConstructorParams& stateData):
- XMLStateTemplate < PlayTrajectory > (stateData), PlayTrajectoryGeneratedBase < PlayTrajectory > (stateData)
+ PlayTrajectory(const XMLStateConstructorParams& stateData) :
+ XMLStateTemplate<PlayTrajectory>(stateData),
+ PlayTrajectoryGeneratedBase<PlayTrajectory>(stateData)
{
}
@@ -49,6 +49,4 @@ namespace armarx::TrajectoryExecutionCode
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
-
-
+} // namespace armarx::TrajectoryExecutionCode
diff --git a/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.cpp
index 465a02d20e688cec8f1c8c7510565e0898117628..f1c03026b1dbc8ffa6ee59998cb80446e4d649a5 100644
--- a/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.cpp
@@ -26,41 +26,42 @@ using namespace armarx;
using namespace TrajectoryExecutionCode;
// DO NOT EDIT NEXT LINE
-TrajectoryExecutionCodeRemoteStateOfferer::SubClassRegistry TrajectoryExecutionCodeRemoteStateOfferer::Registry(TrajectoryExecutionCodeRemoteStateOfferer::GetName(), &TrajectoryExecutionCodeRemoteStateOfferer::CreateInstance);
-
-
-
-TrajectoryExecutionCodeRemoteStateOfferer::TrajectoryExecutionCodeRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < TrajectoryExecutionCodeStatechartContext > (reader)
+TrajectoryExecutionCodeRemoteStateOfferer::SubClassRegistry
+ TrajectoryExecutionCodeRemoteStateOfferer::Registry(
+ TrajectoryExecutionCodeRemoteStateOfferer::GetName(),
+ &TrajectoryExecutionCodeRemoteStateOfferer::CreateInstance);
+
+TrajectoryExecutionCodeRemoteStateOfferer::TrajectoryExecutionCodeRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<TrajectoryExecutionCodeStatechartContext>(reader)
{
}
-void TrajectoryExecutionCodeRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+void
+TrajectoryExecutionCodeRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
-
}
-void TrajectoryExecutionCodeRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+void
+TrajectoryExecutionCodeRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
-
}
-void TrajectoryExecutionCodeRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+void
+TrajectoryExecutionCodeRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
-
}
// DO NOT EDIT NEXT FUNCTION
-std::string TrajectoryExecutionCodeRemoteStateOfferer::GetName()
+std::string
+TrajectoryExecutionCodeRemoteStateOfferer::GetName()
{
return "TrajectoryExecutionCodeRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateOffererFactoryBasePtr TrajectoryExecutionCodeRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+XMLStateOffererFactoryBasePtr
+TrajectoryExecutionCodeRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new TrajectoryExecutionCodeRemoteStateOfferer(reader));
}
-
-
-
diff --git a/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.h b/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.h
index 46df35ec3087d0976bc33b7ccce611cfc347d76f..65ab35e9b0ecb5478f027273504c371873f8369f 100644
--- a/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/TrajectoryExecutionCode/TrajectoryExecutionCodeRemoteStateOfferer.h
@@ -23,12 +23,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "TrajectoryExecutionCodeStatechartContext.generated.h"
namespace armarx::TrajectoryExecutionCode
{
class TrajectoryExecutionCodeRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < TrajectoryExecutionCodeStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ TrajectoryExecutionCodeStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
TrajectoryExecutionCodeRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -42,9 +44,5 @@ namespace armarx::TrajectoryExecutionCode
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
-
-
+} // namespace armarx::TrajectoryExecutionCode
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.cpp
index fd78cf3ffee51fa55a8916f93c6f9e1d9afb0a22..ea80cdcb36204244cd9b4b71b7d24deec6aaaa4e 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.cpp
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.cpp
@@ -28,41 +28,41 @@ using namespace armarx;
using namespace WeissHapticGroup;
// DO NOT EDIT NEXT LINE
-WeissHapticGroupRemoteStateOfferer::SubClassRegistry WeissHapticGroupRemoteStateOfferer::Registry(WeissHapticGroupRemoteStateOfferer::GetName(), &WeissHapticGroupRemoteStateOfferer::CreateInstance);
+WeissHapticGroupRemoteStateOfferer::SubClassRegistry WeissHapticGroupRemoteStateOfferer::Registry(
+ WeissHapticGroupRemoteStateOfferer::GetName(),
+ &WeissHapticGroupRemoteStateOfferer::CreateInstance);
-
-
-WeissHapticGroupRemoteStateOfferer::WeissHapticGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
- XMLRemoteStateOfferer < WeissHapticGroupStatechartContext > (reader)
+WeissHapticGroupRemoteStateOfferer::WeissHapticGroupRemoteStateOfferer(
+ StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer<WeissHapticGroupStatechartContext>(reader)
{
}
-void WeissHapticGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+void
+WeissHapticGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
{
-
}
-void WeissHapticGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+void
+WeissHapticGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
{
-
}
-void WeissHapticGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+void
+WeissHapticGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
{
-
}
// DO NOT EDIT NEXT FUNCTION
-std::string WeissHapticGroupRemoteStateOfferer::GetName()
+std::string
+WeissHapticGroupRemoteStateOfferer::GetName()
{
return "WeissHapticGroupRemoteStateOfferer";
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateOffererFactoryBasePtr WeissHapticGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+XMLStateOffererFactoryBasePtr
+WeissHapticGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
{
return XMLStateOffererFactoryBasePtr(new WeissHapticGroupRemoteStateOfferer(reader));
}
-
-
-
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h
index 33ab415289e44cb56a6b2b8732a63919d6e2eb1c..4a4dfebdb0c3de6037c111cd807a1e812d51a06c 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticGroupRemoteStateOfferer.h
@@ -25,12 +25,14 @@
#pragma once
#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+
#include "WeissHapticGroupStatechartContext.generated.h"
namespace armarx::WeissHapticGroup
{
class WeissHapticGroupRemoteStateOfferer :
- virtual public XMLRemoteStateOfferer < WeissHapticGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ virtual public XMLRemoteStateOfferer<
+ WeissHapticGroupStatechartContext> // Change this statechart context if you need another context (dont forget to change in the constructor as well)
{
public:
WeissHapticGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
@@ -44,9 +46,5 @@ namespace armarx::WeissHapticGroup
static std::string GetName();
static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
static SubClassRegistry Registry;
-
-
};
-}
-
-
+} // namespace armarx::WeissHapticGroup
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
index f825d076038ced8b26b373a110eb44dd39e74b4c..054b289ad381668e3c77c7ca600a6c00295a984a 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.cpp
@@ -24,26 +24,27 @@
#include "WeissHapticSensorTest.h"
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
#include <RobotAPI/libraries/core/observerfilters/MatrixFilters.h>
#include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
-#include <ArmarXCore/observers/variant/DatafieldRef.h>
-
using namespace armarx;
using namespace WeissHapticGroup;
//// DO NOT EDIT NEXT LINE
-WeissHapticSensorTest::SubClassRegistry WeissHapticSensorTest::Registry(WeissHapticSensorTest::GetName(), &WeissHapticSensorTest::CreateInstance);
-
-
+WeissHapticSensorTest::SubClassRegistry
+ WeissHapticSensorTest::Registry(WeissHapticSensorTest::GetName(),
+ &WeissHapticSensorTest::CreateInstance);
WeissHapticSensorTest::WeissHapticSensorTest(const XMLStateConstructorParams& stateData) :
- XMLStateTemplate<WeissHapticSensorTest>(stateData), WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>(stateData)
+ XMLStateTemplate<WeissHapticSensorTest>(stateData),
+ WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>(stateData)
{
}
-
-void WeissHapticSensorTest::onEnter()
+void
+WeissHapticSensorTest::onEnter()
{
HapticUnitObserverInterfacePrx hapticObserver = getHapticObserver();
ChannelRegistry channels = hapticObserver->getAvailableChannels(false);
@@ -60,21 +61,25 @@ void WeissHapticSensorTest::onEnter()
for (std::pair<std::string, ChannelRegistryEntry> pair : channels)
{
std::string tactilePad = pair.first;
- DatafieldRefBasePtr matrixDatafield = new DatafieldRef(hapticObserver, tactilePad, "matrix");
- DatafieldRefBasePtr matrixNulled = hapticObserver->createFilteredDatafield(DatafieldFilterBasePtr(new filters::OffsetFilter()), matrixDatafield);
- DatafieldRefBasePtr matrixMax = hapticObserver->createFilteredDatafield(DatafieldFilterBasePtr(new filters::MatrixMaxFilter()), matrixNulled);
- tactileDatafields_MaximumValueMap.insert(std::make_pair(tactilePad, DatafieldRefPtr::dynamicCast(matrixMax)));
+ DatafieldRefBasePtr matrixDatafield =
+ new DatafieldRef(hapticObserver, tactilePad, "matrix");
+ DatafieldRefBasePtr matrixNulled = hapticObserver->createFilteredDatafield(
+ DatafieldFilterBasePtr(new filters::OffsetFilter()), matrixDatafield);
+ DatafieldRefBasePtr matrixMax = hapticObserver->createFilteredDatafield(
+ DatafieldFilterBasePtr(new filters::MatrixMaxFilter()), matrixNulled);
+ tactileDatafields_MaximumValueMap.insert(
+ std::make_pair(tactilePad, DatafieldRefPtr::dynamicCast(matrixMax)));
}
}
local.setTactileDatafields_MaximumValue(tactileDatafields_MaximumValueMap);
-
-
}
-void WeissHapticSensorTest::run()
+void
+WeissHapticSensorTest::run()
{
- std::map<std::string, DatafieldRefPtr> tactileDatafields_MaximumValueMap = local.getTactileDatafields_MaximumValue();
+ std::map<std::string, DatafieldRefPtr> tactileDatafields_MaximumValueMap =
+ local.getTactileDatafields_MaximumValue();
while (!isRunningTaskStopped()) // stop run function if returning true
{
@@ -92,29 +97,31 @@ void WeissHapticSensorTest::run()
max = std::max(max, padMax);
}
- ARMARX_IMPORTANT << "tactile max value: " << max << "; \n\n" << ss.str() << "\n" << ssNames.str();
+ ARMARX_IMPORTANT << "tactile max value: " << max << "; \n\n"
+ << ss.str() << "\n"
+ << ssNames.str();
usleep(10000); // 100ms
}
-
}
-void WeissHapticSensorTest::onBreak()
+void
+WeissHapticSensorTest::onBreak()
{
// put your user code for the breaking point here
// execution time should be short (<100ms)
}
-void WeissHapticSensorTest::onExit()
+void
+WeissHapticSensorTest::onExit()
{
// put your user code for the exit point here
// execution time should be short (<100ms)
-
}
// DO NOT EDIT NEXT FUNCTION
-XMLStateFactoryBasePtr WeissHapticSensorTest::CreateInstance(XMLStateConstructorParams stateData)
+XMLStateFactoryBasePtr
+WeissHapticSensorTest::CreateInstance(XMLStateConstructorParams stateData)
{
return XMLStateFactoryBasePtr(new WeissHapticSensorTest(stateData));
}
-
diff --git a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
index bce94688c8fbc38b0699509decd56534a6075cbb..565a77816dc97f3c779e10fbbed1b04441516238 100644
--- a/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
+++ b/source/RobotAPI/statecharts/WeissHapticGroup/WeissHapticSensorTest.h
@@ -27,8 +27,7 @@
namespace armarx::WeissHapticGroup
{
- class WeissHapticSensorTest :
- public WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>
+ class WeissHapticSensorTest : public WeissHapticSensorTestGeneratedBase<WeissHapticSensorTest>
{
public:
WeissHapticSensorTest(const XMLStateConstructorParams& stateData);
@@ -47,6 +46,4 @@ namespace armarx::WeissHapticGroup
// use stateparameters instead,
// if classmember are neccessary nonetheless, reset them in onEnter
};
-}
-
-
+} // namespace armarx::WeissHapticGroup
diff --git a/source/RobotAPI/statecharts/operations/RobotControl.cpp b/source/RobotAPI/statecharts/operations/RobotControl.cpp
index d99d262ac3203a9a9037f747b80cc057b02f12e8..f67e493853bfdc518e4f888ea0ba6d0fd030257d 100644
--- a/source/RobotAPI/statecharts/operations/RobotControl.cpp
+++ b/source/RobotAPI/statecharts/operations/RobotControl.cpp
@@ -22,25 +22,28 @@
#include "RobotControl.h"
-#include <RobotAPI/components/units/KinematicUnitObserver.h>
-
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
#include <ArmarXCore/statechart/DynamicRemoteState.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/units/KinematicUnitObserver.h>
namespace armarx
{
- void RobotControl::onInitRemoteStateOfferer()
+ void
+ RobotControl::onInitRemoteStateOfferer()
{
addState<Statechart_Robot>("RobotControl");
}
- void RobotControl::onConnectRemoteStateOfferer()
+ void
+ RobotControl::onConnectRemoteStateOfferer()
{
startRobotStatechart();
}
- void RobotControl::onExitRemoteStateOfferer()
+ void
+ RobotControl::onExitRemoteStateOfferer()
{
removeInstance(stateId);
robotFunctionalState = NULL;
@@ -49,27 +52,30 @@ namespace armarx
// this function creates an instance of the robot-statechart,
// so there is always one running-instance active that can
// be controlled by the GUI
- void RobotControl::startRobotStatechart()
+ void
+ RobotControl::startRobotStatechart()
{
- task = new RunningTask< RobotControl >(this, &RobotControl::createStaticInstance);
+ task = new RunningTask<RobotControl>(this, &RobotControl::createStaticInstance);
task->start();
}
-
- void RobotControl::hardReset(const Ice::Current&)
+ void
+ RobotControl::hardReset(const Ice::Current&)
{
removeInstance(robotFunctionalState->getLocalUniqueId());
startRobotStatechart();
}
- PropertyDefinitionsPtr RobotControl::createPropertyDefinitions()
+ PropertyDefinitionsPtr
+ RobotControl::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new RobotControlContextProperties(getConfigIdentifier()));
}
- void RobotControl::createStaticInstance()
+ void
+ RobotControl::createStaticInstance()
{
getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted, 5000);
stateId = createRemoteStateInstance("RobotControl", NULL, "RobotStatechart", "");
@@ -80,7 +86,8 @@ namespace armarx
robotFunctionalState = stateList.begin()->second;
callRemoteState(stateId, StringVariantContainerBaseMap());
- const std::string proxyName = getProperty<std::string>("XMLStatechartProfile").getValue() + getProperty<std::string>("proxyName").getValue();
+ const std::string proxyName = getProperty<std::string>("XMLStatechartProfile").getValue() +
+ getProperty<std::string>("proxyName").getValue();
const std::string stateName = getProperty<std::string>("stateName").getValue();
ARMARX_IMPORTANT << VAROUT(proxyName) << VAROUT(stateName);
@@ -105,12 +112,13 @@ namespace armarx
}
}
-
-
- void Statechart_Robot::defineState()
+ void
+ Statechart_Robot::defineState()
{
}
- void Statechart_Robot::defineSubstates()
+
+ void
+ Statechart_Robot::defineSubstates()
{
//add substates
StateBasePtr stateFunctional = setInitState(addState<StateRobotControl>("Functional"));
@@ -124,14 +132,13 @@ namespace armarx
addTransition<EvStartRobot>(stateFailure, stateFunctional);
}
-
-
-
- void StateRobotControl::defineSubstates()
+ void
+ StateRobotControl::defineSubstates()
{
// add substates
StateBasePtr stateIdling = addState<State>("Idling");
- StateBasePtr stateRobotPreInitialized = addState<RobotPreInitialized>("RobotPreInitialized");
+ StateBasePtr stateRobotPreInitialized =
+ addState<RobotPreInitialized>("RobotPreInitialized");
StateBasePtr stateRobotInitialized = addState<State>("RobotInitialized");
StateBasePtr stateScenarioRunning = addDynamicRemoteState("DynamicScenarioState");
StateBasePtr stateFatalError = addState<FailureState>("FatalError");
@@ -140,7 +147,8 @@ namespace armarx
// add transitions
addTransition<EvInit>(stateIdling, stateRobotPreInitialized);
addTransition<EvInitialized>(stateRobotPreInitialized, stateRobotInitialized);
- addTransition<EvLoadScenario>(stateRobotInitialized, stateScenarioRunning,
+ addTransition<EvLoadScenario>(stateRobotInitialized,
+ stateScenarioRunning,
PM::createMapping()->mapFromEvent("*", "*"));
addTransition<EvInit>(stateScenarioRunning, stateRobotPreInitialized);
addTransition<Success>(stateScenarioRunning, stateRobotInitialized);
@@ -152,7 +160,8 @@ namespace armarx
addTransitionFromAllStates<Failure>(stateFatalError);
}
- void StateRobotControl::onEnter()
+ void
+ StateRobotControl::onEnter()
{
// install global running conditions for the robot (e.g. temperature < xx°C)
// KinematicUnitObserverInterfacePrx prx = getContext()->getProxy<KinematicUnitObserverInterfacePrx>("KinematicUnitObserver");
@@ -160,16 +169,14 @@ namespace armarx
// ->add(channels::KinematicUnitObserver::jointTemperatures.getDatafield("KinematicUnitObserver", "NECK_JOINT0"), checks::KinematicUnitObserver::larger->getCheckStr(), 50.f));
}
-
-
-
RobotPreInitialized::RobotPreInitialized()
{
}
- void RobotPreInitialized::onEnter()
+ void
+ RobotPreInitialized::onEnter()
{
// issue init on remote components and install condition "RobotInitialized"
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/statecharts/operations/RobotControl.h b/source/RobotAPI/statecharts/operations/RobotControl.h
index 7aca952c718ff55f39188ad2ef246cbf36e85ba7..ee2471df928342ba3ca5cd37ec675b00212b9593 100644
--- a/source/RobotAPI/statecharts/operations/RobotControl.h
+++ b/source/RobotAPI/statecharts/operations/RobotControl.h
@@ -22,9 +22,9 @@
#pragma once
-#include <ArmarXCore/statechart/Statechart.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
#include <ArmarXCore/interface/operations/RobotControlIceBase.h>
+#include <ArmarXCore/statechart/Statechart.h>
namespace armarx
{
@@ -34,18 +34,21 @@ namespace armarx
struct RobotControlContextProperties : StatechartContextPropertyDefinitions
{
- RobotControlContextProperties(std::string prefix):
+ RobotControlContextProperties(std::string prefix) :
StatechartContextPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("XMLStatechartProfile", "", "Name of the statechart profile to be used. This is used as prefix to the proxyName. So GraspGroupRemoteStateOfferer will be Armar3aGraspGroupRemoteStateOfferer");
+ defineOptionalProperty<std::string>(
+ "XMLStatechartProfile",
+ "",
+ "Name of the statechart profile to be used. This is used as prefix to the "
+ "proxyName. So GraspGroupRemoteStateOfferer will be "
+ "Armar3aGraspGroupRemoteStateOfferer");
defineOptionalProperty<std::string>("proxyName", "", "name of the proxy to load");
defineOptionalProperty<std::string>("stateName", "", "name of the state to load");
}
};
-
-
/**
* \class RobotControl
* \brief RobotControl is used for dynamically loading and starting robot programs.
@@ -65,8 +68,10 @@ namespace armarx
* Refernce to the currently active Functionsl state.
*/
StateBasePtr robotFunctionalState;
+
// inherited from RemoteStateOfferer
- std::string getStateOffererName() const override
+ std::string
+ getStateOffererName() const override
{
return "RobotControl";
}
@@ -82,7 +87,7 @@ namespace armarx
private:
void createStaticInstance();
- RunningTask< RobotControl >::pointer_type task;
+ RunningTask<RobotControl>::pointer_type task;
int stateId;
};
@@ -90,6 +95,7 @@ namespace armarx
DEFINEEVENT(EvStopRobot)
DEFINEEVENT(EvStartRobot)
DEFINEEVENT(EvLoadingFailed)
+
/**
* Statechart which describes the most basic states of a robot:
@@ -105,8 +111,7 @@ namespace armarx
}
\enddot
*/
- struct Statechart_Robot :
- StateTemplate<Statechart_Robot>
+ struct Statechart_Robot : StateTemplate<Statechart_Robot>
{
void defineState() override;
void defineSubstates() override;
@@ -118,6 +123,7 @@ namespace armarx
DEFINEEVENT(EvLoadScenario)
DEFINEEVENT(EvStartScenario)
DEFINEEVENT(EvInitFailed)
+
/**
* Statechart which describes the operational states of a robot program.
* The state "Robot Program Running" is a DynamicRemoteState and needs to
@@ -142,8 +148,7 @@ namespace armarx
}
\enddot
*/
- struct StateRobotControl :
- StateTemplate<StateRobotControl>
+ struct StateRobotControl : StateTemplate<StateRobotControl>
{
void onEnter() override;
void defineSubstates() override;
@@ -152,11 +157,9 @@ namespace armarx
/**
* Robot is in the state preinitialized.
*/
- struct RobotPreInitialized :
- StateTemplate<RobotPreInitialized>
+ struct RobotPreInitialized : StateTemplate<RobotPreInitialized>
{
RobotPreInitialized();
void onEnter() override;
};
-}
-
+} // namespace armarx