sas
Modularised monitoring, logging, and control of robots.
Loading...
Searching...
No Matches
sas_common.hpp
1#pragma once
2/*
3# Copyright (c) 2016-2025 Murilo Marques Marinho
4#
5# This file is part of sas_common.
6#
7# sas_common is free software: you can redistribute it and/or modify
8# it under the terms of the GNU Lesser General Public License as published by
9# the Free Software Foundation, either version 3 of the License, or
10# (at your option) any later version.
11#
12# sas_common is distributed in the hope that it will be useful,
13# but WITHOUT ANY WARRANTY; without even the implied warranty of
14# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15# GNU Lesser General Public License for more details.
16#
17# You should have received a copy of the GNU Lesser General Public License
18# along with sas_common. If not, see <https://www.gnu.org/licenses/>.
19#
20# ################################################################
21#
22# Author: Murilo M. Marinho, email: murilomarinho@ieee.org
23#
24# ################################################################
25# 2025.07.10
26# - Removed display_signal_handler_none_bug_info as no longer needed in Jazzy
27# - Added get_ros_optional_parameter
28*/
29
30#include <type_traits>
31#include <rclcpp/rclcpp.hpp>
32
33namespace sas
34{
35
36template <typename T>
44void get_ros_parameter(std::shared_ptr<rclcpp::Node>& node, const std::string& parameter_name, T& t)
45{
46 if (!node->has_parameter(parameter_name))
47 node->declare_parameter<T>(parameter_name);
48
49 if (!node->get_parameter(parameter_name, t))
50 {
51 throw std::runtime_error("::Error loading " + parameter_name);
52 }
53}
54
55template <typename T>
63void get_ros_optional_parameter(std::shared_ptr<rclcpp::Node>& node, const std::string& parameter_name, T& t,
64 const T& default_value)
65{
66 if (!node->has_parameter(parameter_name))
67 node->declare_parameter<T>(parameter_name, default_value);
68 node->get_parameter(parameter_name, t);
69}
70
71template <typename T>
77std::string get_type_name(const std::vector<T>&)
78{
79 if constexpr (std::is_same_v<T, std::string>)
80 {
81 return "std::vector<std::string>";
82 }
83 if constexpr (std::is_same_v<T, int64_t>)
84 {
85 return "std::vector<int64_t>";
86 }
87 if constexpr (std::is_same_v<T, double>)
88 {
89 return "std::vector<double>";
90 }
91 if constexpr (std::is_same_v<T, bool>)
92 {
93 return "std::vector<bool>";
94 }
95 return "";
96}
97
98template <typename T>
114void get_ros_parameter(std::shared_ptr<rclcpp::Node>& node, const std::string& parameter_name, std::vector<T>& v)
115{
116 if (!node->has_parameter(parameter_name))
117 {
118 try
119 {
120 node->declare_parameter<std::vector<T>>(parameter_name);
121 }
122 catch (const rclcpp::exceptions::InvalidParameterTypeException& e)
123 {
124 RCLCPP_INFO_STREAM(node->get_logger(), "sas::get_ros_parameter: List "
125 << parameter_name << " does not have expected type, a.k.a "
126 << get_type_name(v)
127 << ", in the parameter server, checking for empty list...");
128 node->declare_parameter<std::vector<std::string>>(parameter_name);
129 }
130 }
131
132 if (node->get_parameter(parameter_name).get_type() == rclcpp::PARAMETER_STRING_ARRAY)
133 {
134 auto v_checker = node->get_parameter(parameter_name).as_string_array();
135 if (v_checker.size() == 1 && v_checker.at(0) == "EMPTY_LIST")
136 {
137 RCLCPP_INFO_STREAM(node->get_logger(), "sas::get_ros_parameter: List "
138 << parameter_name << " was loaded as an EMPTY list of type "
139 << get_type_name(v) << ".");
140 v = std::vector<T>();
141 return;
142 }
143 else
144 {
145 if constexpr (not std::is_same_v<T, std::string>)
146 {
147 throw std::runtime_error("::Was this list, " + parameter_name +
148 ", supposed to be empty? \n "
149 "It should be a list such as list:[\"EMPTY_LIST\"].");
150 }
151 }
152 }
153 if (!node->get_parameter(parameter_name, v))
154 {
155 throw std::runtime_error("::Unable to get parameter " + parameter_name);
156 }
157}
158
159} // namespace sas