Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
G
gz_gep_tools
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package Registry
Container Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Olivier Stasse
gz_gep_tools
Commits
e933b727
Commit
e933b727
authored
4 weeks ago
by
Olivier Stasse
Browse files
Options
Downloads
Patches
Plain Diff
control_over_gz: Add Reset, read state, and set state of the robot.
parent
8fec53fe
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/gz_gep_tools/control_over_gz.hh
+40
-9
40 additions, 9 deletions
include/gz_gep_tools/control_over_gz.hh
src/control_over_gz.cc
+185
-12
185 additions, 12 deletions
src/control_over_gz.cc
with
225 additions
and
21 deletions
include/gz_gep_tools/control_over_gz.hh
+
40
−
9
View file @
e933b727
/// Standard includes
#include
<gz/sim/EntityComponentManager.hh>
#include
<vector>
#include
<string>
...
...
@@ -12,33 +13,63 @@ class ControlOverGz {
public:
/// Provides the gazebo name.
ControlOverGz
(
std
::
string
&
world_prefix
);
ControlOverGz
(
std
::
string
&
world_prefix
,
bool
debug_level
=
false
);
/// Pause Gazebosim
bool
Pause
();
/// Start Gazebo
bool
Start
();
/// Reset Gazebosim
bool
Reset
();
/// Performing a simulation step
bool
Step
();
/// Getting the simualted time
double
GetSimTime
();
/// Send World Control state to Init ECM.
bool
SendWorldControlStateToInitECM
(
std
::
map
<
std
::
string
,
double
>
&
named_pos_des_d
);
/// Initialization of the ECM by reading the world control state
bool
ReadWorldStateToInitECM
();
/// Display joint values
void
DisplayJointValues
();
protected:
/// Reading GZ sim time
void
CallbackClock
(
const
gz
::
msgs
::
Clock
&
);
/// Send a Pause request
bool
SendPauseRequest
(
bool
abool
);
/// World prefix
/// World prefix
std
::
string
world_prefix_
;
/// list of services
std
::
string
control_gzsim_
;
/// Name of the control service
std
::
string
control_service_gzsim_
;
/// Name of state service
std
::
string
state_service_gzsim_
;
/// Name of wrold control state service.
std
::
string
wrld_ctrl_state_srv_gzsim_
;
/// GZ node
gz
::
transport
::
Node
node_
;
/// Time
double
gz_sim_time_
;
bool
debug_level_
;
/// Robot Entity Component Manager
gz
::
sim
::
EntityComponentManager
Robot_ECM_
;
};
}
This diff is collapsed.
Click to expand it.
src/control_over_gz.cc
+
185
−
12
View file @
e933b727
#include
<cmath>
#include
<cstdint>
#include
<cstring>
#include
<gz/msgs/empty.pb.h>
#include
<gz_gep_tools/control_over_gz.hh>
#include
<gz/msgs.hh>
#include
<gz/transport.hh>
#include
<gz/sim/EntityComponentManager.hh>
#include
<gz/sim/components/Joint.hh>
#include
<gz/sim/Joint.hh>
namespace
gz_transport_hw_tools
{
ControlOverGz
::
ControlOverGz
(
std
::
string
&
world_prefix
)
:
world_prefix_
(
world_prefix
)
{
control_gzsim_
=
world_prefix
+
std
::
string
(
"/control"
);
ControlOverGz
::
ControlOverGz
(
std
::
string
&
world_prefix
,
bool
debug_level
)
:
world_prefix_
(
world_prefix
),
debug_level_
(
debug_level
)
{
/// Create control_gzsim topic name
control_service_gzsim_
=
world_prefix
+
std
::
string
(
"/control"
);
/// State gzsim service
state_service_gzsim_
=
world_prefix
+
std
::
string
(
"/state"
);
std
::
string
gazebo_clock_topic_name
;
gazebo_clock_topic_name
=
world_prefix
+
std
::
string
(
"/clock"
);
node_
.
Subscribe
<
ControlOverGz
,
gz
::
msgs
::
Clock
>
(
gazebo_clock_topic_name
,
&
ControlOverGz
::
CallbackClock
,
this
);
gz_sim_time_
=
std
::
nan
(
"1"
);
wrld_ctrl_state_srv_gzsim_
=
control_service_gzsim_
+
std
::
string
(
"/state"
);
}
void
ControlOverGz
::
CallbackClock
(
const
gz
::
msgs
::
Clock
&
a_gz_time_msg
)
{
if
(
a_gz_time_msg
.
has_sim
())
{
gz_sim_time_
=
a_gz_time_msg
.
sim
().
sec
()
+
1e-9
*
a_gz_time_msg
.
sim
().
nsec
();
if
(
debug_level_
)
std
::
cerr
<<
"ControlOverGz::CallbackClock: Time: "
<<
a_gz_time_msg
.
sim
().
sec
()
<<
" "
<<
a_gz_time_msg
.
sim
().
nsec
()
<<
std
::
endl
;
}
}
double
ControlOverGz
::
GetSimTime
()
{
return
gz_sim_time_
;
}
bool
ControlOverGz
::
Step
()
{
gz
::
msgs
::
WorldControl
req_world_ctrl
;
gz
::
msgs
::
Boolean
rep_bool
;
unsigned
int
timeout
=
3000
;
/// Set reset for everything
req_world_ctrl
.
set_multi_step
(
1
);
req_world_ctrl
.
set_pause
(
true
);
/// Do request.
bool
result
=
true
;
if
(
!
node_
.
Request
(
control_service_gzsim_
,
req_world_ctrl
,
timeout
,
rep_bool
,
result
))
{
std
::
cerr
<<
"Unable to send reset request !"
<<
std
::
endl
;
result
=
false
;
}
///set_allocated_reset(&req_world_reset);
return
result
;
}
bool
ControlOverGz
::
Reset
()
{
gz
::
msgs
::
WorldControl
req_world_ctrl
;
gz
::
msgs
::
WorldReset
req_world_reset
;
gz
::
msgs
::
WorldReset
*
req_world_reset
;
gz
::
msgs
::
Boolean
rep_bool
;
unsigned
int
timeout
=
5
000
;
unsigned
int
timeout
=
3
000
;
/// Set reset for everything
req_world_reset
.
set_all
(
true
);
req_world_reset
=
new
gz
::
msgs
::
WorldReset
();
req_world_reset
->
set_all
(
true
);
req_world_ctrl
.
set_pause
(
true
);
/// Set the world reset inside the world control reset.
req_world_ctrl
.
set_allocated_reset
(
&
req_world_reset
);
req_world_ctrl
.
set_allocated_reset
(
req_world_reset
);
/// Do request.
bool
result
;
node_
.
Request
(
control_gzsim_
,
req_world_ctrl
,
timeout
,
rep_bool
,
result
)
;
if
(
!
node_
.
Request
(
control_
service_
gzsim_
,
req_world_ctrl
,
timeout
,
rep_bool
,
result
)
)
{
std
::
cerr
<<
"Unable to send reset request !"
<<
std
::
endl
;
result
=
false
;
}
/// World Reset will be released because out of scope.
return
result
;
}
...
...
@@ -32,13 +100,13 @@ bool ControlOverGz::SendPauseRequest(bool abool)
gz
::
msgs
::
WorldControl
req_world_ctrl
;
gz
::
msgs
::
Boolean
rep_bool
;
unsigned
int
timeout
=
5000
;
/// Set the world reset inside the world control reset.
req_world_ctrl
.
set_pause
(
abool
);
/// Do request.
bool
result
;
node_
.
Request
(
control_gzsim_
,
req_world_ctrl
,
timeout
,
rep_bool
,
result
);
node_
.
Request
(
control_
service_
gzsim_
,
req_world_ctrl
,
timeout
,
rep_bool
,
result
);
return
result
;
}
...
...
@@ -46,6 +114,111 @@ bool ControlOverGz::Pause() { return SendPauseRequest(true); }
bool
ControlOverGz
::
Start
()
{
return
SendPauseRequest
(
false
);
}
bool
ControlOverGz
::
ReadWorldStateToInitECM
()
{
/// Gazebo messages
/// Response as Serialized map of the entities
gz
::
msgs
::
SerializedStepMap
rep_serialized_step_map
;
/// Request does not have request argument.
gz
::
msgs
::
Empty
req_empty_msg
;
unsigned
int
timeout
=
3000
;
gz
::
msgs
::
Boolean
rep_bool
;
bool
result
=
true
;
/// Get World Control State.
/// by calling the /prefix_world/state Gazebo service.
if
(
!
node_
.
Request
(
state_service_gzsim_
,
req_empty_msg
,
timeout
,
rep_serialized_step_map
,
result
))
{
std
::
cerr
<<
"Unable to send reset request ! timeout "
<<
state_service_gzsim_
<<
" "
<<
result
<<
std
::
endl
;
result
=
false
;
}
/// Initialization of the robot's Entity Component Manager.
Robot_ECM_
.
SetState
(
*
rep_serialized_step_map
.
mutable_state
());
return
result
;
}
void
ControlOverGz
::
DisplayJointValues
()
{
auto
entities
=
Robot_ECM_
.
EntitiesByComponents
(
gz
::
sim
::
components
::
Joint
());
for
(
auto
it_Entity
=
entities
.
begin
();
it_Entity
!=
entities
.
end
();
it_Entity
++
)
{
gz
::
sim
::
Joint
aJoint
(
*
it_Entity
);
// std::cout << "Joint name:" << aJoint.Name(Robot_ECM_).value_or("No joint name");
std
::
vector
<
double
>
empty_vecd
;
std
::
vector
<
double
>
pos_vecd
=
aJoint
.
Position
(
Robot_ECM_
).
value_or
(
empty_vecd
);
if
(
pos_vecd
.
size
()
!=
0
)
std
::
cout
<<
" "
<<
"Position: "
<<
pos_vecd
[
0
];
else
std
::
cout
<<
" no pos "
;
std
::
vector
<
double
>
vel_vecd
=
aJoint
.
Velocity
(
Robot_ECM_
).
value_or
(
empty_vecd
);
if
(
vel_vecd
.
size
()
!=
0
)
std
::
cout
<<
" "
<<
"Velocity: "
<<
vel_vecd
[
1
];
else
std
::
cout
<<
" no vel "
;
std
::
cout
<<
std
::
endl
;
}
}
bool
ControlOverGz
::
SendWorldControlStateToInitECM
(
std
::
map
<
std
::
string
,
double
>
&
named_pos_d
)
{
gz
::
msgs
::
WorldControlState
req_world_ctrl_state
;
gz
::
msgs
::
SerializedState
*
req_serialized_state
;
unsigned
int
timeout
=
3000
;
gz
::
msgs
::
Boolean
rep_bool
;
bool
result
=
true
;
// Modify joint entities in Robot_ECM.
auto
entities
=
Robot_ECM_
.
EntitiesByComponents
(
gz
::
sim
::
components
::
Joint
());
for
(
auto
it_Entity
=
entities
.
begin
();
it_Entity
!=
entities
.
end
();
it_Entity
++
)
{
gz
::
sim
::
Joint
aJoint
(
*
it_Entity
);
std
::
string
jointName
=
aJoint
.
Name
(
Robot_ECM_
).
value_or
(
"No joint name"
);
// std::cout << "Joint name:" << jointName;
std
::
vector
<
double
>
one_vecd
;
one_vecd
.
push_back
(
named_pos_d
[
jointName
]);
// std::cout << " " << one_vecd[0] << std::endl;
aJoint
.
ResetPosition
(
Robot_ECM_
,
one_vecd
);
}
/// Create the serialized state.
req_serialized_state
=
new
gz
::
msgs
::
SerializedState
();
/// Generate the serialized state msg from the ECM.
*
req_serialized_state
=
Robot_ECM_
.
State
();
/// Link it in the WorldControlState msg
req_world_ctrl_state
.
set_allocated_state
(
req_serialized_state
);
/// Set World Control State.
/// by calling the /prefix_world/state/control Gazebo service.
if
(
!
node_
.
Request
(
wrld_ctrl_state_srv_gzsim_
,
req_world_ctrl_state
,
timeout
,
rep_bool
,
result
))
{
std
::
cerr
<<
"Unable to send reset request ! timeout "
<<
wrld_ctrl_state_srv_gzsim_
<<
" "
<<
result
<<
std
::
endl
;
result
=
false
;
}
return
true
;
}
}
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment