TCPControlUnitObserver.cpp
Go to the documentation of this file.
1 /*
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package ArmarX::
17 * @author Mirko Waechter ( mirko.waechter at kit dot edu)
18 * @date 2013
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include "TCPControlUnitObserver.h"
24 
25 //#include <ArmarXCore/core/checks/ConditionCheckEqualsPoseWithTolerance.h>
34 
35 #define TCP_POSE_CHANNEL "TCPPose"
36 #define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
37 
38 namespace armarx
39 {
40 
42  {
43  }
44 
46  {
47  usingTopic(getProperty<std::string>("TCPControlUnitName").getValue() + "State");
48 
49  // register all checks
54  // offerConditionCheck("magnitudeinrange", new ConditionCheckInRange());
55  offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
57 
58 
59 
60  }
61 
63  {
64  offerChannel(TCP_POSE_CHANNEL, "TCP poses of the robot.");
65  offerChannel(TCP_TRANS_VELOCITIES_CHANNEL, "TCP velocities of the robot.");
66 
67  }
68 
70  {
73  }
74 
75  void TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap& poseMap, const Ice::Current&)
76  {
77  std::unique_lock lock(dataMutex);
78  // ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
79  FramedPoseBaseMap::const_iterator it = poseMap.begin();
80 
81  for (; it != poseMap.end(); it++)
82  {
83 
84  FramedPosePtr vec = FramedPosePtr::dynamicCast(it->second);
85  const std::string& tcpName = it->first;
86 
87  if (!existsDataField(TCP_POSE_CHANNEL, tcpName))
88  {
89  offerDataFieldWithDefault(TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
90  }
91  else
92  {
93  setDataField(TCP_POSE_CHANNEL, tcpName, Variant(it->second));
94  }
95 
97 
98  if (!existsChannel(tcpName))
99  {
100  offerChannel(tcpName, "pose components of " + tcpName);
101  offerDataFieldWithDefault(tcpName, "x", Variant(vec->position->x), "X axis");
102  offerDataFieldWithDefault(tcpName, "y", Variant(vec->position->y), "Y axis");
103  offerDataFieldWithDefault(tcpName, "z", Variant(vec->position->z), "Z axis");
104  offerDataFieldWithDefault(tcpName, "qx", Variant(vec->orientation->qx), "Quaternion part x");
105  offerDataFieldWithDefault(tcpName, "qy", Variant(vec->orientation->qy), "Quaternion part y");
106  offerDataFieldWithDefault(tcpName, "qz", Variant(vec->orientation->qz), "Quaternion part z");
107  offerDataFieldWithDefault(tcpName, "qw", Variant(vec->orientation->qw), "Quaternion part w");
108  offerDataFieldWithDefault(tcpName, "frame", Variant(vec->frame), "Reference Frame");
109  }
110  else
111  {
112  setDataField(tcpName, "x", Variant(vec->position->x));
113  setDataField(tcpName, "y", Variant(vec->position->y));
114  setDataField(tcpName, "z", Variant(vec->position->z));
115  setDataField(tcpName, "qx", Variant(vec->orientation->qx));
116  setDataField(tcpName, "qy", Variant(vec->orientation->qy));
117  setDataField(tcpName, "qz", Variant(vec->orientation->qz));
118  setDataField(tcpName, "qw", Variant(vec->orientation->qw));
119  setDataField(tcpName, "frame", Variant(vec->frame));
120  }
121 
122  updateChannel(tcpName);
123 
124  }
125  }
126 
127  void TCPControlUnitObserver::reportTCPVelocities(const FramedDirectionMap& tcpTranslationVelocities, const FramedDirectionMap& tcpOrientationVelocities, const Ice::Current&)
128  {
129  std::unique_lock lock(dataMutex);
130  FramedDirectionMap::const_iterator it = tcpTranslationVelocities.begin();
131 
132  for (; it != tcpTranslationVelocities.end(); it++)
133  {
134 
135  FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(it->second);
136  FramedDirectionPtr vecOri;
137  FramedDirectionMap::const_iterator itOri = tcpOrientationVelocities.find(it->first);
138 
139  if (itOri != tcpOrientationVelocities.end())
140  {
141  vecOri = FramedDirectionPtr::dynamicCast(itOri->second);
142  }
143 
144  const std::string& tcpName = it->first;
145 
146  ARMARX_CHECK_EXPRESSION(vec->frame == vecOri->frame);
147 
149  {
150  offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
151  }
152  else
153  {
154  setDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second));
155  }
156 
158  const std::string channelName = tcpName + "Velocities";
159 
160  if (!existsChannel(channelName))
161  {
162  offerChannel(channelName, "pose components of " + tcpName);
163  offerDataFieldWithDefault(channelName, "x", Variant(vec->x), "X axis");
164  offerDataFieldWithDefault(channelName, "y", Variant(vec->y), "Y axis");
165  offerDataFieldWithDefault(channelName, "z", Variant(vec->z), "Z axis");
166  offerDataFieldWithDefault(channelName, "roll", Variant(vecOri->x), "Roll");
167  offerDataFieldWithDefault(channelName, "pitch", Variant(vecOri->y), "Pitch");
168  offerDataFieldWithDefault(channelName, "yaw", Variant(vecOri->z), "Yaw");
169  offerDataFieldWithDefault(channelName, "frame", Variant(vecOri->frame), "Reference Frame");
170  }
171  else
172  {
173  setDataField(channelName, "x", Variant(vec->x));
174  setDataField(channelName, "y", Variant(vec->y));
175  setDataField(channelName, "z", Variant(vec->z));
176  setDataField(channelName, "roll", Variant(vecOri->x));
177  setDataField(channelName, "pitch", Variant(vecOri->y));
178  setDataField(channelName, "yaw", Variant(vecOri->z));
179  setDataField(channelName, "frame", Variant(vec->frame));
180  }
181 
182  updateChannel(channelName);
183 
184  }
185  }
186 
187 }
188 
189 
armarx::TCPControlUnitObserverPropertyDefinitions
Definition: TCPControlUnitObserver.h:35
armarx::Observer::updateChannel
void updateChannel(const std::string &channelName, const std::set< std::string > &updatedDatafields=std::set< std::string >())
Update all conditions for a channel.
Definition: Observer.cpp:715
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::TCPControlUnitObserver::onConnectObserver
void onConnectObserver() override
Framework hook.
Definition: TCPControlUnitObserver.cpp:62
ConditionCheckEquals.h
ConditionCheckInRange.h
armarx::ConditionCheckApproxPose
Definition: ConditionCheckEqualsPoseWithTolerance.h:32
armarx::TCPControlUnitObserver::dataMutex
std::mutex dataMutex
Definition: TCPControlUnitObserver.h:81
armarx::ConditionCheckSmaller
Definition: ConditionCheckSmaller.h:40
armarx::TCPControlUnitObserver::TCPControlUnitObserver
TCPControlUnitObserver()
Definition: TCPControlUnitObserver.cpp:41
TCPControlUnitObserver.h
armarx::Observer::existsChannel
bool existsChannel(const std::string &channelName) const
Definition: Observer.cpp:1433
ConditionCheckEqualsPoseWithTolerance.h
armarx::TCPControlUnitObserver::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: TCPControlUnitObserver.cpp:69
IceInternal::Handle< FramedPose >
armarx::FramedPoseBaseMap
::std::map< ::std::string, ::armarx::FramedPoseBasePtr > FramedPoseBaseMap
Definition: RobotStateObserver.h:55
TCP_TRANS_VELOCITIES_CHANNEL
#define TCP_TRANS_VELOCITIES_CHANNEL
Definition: TCPControlUnitObserver.cpp:36
armarx::ConditionCheckMagnitudeLarger
Definition: ConditionCheckMagnitudeChecks.h:36
armarx::ConditionCheckUpdated
Definition: ConditionCheckUpdated.h:41
armarx::TCPControlUnitObserver::reportTCPVelocities
void reportTCPVelocities(const FramedDirectionMap &tcpTranslationVelocities, const FramedDirectionMap &tcpOrientationVelocities, const Ice::Current &c=Ice::emptyCurrent) override
Definition: TCPControlUnitObserver.cpp:127
armarx::Observer::setDataField
void setDataField(const std::string &channelName, const std::string &datafieldName, const Variant &value, bool triggerFilterUpdate=true)
set datafield with datafieldName and in channel channelName
Definition: Observer.cpp:465
TCP_POSE_CHANNEL
#define TCP_POSE_CHANNEL
Definition: TCPControlUnitObserver.cpp:35
armarx::TCPControlUnitObserver::reportTCPPose
void reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &c=Ice::emptyCurrent) override
Definition: TCPControlUnitObserver.cpp:75
ExpressionException.h
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:248
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::Observer::existsDataField
bool existsDataField(const std::string &channelName, const std::string &datafieldName) const
Definition: Observer.cpp:1450
ConditionCheckSmaller.h
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::ConditionCheckEquals
Definition: ConditionCheckEquals.h:46
armarx::Observer::offerConditionCheck
void offerConditionCheck(std::string checkName, ConditionCheck *conditionCheck)
Offer a condition check.
Definition: Observer.cpp:273
ConditionCheckMagnitudeChecks.h
ConditionCheckLarger.h
armarx::ConditionCheckLarger
Definition: ConditionCheckLarger.h:40
armarx::Observer::offerDataFieldWithDefault
void offerDataFieldWithDefault(std::string channelName, std::string datafieldName, const Variant &defaultValue, std::string description)
Offer a datafield with default value.
Definition: Observer.cpp:152
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
armarx::TCPControlUnitObserver::onInitObserver
void onInitObserver() override
Framework hook.
Definition: TCPControlUnitObserver.cpp:45
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
ConditionCheckUpdated.h
armarx::Observer::offerChannel
void offerChannel(std::string channelName, std::string description)
Offer a channel.
Definition: Observer.cpp:126