1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
|
/**
* @file RobotCreator.cpp
* @brief
*
* This file RobotCreator.cpp is created at Almende B.V. It is open-source software and part
* of the Common Hybrid Agent Platform (CHAP). A toolbox with a lot of open-source tools.
* Ranging from thread pools, and TCP/IP components to control architectures and learning
* algorithms. This software is published under the GNU Lesser General Public license,
* but the software is not allowed to be used for military use, within the bio-industry
* or for animal experimentation.
*
* @author Anne C. van Rossum
* @date Jul 13, 2010
* @project Replicator FP7
* @company Almende B.V.
* @case
*/
// General files
#include <sstream>
// Delta3D files
#include <dtUtil/log.h>
// Robot3D files
#include <srCore/robotCreator.h>
#include <srCore/robot/robotActorBase.h>
namespace srCore {
/* **************************************************************************************
* Implementation of RobotCreator
* **************************************************************************************/
RobotCreator::RobotCreator() {
}
RobotCreator::~RobotCreator() {
}
/*
* Function: addRobotToGM
*
* Adds a previously registered robot to the game manager.
*/
void RobotCreator::addRobotToGM(int id, const RobotInitData ¶meters, float height) {
std::ostringstream msg; msg.clear(); msg.str("");
msg << "Add robot of type: " << parameters.type << " to the game manager";
LOG_INFO(msg.str());
dtCore::RefPtr<RobotActorBaseProxy> mRobotActorProxy;
dtGame::GameApplication *GA = dtGame::GameApplication::GetInstance(0);
// Create the robot
if (std::string(parameters.type).find("RobotKIT2") != std::string::npos) {
LOG_INFO("Add robot of type RobotKIT2");
GA->GetGameManager()->CreateActor("RobotKIT2", "RobotKIT2", mRobotActorProxy);
// GA->GetGameManager()->CreateActor(*RobotKITType.get(), mRobotActorProxy);
} else
// if (std::string(parameters.type).find(RobotKIT::DEFAULT_NAME) != std::string::npos) {
if (std::string(parameters.type).find("RobotKIT") != std::string::npos) {
LOG_INFO("Add robot of type RobotKIT");
GA->GetGameManager()->CreateActor("RobotKIT", "RobotKIT", mRobotActorProxy);
// GA->GetGameManager()->CreateActor(*RobotKITType.get(), mRobotActorProxy);
} else {
LOG_ERROR("That robot type we do not know!");
return;
}
assert (mRobotActorProxy != NULL);
// Set identification
mRobotActorProxy->setID(id);
mRobotActorProxy->setConnectorStates(parameters.connectorStates);
mRobotActorProxy->SetName(parameters.name);
// Set position and rotation
osg::Vec3f pos = parameters.pos + osg::Vec3f(0.0, 0.0, height);
mRobotActorProxy->SetTranslation(pos);
osg::Matrix matrix = osg::Matrix(parameters.quat);
mRobotActorProxy->SetRotationFromMatrix(matrix);
// Initialise joint angles and sensor values
mRobotActorProxy->setInitialJointAngles(parameters.jointAngles);
mRobotActorProxy->setSensors(parameters.sensors);
// Debug information
msg.clear(); msg.str();
msg << "Joint angles are: " << parameters.jointAngles[0];
LOG_DEBUG(msg.str());
// Finally add actor to the game manager
GA->GetGameManager()->AddActor(*mRobotActorProxy, false, false);
}
} // end of namespace srCore
|