1 ////////////////////////////////////////////////////////////////////////////////////
2 ///
3 /// \file tutorial_07.cpp
4 /// \brief This file is part of a set of tutorials for learning how to use
5 /// JAUS++. This program demonstrates how to find a component with the
6 /// Primitive Driver service, and send it a wrench (i.e. drive)
7 /// command.
8 ///
9 /// <br>Author(s): Daniel Barber
10 /// <br>Created: 24 Jan 2012
11 /// <br>Copyright (c) 2012
12 /// <br>Applied Cognition and Training in Immersive Virtual Environments
13 /// <br>(ACTIVE) Laboratory
14 /// <br>Institute for Simulation and Training (IST)
15 /// <br>University of Central Florida (UCF)
16 /// <br>All rights reserved.
17 /// <br>Email: dbarber@ist.ucf.edu
18 /// <br>Web: http://active.ist.ucf.edu
19 ///
20 /// Redistribution and use in source and binary forms, with or without
21 /// modification, are permitted provided that the following conditions are met:
22 /// * Redistributions of source code must retain the above copyright
23 /// notice, this list of conditions and the following disclaimer.
24 /// * Redistributions in binary form must reproduce the above copyright
25 /// notice, this list of conditions and the following disclaimer in the
26 /// documentation and/or other materials provided with the distribution.
27 /// * Neither the name of the ACTIVE LAB, IST, UCF, nor the
28 /// names of its contributors may be used to endorse or promote products
29 /// derived from this software without specific prior written permission.
30 ///
31 /// THIS SOFTWARE IS PROVIDED BY THE ACTIVE LAB''AS IS'' AND ANY
32 /// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
33 /// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
34 /// DISCLAIMED. IN NO EVENT SHALL UCF BE LIABLE FOR ANY
35 /// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
36 /// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
37 /// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
38 /// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
39 /// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
40 /// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
41 ///
42 ////////////////////////////////////////////////////////////////////////////////////
43 #include <jaus/core/component.h>
44 #include <jaus/mobility/drivers/primitivedriver.h>
45 #include <cxutils/keyboard.h>
46 #include <iostream>
47
48 // NOTE - Run another JAUS program that has a primitive driver like vehicle_sim to
49 // talk to!
50
51
52 int main(int argc, char* argv[])
53 {
54 JAUS::Component component;
55
56 // Add our own services to the component.
57 // By default the Component will already have
58 // the Core Service Set (e.g. Events, Discovery, AccessControl).
59 // as needed
60
61 // Setup identification (types are Subsystem::Vehicle or Subsystem::OCU).
62 // string name is whatever your platform/ocu identification is (e.g. XUV, AUV, OCU).
63 component.DiscoveryService()->SetSubsystemIdentification(JAUS::Subsystem::OCU,
64 "OCU");
65
66 // Initialize component.
67 if(component.Initialize(JAUS::Address(1000, 1, 7)) == false)
68 {
69 return 0;
70 }
71
72 std::cout << "Component Initialized!\n";
73 bool finished = false;
74 while(!finished)
75 {
76 // Find robots with primitive driver service.
77 JAUS::Address::List componentsWithPrimitiveDrivers;
78
79 componentsWithPrimitiveDrivers = component.DiscoveryService()->GetComponentsWithService(JAUS::PrimitiveDriver::Name);
80
81 if(componentsWithPrimitiveDrivers.size() > 0)
82 {
83 // For this tutorial, just take the first ID found, change
84 // to what you need for your project.
85 JAUS::Address primitiveDriverID = componentsWithPrimitiveDrivers[0];
86 // Before we can send a command, we must be in control of the
87 // component, and it must be in a ready state for Wrench Effort commands.
88 while(!finished)
89 {
90 // I'm only saving pointers here to reduce typing, you can
91 // just keep calling the function from component if you want in
92 // your own applications.
93 JAUS::AccessControl* controlService = component.AccessControlService();
94 JAUS::Management* managementService = component.ManagementService();
95
96 // I'm setting a higher authority code than 0 here so I
97 // can override any other program on the network that
98 // wants to control. My programs authority must be
99 // greater than or equal to that of the robots to take
100 // control (default is 0).
101 controlService->SetAuthorityCode(200);
102
103 bool areWeReady = false;
104
105 // If we do not have control, request it.
106 if(controlService->HaveControl(primitiveDriverID) == false)
107 {
108 std::cout << "Taking control of: " << primitiveDriverID.ToString() << std::endl;
109 // Try take control
110 if(controlService->RequestComponentControl(primitiveDriverID))
111 {
112 // Put the driver in a ready state so we can send commands.
113 if(managementService->Resume(primitiveDriverID))
114 {
115 std::cout << "Putting " << primitiveDriverID.ToString() << " in a Ready state\n";
116 areWeReady = true;
117 }
118 }
119 }
120 else
121 {
122 // Make sure robot is in a ready state
123 if(managementService->GetComponentStatus(primitiveDriverID) != JAUS::Management::Status::Ready)
124 {
125 // Put the driver in a ready state so we can send commands.
126 if(managementService->Resume(primitiveDriverID))
127 {
128 areWeReady = true;
129 }
130 }
131 else
132 {
133 areWeReady = true;
134 }
135 }
136
137 // If we have control and everything is ready
138 // send drive commands
139 if(areWeReady)
140 {
141 JAUS::SetWrenchEffort setWrench(primitiveDriverID, component.GetComponentID());
142
143 // Remember, local coordinate system for the robot is
144 // x - positive in front
145 // y - positive to the right
146 // z - positive down
147
148 setWrench.SetPropulsiveLinearEffortX(100); // Full forward
149 //setWrench.SetPropulsiveRotationalEffortZ(100); // Turn 100% right
150
151 std::cout << "Current Command:\n";
152 setWrench.PrintMessageBody();
153
154 component.Send(&setWrench);
155 }
156
157 // Check fo escape key
158 if(CxUtils::GetChar() == 27)
159 {
160 finished = true;
161 // If we are done, let's release control
162 // of the robot to play nice. This will
163 // automatically put the robot component we
164 // were controlling in a standby state
165 controlService->ReleaseComponentControl(primitiveDriverID);
166 break;
167 }
168
169 // Only send commands at 10 Hz in this example
170 // change based on your specific system
171 CxUtils::SleepMs(100);
172 }
173 }
174
175
176 if(CxUtils::GetChar() == 27)
177 {
178 finished = true;
179 break;
180 }
181 CxUtils::SleepMs(1);
182 }
183
184 // Shutdown component. Services will be deleted automatically.
185 component.Shutdown();
186
187 return 0;
188 }
189
190
191 /* End of File */
192