YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ImplementControlMode.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
10
11#include <cstdio>
12using namespace yarp::dev;
13using namespace yarp::os;
14
15namespace {
16YARP_LOG_COMPONENT(IMPLEMENT_LAYER_COMPONENT, "yarp.ImplementControlMode")
17}
18
20 m_helper(nullptr),
21 m_iraw(r)
22{}
23
24bool ImplementControlMode::initialize(int size, const int *amap)
25{
26 if (m_helper != nullptr) {
27 return false;
28 }
29
30 m_helper=(void *)(new ControlBoardHelper(size, amap));
31 yAssert (m_helper != nullptr);
32
33 yAssert(size > 0);
34 m_vectorInt_tmp.resize(size);
35 m_vectorCM_tmp.resize(size);
36 m_vectorSCM_tmp.resize(size);
37
38 return true;
39}
40
45
47{
48 if (m_helper!=nullptr)
49 {
50 delete castToMapper(m_helper);
51 m_helper=nullptr;
52 }
53
54 return true;
55}
56
57ReturnValue ImplementControlMode::getAvailableControlModes(int j, std::vector<yarp::dev::SelectableControlModeEnum>& avail)
58{
59 std::lock_guard lock(m_imp_mutex);
61
62 int k=castToMapper(m_helper)->toHw(j);
63 return m_iraw->getAvailableControlModesRaw(k, avail);
64}
65
67{
68 std::lock_guard lock(m_imp_mutex);
70
71 int k=castToMapper(m_helper)->toHw(j);
72 return m_iraw->getControlModeRaw(k, mode);
73}
74
75ReturnValue ImplementControlMode::getControlModes(std::vector<yarp::dev::ControlModeEnum>& modes)
76{
77 std::lock_guard lock(m_imp_mutex);
79
80 ReturnValue ret=m_iraw->getControlModesRaw(m_vectorCM_tmp);
81
82 for(int idx=0; idx<castToMapper(m_helper)->axes(); idx++)
83 {
84 modes[idx] = m_vectorCM_tmp[castToMapper(m_helper)->toHw(idx)];
85 }
86
87 return ret;
88}
89
90ReturnValue ImplementControlMode::getControlModes(const std::vector<int>& joints, std::vector<yarp::dev::ControlModeEnum>& modes)
91{
92 std::lock_guard lock(m_imp_mutex);
94
95 std::vector<int> vectorInt_tmp(joints.size());
96 for(size_t idx=0; idx<joints.size(); idx++)
97 {
98 vectorInt_tmp[idx] = castToMapper(m_helper)->toHw(joints[idx]);
99 }
100 ReturnValue ret = m_iraw->getControlModesRaw(vectorInt_tmp, modes);
101
102 return ret;
103}
104
106{
107 std::lock_guard lock(m_imp_mutex);
109
110 int k=castToMapper(m_helper)->toHw(j);
111 return m_iraw->setControlModeRaw(k, mode);
112}
113
114ReturnValue ImplementControlMode::setControlModes(const std::vector<int>& joints, const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
115{
116 std::lock_guard lock(m_imp_mutex);
118
119 std::vector<int> vectorInt_tmp(joints.size());
120 for(size_t idx=0; idx<joints.size(); idx++)
121 {
122 vectorInt_tmp[idx] = castToMapper(m_helper)->toHw(joints[idx]);
123 }
124
125 ReturnValue ret = m_iraw->setControlModesRaw(vectorInt_tmp, modes);
126
127 return ret;
128}
129
130ReturnValue ImplementControlMode::setControlModes(const std::vector<yarp::dev::SelectableControlModeEnum>& modes)
131{
132 std::lock_guard lock(m_imp_mutex);
134
135 for(int idx=0; idx<castToMapper(m_helper)->axes(); idx++)
136 {
137 int j = castToMapper(m_helper)->toHw(idx);
138 m_vectorSCM_tmp[j] = modes[idx];
139 }
140
141 ReturnValue ret = m_iraw->setControlModesRaw(m_vectorSCM_tmp);
142
143 return ret;
144}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
FeatureMode mode
#define yAssert(x)
Definition Log.h:388
Interface for setting control mode in control board.
virtual yarp::dev::ReturnValue getControlModesRaw(std::vector< yarp::dev::ControlModeEnum > &mode)=0
virtual yarp::dev::ReturnValue setControlModeRaw(int j, yarp::dev::SelectableControlModeEnum mode)=0
virtual yarp::dev::ReturnValue getAvailableControlModesRaw(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail)=0
virtual yarp::dev::ReturnValue setControlModesRaw(const std::vector< int > &joints, const std::vector< yarp::dev::SelectableControlModeEnum > &mode)=0
virtual yarp::dev::ReturnValue getControlModeRaw(int j, yarp::dev::ControlModeEnum &mode)=0
yarp::dev::ReturnValue setControlModes(const std::vector< int > &joints, const std::vector< yarp::dev::SelectableControlModeEnum > &modes) override
Set the current control mode for a subset of axes.
yarp::dev::ReturnValue getAvailableControlModes(int j, std::vector< yarp::dev::SelectableControlModeEnum > &avail) override
Return a list of available control mode for the given joint.
bool initialize(int k, const int *amap)
yarp::dev::ReturnValue getControlModes(const std::vector< int > &joints, std::vector< yarp::dev::ControlModeEnum > &modes) override
Get the current control mode for a subset of axes.
yarp::dev::ReturnValue setControlMode(int j, yarp::dev::SelectableControlModeEnum mode) override
Set the current control mode.
yarp::dev::ReturnValue getControlMode(int j, yarp::dev::ControlModeEnum &mode) override
Get the current control mode.
A mini-server for performing network communication in the background.
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
SelectableControlModeEnum
An interface to the operating system, including Port based communication.