VisualServoObject.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2014-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotSkillTemplates::FindAndGraspObjectGroup
19  * @author Valerij Wittenbeck ( valerij dot wittenbeck at student dot kit dot edu )
20  * @date 2014
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "VisualServoObject.h"
29 #include <VirtualRobot/Grasping/GraspSet.h>
30 
31 #include <MemoryX/interface/components/PriorKnowledgeInterface.h>
32 #include <MemoryX/interface/observers/ObjectMemoryObserverInterface.h>
39 
40 using namespace armarx;
41 using namespace FindAndGraspObjectGroup;
42 
43 // DO NOT EDIT NEXT LINE
45 
46 
47 
49  XMLStateTemplate < VisualServoObject > (stateData)
50 {
51 }
52 
54 {
55  setLocalObjectChannelFromInput("objectChannel", "objectName"); //still need this?
56  setLocalObjectChannelFromInput("markerChannel", "markerName"); //still need this?
57 
58  //NEW: grasp definition
59  // get grasp definition - Taken from GraspingWithVisualFeedback
60  FindAndGraspObjectContext* context = getContext<FindAndGraspObjectContext>();
61  memoryx::PersistentObjectClassSegmentBasePrx classesSegmentPrx = context->priorMemoryProxy->getObjectClassesSegment();
62  memoryx::CommonStorageInterfacePrx databasePrx = context->priorMemoryProxy->getCommonStorage();
63  memoryx::GridFileManagerPtr fileManager(new memoryx::GridFileManager(databasePrx));
64  memoryx::EntityBasePtr entity = classesSegmentPrx->getEntityByName(getInput<std::string>("objectName"));
65  memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(entity);
66  memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
67  VirtualRobot::ManipulationObjectPtr mo = simoxWrapper->getManipulationObject();
68 
69  // // TODO: remove when grasp definitions can be saved/edited in MemoryX
70  // VirtualRobot::ManipulationObjectPtr mo;
71 
72  // std::string armarx_home;
73  // if(std::getenv("ArmarXHome_DIR")){
74  // armarx_home = std::getenv("ArmarXHome_DIR");
75  // mo = VirtualRobot::ObjectIO::loadManipulationObject(armarx_home + "Armar3/data/scene/Vitalis.xml");
76  // }
77  // else
78  // ARMARX_LOG << eERROR << "Problem with ManipulationObject file for StatechartVisualServoingExample!" << flush;
79 
80 
81  //TODO: Get graspSetName and graspName from the configFile! (in order to try various grasps, in case we chose an unreachable one...)
82 
83  std::string graspSetName = getInput<std::string>("graspSetName");
84  VirtualRobot::GraspSetPtr gs = mo->getGraspSet(graspSetName);
85 
86  if (gs)
87  {
88  std::string graspName = getInput<std::string>("graspName");
89  context->graspDefinition = gs->getGrasp(graspName);
90  ARMARX_DEBUG << "VisualServoObject: getting grasp with name: " << graspName << flush;
91 
92  if (!context->graspDefinition)
93  {
94  ARMARX_WARNING << "No grasp with name " << graspName << " found! " << flush;
95  }
96  }
97  else
98  {
99  ARMARX_WARNING << "No grasp set with name " << graspSetName << " found! " << flush;
100  }
101 
102 
103  ARMARX_VERBOSE << "Done VisualServoObject::onEnter()" << flush;
104 }
105 
107 {
108  FindAndGraspObjectContext* context = getContext<FindAndGraspObjectContext>();
109  context->objectMemoryObserver->releaseObjectClass(getLocal<ChannelRef>("objectChannel")); //which of these do we still need?
110  context->objectMemoryObserver->releaseObjectClass(getLocal<ChannelRef>("markerChannel"));
111 }
112 
113 void VisualServoObject::setLocalObjectChannelFromInput(const std::string& objectChannelName, const std::string& inputName)
114 {
115  FindAndGraspObjectContext* context = getContext<FindAndGraspObjectContext>();
116  std::string objectName = getInput<std::string>(inputName);
117  ChannelRefBasePtr objectChannel = context->objectMemoryObserver->requestObjectClassRepeated(objectName, 30);
118 
119  if (objectChannel)
120  {
121  setLocal(objectChannelName, objectChannel);
122  ARMARX_DEBUG << "setLocal() for objectName: " << objectName << "; objectChannelName: " << objectChannelName;
123  }
124  else
125  {
126  ARMARX_IMPORTANT << "Unknown Object Class: " << objectName;
127  }
128 }
129 
130 // DO NOT EDIT NEXT FUNCTION
132 {
133  return "VisualServoObject";
134 }
135 
136 // DO NOT EDIT NEXT FUNCTION
138 {
139  return XMLStateFactoryBasePtr(new VisualServoObject(stateData));
140 }
141 
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::FindAndGraspObjectContext::priorMemoryProxy
memoryx::PriorKnowledgeInterfacePrx priorMemoryProxy
Definition: FindAndGraspObjectContext.h:153
armarx::XMLStateConstructorParams
Definition: XMLState.h:50
GridFileManager.h
ObjectClass.h
armarx::State::setLocal
void setLocal(std::string const &key, const Variant &value)
setLocal() sets a local parameter.
Definition: State.cpp:417
armarx::FindAndGraspObjectGroup::VisualServoObject::GetName
static std::string GetName()
Definition: VisualServoObject.cpp:131
armarx::FindAndGraspObjectContext::objectMemoryObserver
memoryx::ObjectMemoryObserverInterfacePrx objectMemoryObserver
Definition: FindAndGraspObjectContext.h:152
armarx::FindAndGraspObjectContext
Definition: FindAndGraspObjectContext.h:67
IceInternal::Handle< ObjectClass >
armarx::XMLStateTemplate
Class for legacy to stay compatible with old statecharts.
Definition: XMLState.h:137
armarx::FindAndGraspObjectGroup::VisualServoObject
Definition: VisualServoObject.h:33
armarx::FindAndGraspObjectGroup::VisualServoObject::Registry
static SubClassRegistry Registry
Definition: VisualServoObject.h:48
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
MemoryXCoreObjectFactories.h
memoryx::EntityWrappers::SimoxObjectWrapper
SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject (i.e visualization,...
Definition: SimoxObjectWrapper.h:46
MotionModelAttachedToOtherObject.h
armarx::FindAndGraspObjectGroup::VisualServoObject::CreateInstance
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData)
Definition: VisualServoObject.cpp:137
armarx::FindAndGraspObjectGroup::VisualServoObject::VisualServoObject
VisualServoObject(XMLStateConstructorParams stateData)
Definition: VisualServoObject.cpp:48
armarx::XMLStateFactoryBasePtr
IceInternal::Handle< XMLStateFactoryBase > XMLStateFactoryBasePtr
Definition: XMLState.h:65
armarx::FindAndGraspObjectGroup::VisualServoObject::onEnter
void onEnter() override
Virtual function, in which the behaviour of state is defined, when it is entered. Can be overridden,...
Definition: VisualServoObject.cpp:53
SimoxObjectWrapper.h
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:32
armarx::FindAndGraspObjectGroup::VisualServoObject::onExit
void onExit() override
Virtual function, in which the behaviour of state is defined, when it is exited. Can be overridden,...
Definition: VisualServoObject.cpp:106
memoryx::GridFileManager
GridFileManager provides utility functions for working with files in Mongo GridFS and links to them s...
Definition: GridFileManager.h:42
MemoryXTypesObjectFactories.h
VisualServoObject.h
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::FindAndGraspObjectGroup::VisualServoObject::setLocalObjectChannelFromInput
void setLocalObjectChannelFromInput(const std::string &objectChannelName, const std::string &inputName)
Definition: VisualServoObject.cpp:113
armarx::FindAndGraspObjectContext::graspDefinition
VirtualRobot::GraspPtr graspDefinition
Definition: FindAndGraspObjectContext.h:154
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28