Skip to content

Commit 37d4d05

Browse files
francocipollonefrancocipollone
authored
Merge branch 'ign-gazebo3' into francocipollone/citadel_confirmation_dialog
2 parents 70b097a + 7876ca3 commit 37d4d05

File tree

14 files changed

+476
-545
lines changed

14 files changed

+476
-545
lines changed

‎examples/standalone/external_ecm/CMakeLists.txt

+8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
2+
3+
find_package(ignition-gazebo3 REQUIRED)
4+
5+
add_executable(external_ecm external_ecm.cc)
6+
target_link_libraries(external_ecm
7+
ignition-gazebo3::core)
8+

‎examples/standalone/external_ecm/README.md

+93
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
# External ECM
2+
3+
Example showing how to get a snapshot of all entities and components in a
4+
running simulation from an external program using the state message.
5+
6+
## Build
7+
8+
From the root of the `ign-gazebo` repository, do the following to build the example:
9+
10+
~~~
11+
cd ign-gazebo/examples/standalone/external_ecm
12+
mkdir build
13+
cd build
14+
cmake ..
15+
make
16+
~~~
17+
18+
This will generate the `external_ecm` executable under `build`.
19+
20+
## Run
21+
22+
Start a simulation, for example:
23+
24+
ign gazebo shapes.sdf
25+
26+
On another terminal, run the `external_ecm` executable, passing the name of the
27+
running world you want to inspect:
28+
29+
cd ign-gazebo/examples/standalone/external_ecm
30+
./external_ecm shapes
31+
32+
You should see something like this:
33+
34+
```
35+
$ ./external_ecm shapes
36+
37+
Requesting state for world [shapes] on service [/world/shapes/state]...
38+
39+
Entity [1]
40+
- Name: shapes
41+
- Parent:
42+
Entity [4]
43+
- Name: ground_plane
44+
- Parent: shapes [1]
45+
Entity [5]
46+
- Name: link
47+
- Parent: ground_plane [4]
48+
Entity [6]
49+
- Name: visual
50+
- Parent: link [5]
51+
Entity [7]
52+
- Name: collision
53+
- Parent: link [5]
54+
Entity [8]
55+
- Name: box
56+
- Parent: shapes [1]
57+
Entity [9]
58+
- Name: box_link
59+
- Parent: box [8]
60+
Entity [10]
61+
- Name: box_visual
62+
- Parent: box_link [9]
63+
Entity [11]
64+
- Name: box_collision
65+
- Parent: box_link [9]
66+
Entity [12]
67+
- Name: cylinder
68+
- Parent: shapes [1]
69+
Entity [13]
70+
- Name: cylinder_link
71+
- Parent: cylinder [12]
72+
Entity [14]
73+
- Name: cylinder_visual
74+
- Parent: cylinder_link [13]
75+
Entity [15]
76+
- Name: cylinder_collision
77+
- Parent: cylinder_link [13]
78+
Entity [16]
79+
- Name: sphere
80+
- Parent: shapes [1]
81+
Entity [17]
82+
- Name: sphere_link
83+
- Parent: sphere [16]
84+
Entity [18]
85+
- Name: sphere_visual
86+
- Parent: sphere_link [17]
87+
Entity [19]
88+
- Name: sphere_collision
89+
- Parent: sphere_link [17]
90+
Entity [20]
91+
- Name: sun
92+
- Parent: shapes [1]
93+
```

‎examples/standalone/external_ecm/external_ecm.cc

+98
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
/*
2+
* Copyright (C) 2021 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#include <iostream>
19+
#include <ignition/gazebo/EntityComponentManager.hh>
20+
#include <ignition/gazebo/components/Name.hh>
21+
#include <ignition/gazebo/components/ParentEntity.hh>
22+
#include <ignition/msgs/serialized.pb.h>
23+
#include <ignition/transport/Node.hh>
24+
25+
//////////////////////////////////////////////////
26+
int main(int argc, char **argv)
27+
{
28+
if (argc < 2)
29+
{
30+
std::cout << "Usage: `./external_ecm <world name>`" << std::endl;
31+
return -1;
32+
}
33+
34+
// Get arguments
35+
std::string world = argv[1];
36+
37+
// Create a transport node.
38+
ignition::transport::Node node;
39+
40+
bool executed{false};
41+
bool result{false};
42+
unsigned int timeout{5000};
43+
std::string service{"/world/" + world + "/state"};
44+
45+
std::cout << std::endl << "Requesting state for world [" << world
46+
<< "] on service [" << service << "]..." << std::endl << std::endl;
47+
48+
// Request and block
49+
ignition::msgs::SerializedStepMap res;
50+
executed = node.Request(service, timeout, res, result);
51+
52+
if (!executed)
53+
{
54+
std::cerr << std::endl << "Service call to [" << service << "] timed out"
55+
<< std::endl;
56+
return -1;
57+
}
58+
59+
if (!result)
60+
{
61+
std::cerr << std::endl << "Service call to [" << service << "] failed"
62+
<< std::endl;
63+
return -1;
64+
}
65+
66+
// Instantiate an ECM and populate with data from message
67+
ignition::gazebo::EntityComponentManager ecm;
68+
ecm.SetState(res.state());
69+
70+
// Print some information
71+
ecm.Each<ignition::gazebo::components::Name>(
72+
[&](const ignition::gazebo::Entity &_entity,
73+
const ignition::gazebo::components::Name *_name) -> bool
74+
{
75+
auto parentComp =
76+
ecm.Component<ignition::gazebo::components::ParentEntity>(_entity);
77+
78+
std::string parentInfo;
79+
if (parentComp)
80+
{
81+
auto parentNameComp =
82+
ecm.Component<ignition::gazebo::components::Name>(
83+
parentComp->Data());
84+
85+
if (parentNameComp)
86+
{
87+
parentInfo += parentNameComp->Data() + " ";
88+
}
89+
parentInfo += "[" + std::to_string(parentComp->Data()) + "]";
90+
}
91+
92+
std::cout << "Entity [" << _entity << "]" << std::endl
93+
<< " - Name: " << _name->Data() << std::endl
94+
<< " - Parent: " << parentInfo << std::endl;
95+
96+
return true;
97+
});
98+
}

‎src/systems/diff_drive/CMakeLists.txt

-1
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,6 @@
11
gz_add_system(diff-drive
22
SOURCES
33
DiffDrive.cc
4-
SpeedLimiter.cc
54
PUBLIC_LINK_LIBS
65
ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
76
)

‎src/systems/diff_drive/DiffDrive.cc

+29-38
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
#include <ignition/common/Profiler.hh>
2828
#include <ignition/math/DiffDriveOdometry.hh>
2929
#include <ignition/math/Quaternion.hh>
30+
#include <ignition/math/SpeedLimiter.hh>
3031
#include <ignition/plugin/Register.hh>
3132
#include <ignition/transport/Node.hh>
3233

@@ -36,8 +37,6 @@
3637
#include "ignition/gazebo/Link.hh"
3738
#include "ignition/gazebo/Model.hh"
3839

39-
#include "SpeedLimiter.hh"
40-
4140
using namespace ignition;
4241
using namespace gazebo;
4342
using namespace systems;
@@ -123,10 +122,10 @@ class ignition::gazebo::systems::DiffDrivePrivate
123122
public: transport::Node::Publisher tfPub;
124123

125124
/// \brief Linear velocity limiter.
126-
public: std::unique_ptr<SpeedLimiter> limiterLin;
125+
public: std::unique_ptr<ignition::math::SpeedLimiter> limiterLin;
127126

128127
/// \brief Angular velocity limiter.
129-
public: std::unique_ptr<SpeedLimiter> limiterAng;
128+
public: std::unique_ptr<ignition::math::SpeedLimiter> limiterAng;
130129

131130
/// \brief Previous control command.
132131
public: Commands last0Cmd;
@@ -197,56 +196,48 @@ void DiffDrive::Configure(const Entity &_entity,
197196
this->dataPtr->wheelRadius = _sdf->Get<double>("wheel_radius",
198197
this->dataPtr->wheelRadius).first;
199198

200-
// Parse speed limiter parameters.
201-
bool hasVelocityLimits = false;
202-
bool hasAccelerationLimits = false;
203-
bool hasJerkLimits = false;
204-
double minVel = std::numeric_limits<double>::lowest();
205-
double maxVel = std::numeric_limits<double>::max();
206-
double minAccel = std::numeric_limits<double>::lowest();
207-
double maxAccel = std::numeric_limits<double>::max();
208-
double minJerk = std::numeric_limits<double>::lowest();
209-
double maxJerk = std::numeric_limits<double>::max();
199+
// Instantiate the speed limiters.
200+
this->dataPtr->limiterLin = std::make_unique<ignition::math::SpeedLimiter>();
201+
this->dataPtr->limiterAng = std::make_unique<ignition::math::SpeedLimiter>();
210202

203+
// Parse speed limiter parameters.
211204
if (_sdf->HasElement("min_velocity"))
212205
{
213-
minVel = _sdf->Get<double>("min_velocity");
214-
hasVelocityLimits = true;
206+
const double minVel = _sdf->Get<double>("min_velocity");
207+
this->dataPtr->limiterLin->SetMinVelocity(minVel);
208+
this->dataPtr->limiterAng->SetMinVelocity(minVel);
215209
}
216210
if (_sdf->HasElement("max_velocity"))
217211
{
218-
maxVel = _sdf->Get<double>("max_velocity");
219-
hasVelocityLimits = true;
212+
const double maxVel = _sdf->Get<double>("max_velocity");
213+
this->dataPtr->limiterLin->SetMaxVelocity(maxVel);
214+
this->dataPtr->limiterAng->SetMaxVelocity(maxVel);
220215
}
221216
if (_sdf->HasElement("min_acceleration"))
222217
{
223-
minAccel = _sdf->Get<double>("min_acceleration");
224-
hasAccelerationLimits = true;
218+
const double minAccel = _sdf->Get<double>("min_acceleration");
219+
this->dataPtr->limiterLin->SetMinAcceleration(minAccel);
220+
this->dataPtr->limiterAng->SetMinAcceleration(minAccel);
225221
}
226222
if (_sdf->HasElement("max_acceleration"))
227223
{
228-
maxAccel = _sdf->Get<double>("max_acceleration");
229-
hasAccelerationLimits = true;
224+
const double maxAccel = _sdf->Get<double>("max_acceleration");
225+
this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel);
226+
this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel);
230227
}
231228
if (_sdf->HasElement("min_jerk"))
232229
{
233-
minJerk = _sdf->Get<double>("min_jerk");
234-
hasJerkLimits = true;
230+
const double minJerk = _sdf->Get<double>("min_jerk");
231+
this->dataPtr->limiterLin->SetMinJerk(minJerk);
232+
this->dataPtr->limiterAng->SetMinJerk(minJerk);
235233
}
236234
if (_sdf->HasElement("max_jerk"))
237235
{
238-
maxJerk = _sdf->Get<double>("max_jerk");
239-
hasJerkLimits = true;
236+
const double maxJerk = _sdf->Get<double>("max_jerk");
237+
this->dataPtr->limiterLin->SetMaxJerk(maxJerk);
238+
this->dataPtr->limiterAng->SetMaxJerk(maxJerk);
240239
}
241240

242-
// Instantiate the speed limiters.
243-
this->dataPtr->limiterLin = std::make_unique<SpeedLimiter>(
244-
hasVelocityLimits, hasAccelerationLimits, hasJerkLimits,
245-
minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk);
246-
247-
this->dataPtr->limiterAng = std::make_unique<SpeedLimiter>(
248-
hasVelocityLimits, hasAccelerationLimits, hasJerkLimits,
249-
minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk);
250241

251242
double odomFreq = _sdf->Get<double>("odom_publish_frequency", 50).first;
252243
if (odomFreq > 0)
@@ -502,11 +493,11 @@ void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info,
502493
angVel = this->targetVel.angular().z();
503494
}
504495

505-
const double dt = std::chrono::duration<double>(_info.dt).count();
506-
507496
// Limit the target velocity if needed.
508-
this->limiterLin->Limit(linVel, this->last0Cmd.lin, this->last1Cmd.lin, dt);
509-
this->limiterAng->Limit(angVel, this->last0Cmd.ang, this->last1Cmd.ang, dt);
497+
this->limiterLin->Limit(
498+
linVel, this->last0Cmd.lin, this->last1Cmd.lin, _info.dt);
499+
this->limiterAng->Limit(
500+
angVel, this->last0Cmd.ang, this->last1Cmd.ang, _info.dt);
510501

511502
// Update history of commands.
512503
this->last1Cmd = last0Cmd;

0 commit comments

Comments
 (0)