ros2_canopen master
C++ ROS CANopen Library
Loading...
Searching...
No Matches
mode_target_helper.hpp
Go to the documentation of this file.
1// Copyright 2023 Christoph Hellmann Santos
2// Copyright 2014-2022 Authors of ros_canopen
3//
4// This program is free software: you can redistribute it and/or modify
5// it under the terms of the GNU General Public License as published by
6// the Free Software Foundation, either version 3 of the License, or
7// (at your option) any later version.
8//
9// This program is distributed in the hope that it will be useful,
10// but WITHOUT ANY WARRANTY; without even the implied warranty of
11// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12// GNU General Public License for more details.
13//
14// You should have received a copy of the GNU General Public License
15// along with this program. If not, see <https://www.gnu.org/licenses/>.
16//
17
18#ifndef MODE_TARGET_HELPER_HPP
19#define MODE_TARGET_HELPER_HPP
20
21#include <atomic>
22#include <boost/numeric/conversion/cast.hpp>
23#include <cmath>
24#include <iostream>
25#include <limits>
26#include <memory>
27#include <string>
28#include <vector>
29
31
32namespace ros2_canopen
33{
34
35template <typename T>
36class ModeTargetHelper : public Mode
37{
38 T target_;
39 std::atomic<bool> has_target_;
40
41public:
42 ModeTargetHelper(uint16_t mode) : Mode(mode) {}
43 bool hasTarget() { return has_target_; }
44 T getTarget() { return target_; }
45 virtual bool setTarget(const double & val)
46 {
47 if (std::isnan(val))
48 {
49 // std::cout << "canopen_402 target command is not a number" << std::endl;
50 RCLCPP_DEBUG(rclcpp::get_logger("canopen_402_target"), "Target command is not a number");
51 return false;
52 }
53
54 using boost::numeric_cast;
55 using boost::numeric::negative_overflow;
56 using boost::numeric::positive_overflow;
57
58 try
59 {
60 target_ = numeric_cast<T>(val);
61 }
62 catch (negative_overflow &)
63 {
64 std::cout << "canopen_402 Command " << val
65 << " does not fit into target, clamping to min limit" << std::endl;
66 target_ = std::numeric_limits<T>::min();
67 }
68 catch (positive_overflow &)
69 {
70 std::cout << "canopen_402 Command " << val
71 << " does not fit into target, clamping to max limit" << std::endl;
72 target_ = std::numeric_limits<T>::max();
73 }
74 catch (...)
75 {
76 std::cout << "canopen_402 Was not able to cast command " << val << std::endl;
77 return false;
78 }
79
80 has_target_ = true;
81 return true;
82 }
83 virtual bool start()
84 {
85 has_target_ = false;
86 return true;
87 }
88};
89} // namespace ros2_canopen
90
91#endif // MODE_TARGET_HELPER_HPP
Definition mode_target_helper.hpp:37
virtual bool setTarget(const double &val)
Definition mode_target_helper.hpp:45
virtual bool start()
Definition mode_target_helper.hpp:83
bool hasTarget()
Definition mode_target_helper.hpp:43
T getTarget()
Definition mode_target_helper.hpp:44
ModeTargetHelper(uint16_t mode)
Definition mode_target_helper.hpp:42
Definition mode.hpp:30
Definition configuration_manager.hpp:28